Local position topic and bug fixes:

closes #121, closes #125, closes #126, closes #133

ReadMe.md

@@ -1,6 +1,8 @@
-# DJI Onboard SDK ROS  3.3.1
+# DJI Onboard SDK ROS 3.3
 
-**Version 3.3.1 was developed to support N3/A3 FW 1.7.5 and above, and is backword compatible with M100 FW 1.3.1.**
+## Latest Update
+
+The latest update on Sep 8 2017 brought back the local position topic in Cartesian frame (ENU) and addressed several issues reported on github. The `dji_sdk/SetLocalPosRef` service and the `dji_sdk/local_position` topic were added.
 
 ## Quick Start Guide 
 
@@ -10,47 +12,50 @@ The ROS package is a full rewrite of DJI Onboard SDK ROS, with changes in APIs,
 
 ROS Wiki can be found [here](http://wiki.ros.org/dji_sdk). Please be sure to read the [release notes](https://developer.dji.com/onboard-sdk/documentation/appendix/releaseNotes.html).
 
+DJI Onboard SDK ROS 3.3 was developed to support N3/A3 FW 1.7.5 and above, and is backword compatible with M100 FW 1.3.1.
+
 ## Firmware Compatibility
 
-| Aircraft/FC       | Firmware Package Version | Flight Controller Version  | OSDK Version Support      |
+| Aircraft/FC       | Firmware Package Version | Flight Controller Version  | OSDK-ROS Branch      |
 |---------------    |--------------------------|----------------------------|----------------------     |
-| **A3/A3 Pro**     | **1.7.1.5**              | **3.2.36.8**               | **OSDK 3.3.1 (Current)**  |
-|                   | 1.7.0.5                  | 3.2.15.50                  | OSDK 3.2                  |
-|                   | 1.7.0.0                  | 3.2.15.37                  | OSDK 3.2                  |
+| **A3/A3 Pro**     | **1.7.1.5**              | **3.2.36.8**               | **OSDK-ROS 3.3 (Current)**  |
+|                   | 1.7.0.5                  | 3.2.15.50                  | OSDK-ROS 3.2                  |
+|                   | 1.7.0.0                  | 3.2.15.37                  | OSDK-ROS 3.2                  |
 |                   |                          |                            |                           |
-| **N3**            | **1.7.1.5**              | **3.2.36.8**               | **OSDK 3.3.1 (Current)**  |
-|                   | 1.7.0.0                  | 3.2.15.37                  | OSDK 3.2                  |
+| **N3**            | **1.7.1.5**              | **3.2.36.8**               | **OSDK-ROS 3.3 (Current)**  |
+|                   | 1.7.0.0                  | 3.2.15.37                  | OSDK-ROS 3.2                  |
 |                   |                          |                            |                           |
-| **M600/M600 Pro** | Coming soon!             | Coming soon!               | **OSDK 3.3.1 (Current)**  |
-|                   | 1.0.1.20                 | 3.2.15.62                  | OSDK 3.2                  |
-|                   | 1.0.0.80                 | 3.2.15.00                  | OSDK 3.2                  |
+| **M600/M600 Pro** | 1.0.1.60                 | 3.2.41.5                   | **OSDK-ROS 3.3 (Current)**  |
+|                   | 1.0.1.20                 | 3.2.15.62                  | OSDK-ROS 3.2                  |
+|                   | 1.0.0.80                 | 3.2.15.00                  | OSDK-ROS 3.2                  |
 |                   |                          |                            |                           |
-| **M100**          | 1.3.1.0                  | 3.1.10.0                   | **OSDK 3.3.1 (Current),** |
-|                   |                          |                            | and OSDK 3.2              |
+| **M100**          | 1.3.1.0                  | 3.1.10.0                   | **OSDK-ROS 3.3 (Current),** |
+|                   |                          |                            | and OSDK-ROS 3.2              |
 
 ## Notes on differences between M100 and A3/N3 setup
 
-Backward compatibility for M100 was brought in version 3.3.1. However, due to the limitations of the flight controller of M100, some new features such as hardware sync, MFIO, on demand telemetry data (subscription) are only supported by A3/N3, and some settings for M100 are different from those for A3/N3.
+Onboard SDK ROS is backward compatible with M100. However, due to the limitations of the flight controller of M100, some new features such as hardware sync, MFIO, on demand telemetry data (subscription) are only supported by A3/N3, and some settings for M100 are different from those for A3/N3.
 
-1. The DJI Assistant 2 for M100 and for A3/N3 are slighly different. Please download DJI Assistant 2 from the corresponding product webpage.
+1. The DJI Assistant 2 for M100 and for A3/N3 are slighly **different**. Please download DJI Assistant 2 from the corresponding product webpage.
 
-2. The DJI SDK ROS package requires different baud rate for M100 and A3/N3. For M100, set the baud rate to 230400 in DJI Assistant 2's "SDK" tab, and the sdk.launch file; while for A3/N3, use 921600.
+2. The DJI SDK ROS package requires **different baud rate** for M100 and A3/N3. For M100, set the baud rate to 230400 in DJI Assistant 2's "SDK" tab, and the sdk.launch file; while for A3/N3, use 921600.
 
-3. For M100, on the right side of the "SDK setting" tab of DJI Assistant 2, set the Data Type of ACC and GYRO to "Raw Data", and ALTI to "Data Fusion". The reason is that the raw data of acc and gyro are part of the /dji_sdk/imu message.
+3. For M100, on the right side of the "SDK setting" tab of DJI Assistant 2, set the Data Type of ACC and GYRO to "Raw Data", and ALTI to "Data Fusion". The reason is that the raw data of acc and gyro are part of the `/dji_sdk/imu` message.
 
-4. The flight_status enums for M100 and A3/N3 are different. See dji\_sdk.h for details and demo_flight_control for examples.
+4. The `flight_status` enums for M100 and A3/N3 are different. See `dji_sdk.h` for details and `demo_flight_control` for examples.
 
-5. Some topics  are only available on A3/N3: display_mode, angular_velocity_fused, acceleration_ground_fused, trigger_time. 
+5. Some topics  are only available on A3/N3: `display_mode`, `angular_velocity_fused`, `acceleration_ground_fused`, `trigger_time`. 
 
 6. The imu topic is published at 400Hz on A3/N3, and at 100Hz on M100.
 
-7. Some services are only available on A3/N3: mfio_config, mfio_set_value, set_hardsyc
+7. Some services are only available on A3/N3: `mfio_config`, `mfio_set_value`, `set_hardsyc`
 
 ## Support
 
 You can get support from DJI and the community with the following methods:
 
-- Github Issues or [gitter.im](https://gitter.im/dji-sdk/Onboard-SDK)
+- Report issue on github
+- Email to [email protected]
 - [**DJI Forum**](http://forum.dev.dji.com/en)
 
 

dji_sdk/CMakeLists.txt

@@ -67,6 +67,7 @@ add_service_files(
   CameraAction.srv
   DroneTaskControl.srv
   SDKControlAuthority.srv
+  SetLocalPosRef.srv
   MFIOConfig.srv
   MFIOSetValue.srv
   DroneArmControl.srv

dji_sdk/include/dji_sdk/dji_sdk_node.h

@@ -59,6 +59,7 @@
 #include <dji_sdk/MFIOConfig.h>
 #include <dji_sdk/MFIOSetValue.h>
 #include <dji_sdk/SDKControlAuthority.h>
+#include <dji_sdk/SetLocalPosRef.h>
 #include <dji_sdk/SendMobileData.h>
 #include <dji_sdk/QueryDroneVersion.h>
 //! SDK library
@@ -97,6 +98,7 @@ class DJISDKNode
   bool initFlightControl(ros::NodeHandle& nh);
   bool initSubscriber(ros::NodeHandle& nh);
   bool initPublisher(ros::NodeHandle& nh);
+  bool initActions(ros::NodeHandle& nh);
   bool initDataSubscribeFromFC();
   void cleanUpSubscribeFromFC();
   bool validateSerialDevice(LinuxSerialDevice* serialDevice);
@@ -134,6 +136,10 @@ class DJISDKNode
   bool sdkCtrlAuthorityCallback(
     dji_sdk::SDKControlAuthority::Request&  request,
     dji_sdk::SDKControlAuthority::Response& response);
+
+  bool setLocalPosRefCallback(
+      dji_sdk::SetLocalPosRef::Request&  request,
+      dji_sdk::SetLocalPosRef::Response& response);
   //! control service callbacks
   bool droneArmCallback(dji_sdk::DroneArmControl::Request&  request,
                         dji_sdk::DroneArmControl::Response& response);
@@ -253,6 +259,8 @@ class DJISDKNode
   ros::ServiceServer set_hardsync_server;
   //! Query FW version of FC
   ros::ServiceServer query_version_server;
+  //! Set Local position reference
+  ros::ServiceServer local_pos_ref_server;
 
   //! flight control subscribers
   ros::Subscriber flight_control_sub;
@@ -280,6 +288,8 @@ class DJISDKNode
   ros::Publisher gimbal_angle_publisher;
   ros::Publisher displaymode_publisher;
   ros::Publisher rc_publisher;
+  //! Local Position Publisher (Publishes local position in ENU frame)
+  ros::Publisher local_position_publisher;
 
   //! constant
   const int WAIT_TIMEOUT           = 10;
@@ -321,9 +331,19 @@ class DJISDKNode
 
   bool align_time_with_FC;
 
+  bool local_pos_ref_set;
+
   void alignRosTimeWithFlightController(ros::Time now_time, uint32_t tick);
   void setUpM100DefaultFreq(uint8_t freq[16]);
   void setUpA3N3DefaultFreq(uint8_t freq[16]);
+  void gpsConvertENU(double &ENU_x, double &ENU_y,
+                     double gps_t_lon, double gps_t_lat,
+                     double gps_r_lon, double gps_r_lat);
+
+
+  double local_pos_ref_latitude, local_pos_ref_longitude, local_pos_ref_altitude;
+  double current_gps_latitude, current_gps_longitude, current_gps_altitude;
+  int current_gps_health;
 };
 
 #endif // DJI_SDK_NODE_MAIN_H

dji_sdk/msg/MissionHotpointTask.msg

@@ -1,8 +1,8 @@
-float64 latitude
-float64 longitude
-float64 altitude
-float64 radius
-float32 angular_speed
+float64 latitude  # degree
+float64 longitude # degree
+float64 altitude  # meter
+float64 radius    # meter
+float32 angular_speed #deg/s
 uint8 is_clockwise
 uint8 start_point
 uint8 yaw_mode

dji_sdk/msg/MissionWaypoint.msg

@@ -1,5 +1,5 @@
-float64 latitude          # radian
-float64 longitude         # radian
+float64 latitude          # degree
+float64 longitude         # degree
 float32 altitude          # relative altitude from takeoff point
 float32 damping_distance  # Bend length (effective coordinated turn mode only)
 int16 target_yaw          # Yaw (degree)

dji_sdk/msg/MissionWaypointAction.msg

@@ -1,5 +1,6 @@
-# first 4 bit: Total number of actions
-# last 4 bit: Total running times
+# action_repeat
+# lower 4 bit: Total number of actions
+# hight 4 bit: Total running times
 uint8 action_repeat
 uint8[16] command_list
-int16[16] command_parameter
+uint16[16] command_parameter

dji_sdk/src/modules/dji_sdk_node.cpp

@@ -29,6 +29,10 @@ DJISDKNode::DJISDKNode(ros::NodeHandle& nh, ros::NodeHandle& nh_private)
   nh_private.param("align_time",    align_time_with_FC, true);
   nh_private.param("use_broadcast", user_select_BC, false);
 
+  //! Default values for local Position
+  local_pos_ref_latitude = local_pos_ref_longitude = local_pos_ref_altitude = 0;
+  local_pos_ref_set = false;
+
   // @todo need some error handling for init functions
   //! @note parsing launch file to get environment parameters
   if (!initVehicle(nh_private))
@@ -147,6 +151,7 @@ bool DJISDKNode::initServices(ros::NodeHandle& nh) {
   mission_status_server     = nh.advertiseService("dji_sdk/mission_status",                 &DJISDKNode::missionStatusCallback,          this);
   send_to_mobile_server     = nh.advertiseService("dji_sdk/send_data_to_mobile",            &DJISDKNode::sendToMobileCallback,           this);
   query_version_server      = nh.advertiseService("dji_sdk/query_drone_version",            &DJISDKNode::queryVersionCallback,           this);
+  local_pos_ref_server      = nh.advertiseService("dji_sdk/set_local_pos_ref",              &DJISDKNode::setLocalPosRefCallback,         this);
 
   // A3/N3 only
   if(!isM100())
@@ -270,6 +275,9 @@ DJISDKNode::initPublisher(ros::NodeHandle& nh)
   gimbal_angle_publisher =
     nh.advertise<geometry_msgs::Vector3Stamped>("dji_sdk/gimbal_angle", 10);
 
+  local_position_publisher =
+      nh.advertise<geometry_msgs::PointStamped>("dji_sdk/local_position", 10);
+
   if (telemetry_from_fc == USE_BROADCAST)
   {
     ACK::ErrorCode broadcast_set_freq_ack;
@@ -308,7 +316,7 @@ DJISDKNode::initPublisher(ros::NodeHandle& nh)
     }
     else
     {
-      ROS_INFO("align_time_with_FC set to false. We will time stamp messages based on flight controller time!");
+      ROS_INFO("align_time_with_FC set to true. We will time stamp messages based on flight controller time!");
     }
 
     // Extra topics that is only available from subscription
@@ -517,3 +525,13 @@ DJISDKNode::setUpA3N3DefaultFreq(uint8_t freq[16])
   freq[12] = DataBroadcast::FREQ_10HZ;
   freq[13] = DataBroadcast::FREQ_10HZ;
 }
+
+void DJISDKNode::gpsConvertENU(double &ENU_x, double &ENU_y,
+                                 double gps_t_lon, double gps_t_lat,
+                                 double gps_r_lon, double gps_r_lat)
+{
+  double d_lon = gps_t_lon - gps_r_lon;
+  double d_lat = gps_t_lat - gps_r_lat;
+  ENU_y = DEG2RAD(d_lat) * C_EARTH;
+  ENU_x = DEG2RAD(d_lon) * C_EARTH * cos(DEG2RAD(gps_t_lat));
+};

dji_sdk/src/modules/dji_sdk_node_mission_services.cpp

@@ -74,7 +74,7 @@ DJISDKNode::missionWpUploadCallback(
   int                         i = 0;
   for (auto waypoint : request.waypoint_task.mission_waypoint)
   {
-    wpData.latitude        = waypoint.latitude * C_PI / 180;
+    wpData.latitude        = waypoint.latitude  * C_PI / 180;
     wpData.longitude       = waypoint.longitude * C_PI / 180;
     wpData.altitude        = waypoint.altitude;
     wpData.damping         = waypoint.damping_distance;
@@ -251,6 +251,7 @@ DJISDKNode::missionWpGetInfoCallback(
   else
   {
     ROS_ERROR("no waypoint mission initiated ");
+    return false;
   }
 
   response.waypoint_task.mission_waypoint.resize(info.indexNumber);
@@ -263,6 +264,30 @@ DJISDKNode::missionWpGetInfoCallback(
   response.waypoint_task.action_on_rc_lost  = info.RCLostAction;
   response.waypoint_task.gimbal_pitch_mode  = info.gimbalPitch;
 
+  for (int i=0; i< info.indexNumber; i++)
+  {
+    DJI::OSDK::WayPointSettings wpData; 
+    wpData = vehicle->missionManager->wpMission->getIndex(i, 10).data;
+    response.waypoint_task.mission_waypoint[i].latitude            = wpData.latitude  * 180.0 / C_PI;
+    response.waypoint_task.mission_waypoint[i].longitude           = wpData.longitude * 180.0 / C_PI;
+    response.waypoint_task.mission_waypoint[i].altitude            = wpData.altitude;
+    response.waypoint_task.mission_waypoint[i].damping_distance    = wpData.damping;
+    response.waypoint_task.mission_waypoint[i].target_yaw          = wpData.yaw;
+    response.waypoint_task.mission_waypoint[i].target_gimbal_pitch = wpData.gimbalPitch;
+    response.waypoint_task.mission_waypoint[i].turn_mode           = wpData.turnMode;
+    response.waypoint_task.mission_waypoint[i].has_action          = wpData.hasAction;
+    response.waypoint_task.mission_waypoint[i].action_time_limit   = wpData.actionTimeLimit;
+    response.waypoint_task.mission_waypoint[i].waypoint_action.action_repeat = wpData.actionNumber + (wpData.actionRepeat << 4);
+
+    std::copy(std::begin(wpData.commandList),
+              std::end(wpData.commandList), 
+              response.waypoint_task.mission_waypoint[i].waypoint_action.command_list.begin());
+
+    std::copy(std::begin(wpData.commandParameter),
+              std::end(wpData.commandParameter), 
+              response.waypoint_task.mission_waypoint[i].waypoint_action.command_parameter.begin());
+
+  }
   return true;
 }
 
@@ -364,8 +389,8 @@ DJISDKNode::missionHpGetInfoCallback(
     ROS_ERROR("no hotpoint mission initiated ");
   }
 
-  response.hotpoint_task.latitude      = info.latitude;
-  response.hotpoint_task.longitude     = info.longitude;
+  response.hotpoint_task.latitude      = info.latitude  * 180.0 / C_PI;
+  response.hotpoint_task.longitude     = info.longitude * 180.0 / C_PI;
   response.hotpoint_task.altitude      = info.height;
   response.hotpoint_task.radius        = info.radius;
   response.hotpoint_task.angular_speed = info.yawRate;

dji_sdk/src/modules/dji_sdk_node_publisher.cpp

@@ -11,7 +11,6 @@
 
 #include <dji_sdk/dji_sdk_node.h>
 #include <tf/tf.h>
-
 #include <sensor_msgs/Joy.h>
 
 #define _TICK2ROSTIME(tick) (ros::Duration((double)(tick) / 1000.0))
@@ -117,8 +116,29 @@ DJISDKNode::dataBroadcastCallback()
     gps_pos.latitude        = global_pos.latitude * 180 / C_PI;
     gps_pos.longitude       = global_pos.longitude * 180 / C_PI;
     gps_pos.altitude        = global_pos.altitude;
+    this->current_gps_latitude = gps_pos.latitude;
+    this->current_gps_longitude = gps_pos.longitude;
+    this->current_gps_altitude = gps_pos.altitude;
+    this->current_gps_health = global_pos.health;
     gps_position_publisher.publish(gps_pos);
 
+    if(local_pos_ref_set)
+    {
+      geometry_msgs::PointStamped local_pos;
+      local_pos.header.frame_id = "/local";
+      local_pos.header.stamp = gps_pos.header.stamp;
+      gpsConvertENU(local_pos.point.x, local_pos.point.y, gps_pos.longitude,
+                       gps_pos.latitude, this->local_pos_ref_longitude, this->local_pos_ref_latitude);
+      local_pos.point.z = gps_pos.altitude - this->local_pos_ref_altitude;
+      /*!
+      * note: We are now following REP 103 to use ENU for
+      *       short-range Cartesian representations. Local position is published
+      *       in ENU Frame
+      */
+
+      this->local_position_publisher.publish(local_pos);
+    }
+
     std_msgs::Float32 agl_height;
     agl_height.data = global_pos.height;
     height_publisher.publish(agl_height);
@@ -271,8 +291,27 @@ DJISDKNode::publish50HzData(Vehicle* vehicle, RecvContainer recvFrame,
   gps_pos.latitude        = fused_gps.latitude * 180.0 / C_PI;   //degree
   gps_pos.longitude       = fused_gps.longitude * 180.0 / C_PI;  //degree
   gps_pos.altitude        = fused_gps.altitude;                //meter
+  p->current_gps_latitude = gps_pos.latitude;
+  p->current_gps_longitude = gps_pos.longitude;
+  p->current_gps_altitude = gps_pos.altitude;
   p->gps_position_publisher.publish(gps_pos);
 
+  if(p->local_pos_ref_set)
+  {
+    geometry_msgs::PointStamped local_pos;
+    local_pos.header.frame_id = "/local";
+    local_pos.header.stamp = gps_pos.header.stamp;
+    p->gpsConvertENU(local_pos.point.x, local_pos.point.y, gps_pos.longitude,
+        gps_pos.latitude, p->local_pos_ref_longitude, p->local_pos_ref_latitude);
+    local_pos.point.z = gps_pos.altitude - p->local_pos_ref_altitude;
+   /*!
+   * note: We are now following REP 103 to use ENU for
+   *       short-range Cartesian representations. Local position is published
+   *       in ENU Frame
+   */
+    p->local_position_publisher.publish(local_pos);
+  }
+
   Telemetry::TypeMap<Telemetry::TOPIC_HEIGHT_FUSION>::type fused_height =
     vehicle->subscribe->getValue<Telemetry::TOPIC_HEIGHT_FUSION>();
   std_msgs::Float32 height;
@@ -307,6 +346,7 @@ DJISDKNode::publish50HzData(Vehicle* vehicle, RecvContainer recvFrame,
     vehicle->subscribe->getValue<Telemetry::TOPIC_GPS_CONTROL_LEVEL>();
   std_msgs::UInt8 msg_gps_ctrl_level;
   msg_gps_ctrl_level.data = gps_ctrl_level;
+  p->current_gps_health = gps_ctrl_level;
   p->gps_health_publisher.publish(msg_gps_ctrl_level);
 
   Telemetry::TypeMap<Telemetry::TOPIC_GIMBAL_ANGLES>::type gimbal_angle =