init commit

CMakeLists.txt

+/opt/ros/noetic/share/catkin/cmake/toplevel.cmake
\ No newline at end of file

Onboard-SDK-ROS/ReadMe.md

+# DJI Onboard SDK ROS 3.7
+
+## Latest Update
+
+OSDK-ROS 3.7 was released on 14 Aug 2018. This release adds many new telemetry topics, an emergency stop API and more for the A3, N3 and M210/M210RTK platforms. Please see the [release notes](https://developer.dji.com/onboard-sdk/documentation/appendix/releaseNotes.html) for more information.
+
+## Quick Start Guide 
+
+This repository contains the Onboard SDK ROS wrapper and demos. The ROS package requires Onboard SDK (djiosdk-core) to be installed to your system prior to running it. For detailed setup instructions, please follow the documentation [here](http://developer.dji.com/onboard-sdk/documentation/sample-doc/sample-setup.html#ros-oes). 
+
+We encourage you to take a look at the documentation for full details. 
+
+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).
+
+## Firmware Compatibility
+
+This chart shows the latest firmware that were available and are supported at the time of 3.7 release.
+
+| Aircraft/FC       | Firmware Package Version | Flight Controller Version | OSDK Branch                | Notes                                                                 |
+|-------------------|--------------------------|---------------------------|----------------------------|-----------------------------------------------------------------------|
+| **M210/M210 RTK** | **1.1.0913**             | **3.3.10.4**              | **OSDK-ROS 3.7**           |                                                                       |
+|                   |                          |                           |                            |                                                                       |
+| **M600/M600 Pro** | **1.0.1.66**             | **3.2.41.13**             | **OSDK-ROS 3.7**           |                                                                       |
+|                   |                          |                           |                            |                                                                       |
+| **A3/A3 Pro**     | **1.7.6.0**              | **3.3.8.39**              | **OSDK-ROS 3.7**           |                                                                       |
+|                   |                          |                           |                            |                                                                       |
+| **N3**            | **1.7.6.0**              | **3.3.8.39**              | **OSDK-ROS 3.7**           |                                                                       |
+|                   |                          |                           |                            |                                                                       |
+| **M100**          | 1.3.1.82                 | **3.1.10.0**              | **OSDK-ROS 3.7**           |                                                                       |
+
+## Notes on differences between M100 and A3/N3/M600/M210 setup
+
+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.
+
+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/M600/M210, 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.
+
+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`. 
+
+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`
+
+## Support
+
+You can get support from DJI and the community with the following methods:
+
+- **Email to dev@dji.com**
+- Report issue on github
+- [**DJI Forum**](http://forum.dev.dji.com/en)
+
+
+
+

Onboard-SDK-ROS/dji_sdk/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(dji_sdk)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  message_generation
+  nav_msgs
+  roscpp
+  rospy
+  std_msgs
+  sensor_msgs
+)
+find_package(DJIOSDK REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  Gimbal.msg
+  Waypoint.msg
+  WaypointList.msg
+  MobileData.msg
+  MissionWaypointAction.msg
+  MissionWaypoint.msg
+  MissionWaypointTask.msg
+  MissionHotpointTask.msg
+  FlightAnomaly.msg
+  VOPosition.msg
+)
+
+## Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  Activation.srv
+  CameraAction.srv
+  DroneTaskControl.srv
+  SDKControlAuthority.srv
+  SetLocalPosRef.srv
+  MFIOConfig.srv
+  MFIOSetValue.srv
+  DroneArmControl.srv
+  MissionStatus.srv
+  MissionWpAction.srv
+  WaypointControl.srv
+  MissionHpAction.srv
+  MissionWpUpload.srv
+  MissionWpSetSpeed.srv
+  MissionWpGetSpeed.srv
+  MissionWpGetInfo.srv
+  MissionHpUpload.srv
+  MissionHpGetInfo.srv
+  MissionHpUpdateYawRate.srv
+  MissionHpUpdateRadius.srv
+  MissionHpResetYaw.srv
+  SendMobileData.srv
+  SetHardSync.srv
+  QueryDroneVersion.srv
+  Stereo240pSubscription.srv
+  StereoVGASubscription.srv
+  StereoDepthSubscription.srv
+  SetupCameraStream.srv
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  geometry_msgs
+  nav_msgs
+  std_msgs
+  sensor_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS
+        message_runtime
+        geometry_msgs
+        nav_msgs
+        std_msgs
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  include
+  ${DJIOSDK_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+# add_library(dji_sdk
+#   src/${PROJECT_NAME}/dji_sdk.cpp
+# )
+
+## Declare a cpp executable
+add_executable(dji_sdk_node
+  src/main.cpp
+  src/modules/dji_sdk_node_control.cpp
+  src/modules/dji_sdk_node_services.cpp
+  src/modules/dji_sdk_node.cpp
+  src/modules/dji_sdk_node_mission_services.cpp
+  src/modules/dji_sdk_node_subscriber.cpp
+  src/modules/dji_sdk_node_publisher.cpp
+  src/modules/dji_sdk_node_mobile_comm.cpp
+)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+add_dependencies(dji_sdk_node
+  dji_sdk_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(dji_sdk_node
+  ${catkin_LIBRARIES}
+  ${DJIOSDK_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(TARGETS dji_sdk_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )

Onboard-SDK-ROS/dji_sdk/include/dji_sdk/dji_sdk.h

+/** @file dji_sdk.h
+ *  @version 3.7
+ *  @date July, 2018
+ *
+ *  @brief
+ *  Definitions and Enums for client code to use dji_sdk ros wrapper
+ *
+ *  @copyright 2018 DJI. All rights reserved.
+ *
+ */
+
+#ifndef PROJECT_DJI_SDK_H_H
+#define PROJECT_DJI_SDK_H_H
+#include <djiosdk/dji_control.hpp>
+#include <djiosdk/dji_status.hpp>
+#include <djiosdk/dji_version.hpp>
+namespace DJISDK {
+
+/*!
+ * This enum is used with service query_drone_version to
+ * check if the drone is M100 or not. We only support
+ * M100 with this particular FW version.
+ */
+enum DroneFirmwareVersion
+{
+  M100_31 = DJI::OSDK::Version::M100_31,
+};
+
+typedef enum AircraftVersion
+{
+  UNKNOWN,
+  M100,
+  M600,
+  A3,
+  N3,
+  M210
+} AircraftVersion;
+
+enum FlightControlFlag
+{
+  HORIZONTAL_ANGLE         = DJI::OSDK::Control::HORIZONTAL_ANGLE,
+  HORIZONTAL_VELOCITY      = DJI::OSDK::Control::HORIZONTAL_VELOCITY,
+  HORIZONTAL_POSITION      = DJI::OSDK::Control::HORIZONTAL_POSITION,
+  // Horizontal angular rate is supported only by A3/N3 based platform
+  // and is NOT supported by M100
+  HORIZONTAL_ANGULAR_RATE  = DJI::OSDK::Control::HORIZONTAL_ANGULAR_RATE,
+
+  VERTICAL_VELOCITY = DJI::OSDK::Control::VERTICAL_VELOCITY,
+  VERTICAL_POSITION = DJI::OSDK::Control::VERTICAL_POSITION,
+  VERTICAL_THRUST   = DJI::OSDK::Control::VERTICAL_THRUST,
+
+  YAW_ANGLE = DJI::OSDK::Control::YAW_ANGLE,
+  YAW_RATE  = DJI::OSDK::Control::YAW_RATE,
+
+  HORIZONTAL_GROUND = DJI::OSDK::Control::HORIZONTAL_GROUND,
+  HORIZONTAL_BODY   = DJI::OSDK::Control::HORIZONTAL_BODY,
+
+  STABLE_DISABLE = DJI::OSDK::Control::STABLE_DISABLE,
+  STABLE_ENABLE  = DJI::OSDK::Control::STABLE_ENABLE
+};
+
+/*!
+ * Refer to demo_flight_control.cpp in dji_sdk_demo for how to
+ * use the display mode.
+ */
+enum DisplayMode
+{
+  /*! This mode requires the user to manually
+   * control the aircraft to remain stable in air. */
+  MODE_MANUAL_CTRL=DJI::OSDK::VehicleStatus::DisplayMode::MODE_MANUAL_CTRL,
+  /*! In this mode, the aircraft can keep
+   * attitude stabilization and only use the
+   * barometer for positioning to control the altitude. <br>
+   * The aircraft can not autonomously locate and hover stably.*/
+  MODE_ATTITUDE=DJI::OSDK::VehicleStatus::DisplayMode::MODE_ATTITUDE,
+
+  /*! The aircraft is in normal GPS mode. <br>
+   * In normal GPS mode, the aircraft can
+   * autonomously locate and hover stably. <br>
+   *  The sensitivity of the aircraft to the
+   *  command response is moderate.
+   */
+  MODE_P_GPS=DJI::OSDK::VehicleStatus::DisplayMode::MODE_P_GPS,
+  /*! In hotpoint mode */
+  MODE_HOTPOINT_MODE=DJI::OSDK::VehicleStatus::DisplayMode::MODE_HOTPOINT_MODE,
+  /*! In this mode, user can push the throttle
+   * stick to complete stable take-off. */
+  MODE_ASSISTED_TAKEOFF=DJI::OSDK::VehicleStatus::DisplayMode::MODE_ASSISTED_TAKEOFF,
+  /*! In this mode, the aircraft will autonomously
+   * start motor, ascend and finally hover. */
+  MODE_AUTO_TAKEOFF=DJI::OSDK::VehicleStatus::DisplayMode::MODE_AUTO_TAKEOFF,
+  /*! In this mode, the aircraft can land autonomously. */
+  MODE_AUTO_LANDING=DJI::OSDK::VehicleStatus::DisplayMode::MODE_AUTO_LANDING,
+  /*! In this mode, the aircraft can antonomously return the
+   * last recorded Home Point. <br>
+   * There are three types of this mode: Smart RTH(Return-to-Home),
+   * Low Batterry RTH, and Failsafe RTTH.  */
+  MODE_NAVI_GO_HOME=DJI::OSDK::VehicleStatus::DisplayMode::MODE_NAVI_GO_HOME,
+  /*! In this mode, the aircraft is controled by SDK API. <br>
+   * User can directly define the control mode of horizon
+   * and vertical directions and send control datas to aircraft. */
+  MODE_NAVI_SDK_CTRL=DJI::OSDK::VehicleStatus::DisplayMode::MODE_NAVI_SDK_CTRL,
+  
+  /*! drone is forced to land, might due to low battery */
+  MODE_FORCE_AUTO_LANDING=DJI::OSDK::VehicleStatus::DisplayMode::MODE_FORCE_AUTO_LANDING,
+  /*! drone will search for the last position where the rc is not lost */
+  MODE_SEARCH_MODE =DJI::OSDK::VehicleStatus::DisplayMode::MODE_SEARCH_MODE,
+  /*! Mode for motor starting. <br>
+   * Every time user unlock the motor, this will be the first mode. */
+  MODE_ENGINE_START = DJI::OSDK::VehicleStatus::DisplayMode::MODE_ENGINE_START
+};
+
+/*!
+ * Note that the flight status for M100 and A3/N3 are different.
+ *
+ * Refer to demo_flight_control.cpp in dji_sdk_demo for how to
+ * use the flight status.
+ *
+ */
+enum FlightStatus
+{
+  STATUS_STOPPED   = DJI::OSDK::VehicleStatus::FlightStatus::STOPED,
+  STATUS_ON_GROUND = DJI::OSDK::VehicleStatus::FlightStatus::ON_GROUND,
+  STATUS_IN_AIR    = DJI::OSDK::VehicleStatus::FlightStatus::IN_AIR
+};
+
+enum M100FlightStatus
+{
+  M100_STATUS_ON_GROUND        = DJI::OSDK::VehicleStatus::M100FlightStatus::ON_GROUND_STANDBY,
+  M100_STATUS_TAKINGOFF        = DJI::OSDK::VehicleStatus::M100FlightStatus::TAKEOFF,
+  M100_STATUS_IN_AIR           = DJI::OSDK::VehicleStatus::M100FlightStatus::IN_AIR_STANDBY,
+  M100_STATUS_LANDING          = DJI::OSDK::VehicleStatus::M100FlightStatus::LANDING,
+  M100_STATUS_FINISHED_LANDING = DJI::OSDK::VehicleStatus::M100FlightStatus::FINISHING_LANDING
+};
+
+}
+
+
+#endif //PROJECT_DJI_SDK_H_H

Onboard-SDK-ROS/dji_sdk/launch/sdk.launch

+<launch>
+    <node pkg="dji_sdk" type="dji_sdk_node" name="dji_sdk" output="screen">
+      <!-- node parameters -->
+      <param name="serial_name" type="string" value="/dev/dji_usb"/>
+      <param name="baud_rate" type="int" value="921600"/>
+      <param name="app_id" type="int" value="1069806"/>
+      <param name="app_version" type="int" value="1"/>
+      <param name="align_time" type="bool" value="false"/>
+      <param name="enc_key" type="string" value="e180b993ca8365653437fbafe7211ba040386d77c3c87627882857a11bd8efbd"/>
+      <param name="use_broadcast" type="bool" value="false"/>
+    </node>
+
+    <node name="waypointGenerator" pkg="dji_sdk" type="waypointGenerator.py">
+      <!-- type of path, from "points" below, "spiral", or "squares" -->
+      <param name="pathType" type="string" value="points"/>
+      <param name="spiralRadius" type="double" value="1"/>
+      <param name="spiralLoops" type="double" value="2"/>
+      <param name="spiralHeight" type="double" value="1"/>
+      <param name="altitudeIncrement" type="double" value="10"/>
+      <param name="altitude" type="double" value="60"/>
+      <param name="droneNum" type="double" value="2"/>
+      <rosparam command="load" param="anchorPoint">
+        <!-- kentland airstrip anchor point -->
+        [37.197280, -80.577791]
+      </rosparam>
+      <!-- kentland dual drone test -->
+      <rosparam command="load" param="pathPoints">
+        [[[37.197674, -80.578485],
+        [37.198212, -80.578313],
+        [37.198725, -80.577787],
+        [37.199221, -80.578077],
+        [37.199725, -80.579010],
+        [37.199417, -80.579772],
+        [37.198845, -80.580244]],
+        [[37.197298, -80.577068],
+        [37.197324, -80.576307],
+        [37.196905, -80.575781],
+        [37.196366, -80.575105],
+        [37.195879, -80.575438],
+        [37.195674, -80.576822]]]
+      </rosparam>
+  </node>
+
+  <node pkg="dji_sdk" type="imageActivation.py" name="imageActivation" output="screen"/>
+  <node pkg="dji_sdk" type="waypointDriver.py" name="waypointDriver" output="screen">
+    <param name="drone_id" type="int" value="0"/>
+    <param name="base_speed" type="double" value="1.0"/>
+  </node>
+</launch>

Onboard-SDK-ROS/dji_sdk/msg/FlightAnomaly.msg

+# Here is an example how to use FlightAnomaly msg:
+#
+# void flightAnomalyCallback(const dji_sdk::FlightAnomaly::ConstPtr & msg)
+# {
+#   uint32_t flightAnomalyData = msg->data;
+#   if(flightAnomalyData)
+#   {
+#     ROS_INFO("Flight Anomaly Reported by Flight Controller. Here are details:");
+#     if(flightAnomalyData && dji_sdk::FlightAnomaly::COMPASS_INSTALLATION_ERROR)
+#     {
+#         ROS_INFO("COMPASS_INSTALLATION_ERROR");
+#     }
+#
+#     if(flightAnomalyData && dji_sdk::FlightAnomaly::IMU_INSTALLATION_ERROR)
+#     {
+#         ROS_INFO("IMU_INSTALLATION_ERROR");
+#     }
+#
+#     // etc...
+#   }
+# }
+#
+#
+
+# constants for anomaly details
+uint32 IMPACT_IN_AIR              = 1     #
+uint32 RANDOM_FLY                 = 2     #
+uint32 VERTICAL_CONTROL_FAIL      = 4     #
+uint32 HORIZONTAL_CONTROL_FAIL    = 8     #
+uint32 YAW_CONTROL_FAIL           = 16    #
+uint32 AIRCRAFT_IS_FALLING        = 32    #
+uint32 STRONG_WIND_LEVEL1         = 64    #
+uint32 STRONG_WIND_LEVEL2         = 128   #
+uint32 COMPASS_INSTALLATION_ERROR = 256   #
+uint32 IMU_INSTALLATION_ERROR     = 512   #
+uint32 ESC_TEMPERATURE_HIGH       = 1024  #
+uint32 ESC_DISCONNECTED           = 2048  #
+uint32 GPS_YAW_ERROR              = 4096  #
+
+#
+uint32 data
+

Onboard-SDK-ROS/dji_sdk/msg/Gimbal.msg

+Header header
+# ts is the time it takes to achieve the desired angle,
+# so the shorter the time, the faster the gimbal rotates.
+int32 ts        # sec
+
+# Mode is 1 byte size:
+# Bit #:      | Set to 0:                             | Set to 1:
+# ----------- | ------------------------------------- | -------------------------------------
+# bit 0       | Incremental control,                  | Absolute control,
+#             | the angle reference is the            | the angle reference is
+#             | current Gimbal location               | related to configuration in DJI Go App
+# bit 1       | Gimbal will follow the command in Yaw | Gimbal will maintain position in Yaw
+# bit 2       | Roll invalid bit, the same as bit[1]  | Roll invalid bit, the same as bit[1]
+# bit 3       |Pitch invalid bit, the same as bit[1]  | Pitch invalid bit, the same as bit[1]
+# bit [4:7]   | bit [4:7]: reserved, set to be 0      |
+uint8 mode
+float32 yaw     # rads
+float32 pitch   # rads
+float32 roll    # rads

Onboard-SDK-ROS/dji_sdk/msg/MissionHotpointTask.msg

+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

Onboard-SDK-ROS/dji_sdk/msg/MissionWaypoint.msg

+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)
+int16 target_gimbal_pitch # Gimbal pitch
+uint8 turn_mode           # 0: clockwise, 1: counter-clockwise
+uint8 has_action          # 0: no, 1: yes
+uint16 action_time_limit
+MissionWaypointAction waypoint_action

Onboard-SDK-ROS/dji_sdk/msg/MissionWaypointAction.msg

+# action_repeat
+# lower 4 bit: Total number of actions
+# hight 4 bit: Total running times
+uint8 action_repeat
+uint8[16] command_list
+uint16[16] command_parameter

Onboard-SDK-ROS/dji_sdk/msg/MissionWaypointTask.msg

+# constant for action_on_finish
+uint8 FINISH_NO_ACTION       = 0  # no action
+uint8 FINISH_RETURN_TO_HOME  = 1  # return to home
+uint8 FINISH_AUTO_LANDING    = 2  # auto landing
+uint8 FINISH_RETURN_TO_POINT = 3  # return to point 0
+uint8 FINISH_NO_EXIT         = 4  # infinite mode， no exit
+
+# constant for yaw_mode
+uint8 YAW_MODE_AUTO     = 0       # auto mode (point to next waypoint)
+uint8 YAW_MODE_LOCK     = 1       # lock as an initial value
+uint8 YAW_MODE_RC       = 2       # controlled by RC
+uint8 YAW_MODE_WAYPOINT = 3       # use waypoint's yaw(tgt_yaw)
+
+# constant for trace_mode
+uint8 TRACE_POINT       = 0       # point to point, after reaching the target waypoint hover, complete waypt action (if any), then fly to the next waypt
+uint8 TRACE_COORDINATED = 1       # 1: Coordinated turn mode, smooth transition between waypts, no waypts task
+
+# constants for action_on_rc_lost
+uint8 ACTION_FREE       = 0       # exit waypoint and failsafe
+uint8 ACTION_AUTO       = 1       # continue the waypoint
+
+# constants for gimbal_pitch_mode
+uint8 GIMBAL_PITCH_FREE = 0       # free mode, no control on gimbal
+uint8 GIMBAL_PITCH_AUTO = 1       # auto mode, Smooth transition between waypoints on gimbal
+
+float32 velocity_range    # Maximum speed joystick input(2~15m)
+float32 idle_velocity     # Cruising Speed (without joystick input, no more than vel_cmd_range)
+uint8 action_on_finish    # See constants above for possible actions
+uint8 mission_exec_times  # 1: once ; 2: twice
+uint8 yaw_mode            # see constants above for possible yaw modes
+uint8 trace_mode          # see constants above for possible trace modes
+uint8 action_on_rc_lost   # see constants above for possible actions
+uint8 gimbal_pitch_mode   # see constants above for pissible gimbal modes
+MissionWaypoint[] mission_waypoint  # a vector of waypoints
+

Onboard-SDK-ROS/dji_sdk/msg/MobileData.msg

+uint8[] data
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/msg/VOPosition.msg

+Header header
+float32 x
+float32 y
+float32 z
+uint8   xHealth
+uint8   yHealth
+uint8   zHealth
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/msg/Waypoint.msg

+float64 latitude      # in degree
+float64 longitude     # in degree
+float32 altitude
+int16 heading         #heading is in [-180,180]
+uint16 staytime       # in second
+

Onboard-SDK-ROS/dji_sdk/msg/WaypointList.msg

+Waypoint[] waypoint_list

Onboard-SDK-ROS/dji_sdk/package.xml

+<?xml version="1.0"?>
+<package>
+  <name>dji_sdk</name>
+  <version>3.2.0</version>
+  <description>A ROS package using DJI Onboard SDK. </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="norman.li@dji.com">Norman Li</maintainer>
+  <maintainer email="arjun.menon@dji.com">Arjun Menon</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>MIT</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <url type="website">http://developer.dji.com/onboard-sdk/documentation/github-platform-docs/ROS/README.html</url>
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <author email="arjun.menon@dji.com">Arjun Menon</author>
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>actionlib</run_depend>
+  <run_depend>actionlib_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

Onboard-SDK-ROS/dji_sdk/scripts/imageActivation.py

+#!/usr/bin/python3
+
+# creates a node to call the dji service that activates main camera images
+# TBD: also activate the usb connection from the drone to enable usb-connected flight
+# this command: ./M210ConfigTool --usb-port /dev/ttyACM0 --config-file UserConfig.txt --usb-connected-flight on
+import rospy
+from dji_sdk.srv import StereoVGASubscription
+from dji_sdk.srv import SetupCameraStream
+
+def main():
+    rospy.init_node('imageActivation')
+
+    # request drone to open stereo images
+    rospy.wait_for_service('/dji_sdk/setup_camera_stream')
+    try:
+        camera_service_proxy = rospy.ServiceProxy('/dji_sdk/setup_camera_stream', SetupCameraStream)
+        response = camera_service_proxy(1, 1)
+        rospy.loginfo(response)
+    except:
+        rospy.loginfo("image service call failed")
+
+if __name__ == '__main__':
+    main()

Onboard-SDK-ROS/dji_sdk/scripts/waypointDriver.py

+#!/usr/bin/python3
+
+import rospy
+import numpy as np
+import sys
+from dji_sdk.srv import SDKControlAuthority
+from dji_sdk.srv import MissionWpUpload
+from dji_sdk.srv import MissionWpAction
+from dji_sdk.srv import MissionWpActionRequest
+from dji_sdk.srv import DroneTaskControl
+from dji_sdk.srv import DroneTaskControlRequest
+from dji_sdk.srv import MissionWpUploadRequest
+
+from dji_sdk.msg import MissionWaypoint
+from dji_sdk.msg import MissionWaypointTask
+from dji_sdk.msg import MissionWaypoint
+from dji_sdk.msg import MissionWaypointAction
+
+from dji_sdk.srv import WaypointControl
+from dji_sdk.srv import WaypointControlResponse
+
+class WaypointDriver():
+
+    def setupWaypoints(self):
+
+        # wait for parameters to show up
+        while not rospy.has_param("/robot_list/robot_"+str(self.droneId)+"/numWaypoints"):
+            rospy.loginfo("waiting for waypoint parameters...")
+
+        missionTask = MissionWaypointTask()
+        missionTask.velocity_range     = np.float32(2.0)
+        missionTask.idle_velocity      = np.float32(self.baseSpeed)
+        missionTask.action_on_finish   = np.uint8(MissionWaypointTask.FINISH_NO_ACTION)
+        missionTask.mission_exec_times = np.uint8(1)
+        missionTask.yaw_mode		    = np.uint8(MissionWaypointTask.YAW_MODE_RC)
+        missionTask.trace_mode	        = np.uint8(MissionWaypointTask.TRACE_COORDINATED)
+        missionTask.action_on_rc_lost  = np.uint8(MissionWaypointTask.ACTION_AUTO)
+        missionTask.gimbal_pitch_mode  = np.uint8(MissionWaypointTask.GIMBAL_PITCH_FREE)
+
+        numWaypoints = rospy.get_param("/robot_list/robot_"+str(self.droneId)+"/numWaypoints")
+        waypointList = []
+
+        for i in range(numWaypoints):
+            waypt = rospy.get_param("/robot_list/robot_"+str(self.droneId)+"/waypoints/waypoint_" + str(i))
+            rospy.loginfo("Waypoint read as (Ind, Lat, Lon, Alt, Yaw): {:.0f} {:.3f} {:.3f} {:.3f} {:.3f}".format(waypt[0], waypt[1], waypt[2], waypt[3], waypt[4]))
+            missionWaypoint = MissionWaypoint()
+
+            # specific settings
+            missionWaypoint.latitude = np.float64(waypt[1])
+            missionWaypoint.longitude = np.float64(waypt[2])
+            missionWaypoint.altitude = np.float32(waypt[3])
+            missionWaypoint.target_yaw = np.int16(0)
+            # missionWaypoint.target_yaw = np.int16(waypt[4])
+
+            # default settings
+            missionWaypoint.damping_distance    = np.float32(1.0)
+            missionWaypoint.target_gimbal_pitch = np.int16(0)
+            missionWaypoint.turn_mode           = np.uint8(1)
+            missionWaypoint.has_action          = np.uint8(0)
+            missionWaypoint.action_time_limit   = np.uint16(0)
+
+            missionWaypoint.waypoint_action = MissionWaypointAction()
+            missionWaypoint.waypoint_action.action_repeat = np.uint8(0)
+            missionWaypoint.waypoint_action.command_list = [np.uint8(0)]*16
+            missionWaypoint.waypoint_action.command_parameter = [np.uint16(0)]*16
+
+            waypointList.append(missionWaypoint)
+
+        missionTask.mission_waypoint = waypointList
+        rospy.loginfo("sending waypoint upload command")
+        rospy.loginfo(sys.getsizeof(missionTask))
+        # waypointUpload = MissionWpUpload()
+        waypointUpload = MissionWpUploadRequest()
+        waypointUpload.waypoint_task = missionTask
+        response = self.missionWaypointUploadProxy(waypointUpload)
+        rospy.loginfo(response)
+
+    def startWaypoints(self):
+        waypointAction = MissionWpActionRequest()
+        waypointAction.action = MissionWpActionRequest.ACTION_START
+        response = self.missionWaypointActionProxy(waypointAction)
+        return response
+
+    def stopWaypoints(self):
+        waypointAction = MissionWpActionRequest()
+        waypointAction.action = MissionWpActionRequest.ACTION_STOP
+        response = self.missionWaypointActionProxy(waypointAction)
+        return response
+
+    def land(self):
+        flightTaskControl = DroneTaskControlRequest()
+        flightTaskControl.task = DroneTaskControlRequest.TASK_LAND
+        response = self.droneTaskControlProxy(flightTaskControl)
+        return response
+
+    def takeoff(self):
+        flightTaskControl = DroneTaskControlRequest()
+        flightTaskControl.task = DroneTaskControlRequest.TASK_TAKEOFF
+        response = self.droneTaskControlProxy(flightTaskControl)
+        return response
+
+    def goHome(self):
+        flightTaskControl = DroneTaskControlRequest()
+        flightTaskControl.task = DroneTaskControlRequest.TASK_GOHOME
+        response = self.droneTaskControlProxy(flightTaskControl)
+        return response
+
+    def controlService(self, req):
+        if (req.data == 0):
+            # takeoff
+            rospy.loginfo("recieved takeoff command")
+            response = self.takeoff()
+            rospy.loginfo(response)
+        elif (req.data == 1):
+            # land
+            rospy.loginfo("recieved land command")
+            response = self.goHome()
+            rospy.loginfo(response)
+            response = self.land()
+            rospy.loginfo(response)
+        elif (req.data == 2):
+            # land
+            rospy.loginfo("recieved start waypoints command")
+            response = self.setupWaypoints()
+            rospy.loginfo(response)
+            # response = self.takeoff()
+            # rospy.loginfo(response)
+            response = self.startWaypoints()
+            rospy.loginfo(response)
+        elif (req.data == 3):
+            # land
+            rospy.loginfo("recieved stop waypoints command")
+            response = self.stopWaypoints()
+            rospy.loginfo(response)
+            response = self.goHome()
+            rospy.loginfo(response)
+            response = self.land()
+            rospy.loginfo(response)
+        else:
+            rospy.loginfo("unknown control request")
+
+        return WaypointControlResponse(result = True)
+
+    def __init__(self):
+        rospy.init_node('waypointDriver')
+
+        # make service proxies for all services we need
+        rospy.loginfo("waiting for services...")
+        rospy.wait_for_service('/dji_sdk/sdk_control_authority')
+        rospy.wait_for_service('/dji_sdk/mission_waypoint_upload')
+        rospy.wait_for_service('/dji_sdk/mission_waypoint_action')
+        rospy.wait_for_service('/dji_sdk/drone_task_control')
+        rospy.loginfo("all services ready!")
+
+        rospy.loginfo("waiting for parameters...")
+
+        while not rospy.has_param("~drone_id"):
+            rospy.loginfo("waiting for drone_id param...")
+        self.droneId = rospy.get_param("~drone_id")
+
+        while not rospy.has_param("~base_speed"):
+            rospy.loginfo("waiting for base_speed param...")
+        self.baseSpeed = rospy.get_param("~base_speed")
+
+        rospy.loginfo("all parameters recieved!")
+
+        self.sdkControlProxy = rospy.ServiceProxy('/dji_sdk/sdk_control_authority', SDKControlAuthority)
+        self.missionWaypointUploadProxy = rospy.ServiceProxy('/dji_sdk/mission_waypoint_upload', MissionWpUpload)
+        self.missionWaypointActionProxy = rospy.ServiceProxy('/dji_sdk/mission_waypoint_action', MissionWpAction)
+        self.droneTaskControlProxy = rospy.ServiceProxy('/dji_sdk/drone_task_control', DroneTaskControl)
+        rospy.Service('waypoint_control', WaypointControl, self.controlService)
+
+        rospy.loginfo("requesting sdk control authority...")
+        response = self.sdkControlProxy(np.uint8(1))
+        rospy.loginfo(response)
+        rospy.loginfo("obtained control authority.")
+
+if __name__ == '__main__':
+    wayptDriver = WaypointDriver()
+    rospy.spin()

Onboard-SDK-ROS/dji_sdk/scripts/waypointGenerator.py

+#!/usr/bin/python3
+from pyproj import Transformer
+import numpy as np
+import rospy
+import math
+
+def meters2lat_lon(meters_point):
+    # returns point converted to lat lon from meters coordinate system
+    transformer = Transformer.from_crs(3857, 4326)
+    latlon_point = transformer.transform(meters_point[0], meters_point[1])
+    return latlon_point
+
+def lat_lon2meters(latlon_point):
+    # returns point converted to meters from lat lon coordinate system
+    transformer = Transformer.from_crs(4326, 3857)
+    meters_point = transformer.transform(latlon_point[0], latlon_point[1])
+    return meters_point
+
+def normalizeAngle(angle):
+    newAngle = angle%360
+    newAngle = (newAngle + 360)%360
+    if(newAngle >= 180):
+        newAngle -= 360
+    return newAngle
+
+def point_rotation(origin, pt, ang):
+    # returns the pt rotated about the origin by ang (ang in degrees)
+    c = math.cos(math.radians(ang))
+    s = math.sin(math.radians(ang))
+    # translate to origin
+    pt_temp = [pt[0] - origin[0], pt[1] - origin[1]]
+    pt_spun = [ pt_temp[0]*c - pt_temp[1]*s, pt_temp[0]*s + pt_temp[1]*c ]
+    # translate back to frame
+    pt_spun = [pt_spun[0] + origin[0], pt_spun[1] + origin[1]]
+    return pt_spun
+
+def generateWaypoints():
+    rospy.init_node("waypointGenerator")
+    anchorPoint = rospy.get_param("~anchorPoint")
+    pathPoints = rospy.get_param("~pathPoints")
+    pathType = rospy.get_param("~pathType")
+    baseAltitude = rospy.get_param("~altitude")
+    droneNum = int(rospy.get_param("~droneNum"))
+    spiralRadius = rospy.get_param("~spiralRadius")
+    spiralHeight = rospy.get_param("~spiralHeight")
+    spiralLoops = int(rospy.get_param("~spiralLoops"))
+    altitudeIncrement = rospy.get_param("~altitudeIncrement") # how much to separate drones on z axis
+
+    apMeters = lat_lon2meters(anchorPoint)
+    tempDict = {"waypoints" : {}, "positions" : {}, "numWaypoints" : 0, "numPositions" : 0}
+    waypointDict = {}
+
+    # path type if
+    if (pathType == "points" and len(pathPoints) != 0):
+
+        for d in range(droneNum):
+            waypointDict["robot_{}".format(d)] = {}
+            waypointDict["robot_{}".format(d)] = {}
+            waypointDict["robot_{}".format(d)]["waypoints"] = {}
+            waypointDict["robot_{}".format(d)]["positions"] = {}
+
+            # quick check if path points has levels
+            try:
+                for i, pt in enumerate(pathPoints[d]):
+                # for i, offset in enumerate(waypointOffsets):
+                    waypointLatLon = pt
+                    heading = 0.0
+                    latLonPt = lat_lon2meters(pt)
+                    positionXY = [latLonPt[0] - apMeters[0], latLonPt[1] - apMeters[1]]
+
+                    # waypoints
+                    waypointDict["robot_{}".format(d)]["waypoints"].update({"waypoint_{}".format(i) : [i, waypointLatLon[0], waypointLatLon[1], baseAltitude + d*altitudeIncrement, heading]})
+                    # positions
+                    waypointDict["robot_{}".format(d)]["positions"].update({"position_{}".format(i) : [i, positionXY[0], positionXY[1], baseAltitude + d*altitudeIncrement, heading]})
+
+                # do this betta
+                waypointDict["robot_{}".format(d)]["numWaypoints"] = len(pathPoints[d])
+                waypointDict["robot_{}".format(d)]["numPositions"] = len(pathPoints[d])
+            except TypeError as e:
+                print("path points are not ordered correctly")
+                rospy.loginfo("path points are not ordered correctly")
+
+    elif (pathType == "squares"):
+        # musical drones trajectory is convient test case, but general path is possible
+        # ring sizes
+        big = 10.0
+        med = 5.0
+        sml = 2.0
+
+        waypointOffsets = [
+        [-big, -big],
+        [big, -big],
+        [big, big],
+        [med, med],
+        [-med, med],
+        [-med, -med],
+        [-sml, -sml],
+        [sml, -sml],
+        [sml, sml],
+        [big, -big],
+        ]
+
+        for d in range(droneNum):
+            # rospy.loginfo("blah!")
+            # print(d)
+            waypointDict["robot_{}".format(d)] = {}
+            waypointDict["robot_{}".format(d)] = {}
+            waypointDict["robot_{}".format(d)]["waypoints"] = {}
+            waypointDict["robot_{}".format(d)]["positions"] = {}
+            for i, offset in enumerate(waypointOffsets):
+                waypointLatLon = meters2lat_lon([apMeters[0] + offset[0], apMeters[1] + offset[1]])
+                waypointLatLonRev = meters2lat_lon([apMeters[0] - offset[0], apMeters[1] - offset[1]])
+                heading = normalizeAngle(45*i)
+
+                # waypoints
+                waypointDict["robot_{}".format(d)]["waypoints"].update({"waypoint_{}".format(i) : [i, waypointLatLon[0], waypointLatLon[1], baseAltitude + d*waypointAltitudeIncrement, heading]})
+                # positions
+                waypointDict["robot_{}".format(d)]["positions"].update({"position_{}".format(i) : [i, offset[0], offset[1], baseAltitude + d*waypointAltitudeIncrement, heading]})
+
+            # do this betta
+            waypointDict["robot_{}".format(d)]["numWaypoints"] = len(waypointOffsets)
+            waypointDict["robot_{}".format(d)]["numPositions"] = len(waypointOffsets)
+    elif (pathType == "spiral"):
+        # sprial path with heading pointing towards center of spiral
+        theta = 0.0
+        radians = np.pi*2.0*spiralLoops # total radians in spiral
+        loopPoints = 20 # 20 steps per single loop
+        totalPoints = loopPoints*spiralLoops
+        thetaStep = radians/totalPoints
+        heightStep = spiralHeight/totalPoints
+        height = 0.0
+
+        for d in range(droneNum):
+            waypointDict["robot_{}".format(d)] = {}
+            waypointDict["robot_{}".format(d)] = {}
+            waypointDict["robot_{}".format(d)]["waypoints"] = {}
+            waypointDict["robot_{}".format(d)]["positions"] = {}
+            # spiral loop
+            for i in range(totalPoints):
+                offset = [spiralRadius*math.cos(theta), spiralRadius*math.sin(theta)]
+                waypointLatLon = meters2lat_lon([apMeters[0] + offset[0], apMeters[1] + offset[1]])
+                heading = normalizeAngle(math.degrees(-theta - (np.pi/2.0)))
+
+                # waypoints
+                waypointDict["robot_{}".format(d)]["waypoints"].update({"waypoint_{}".format(i) : [i, waypointLatLon[0], waypointLatLon[1], baseAltitude + height + d*altitudeIncrement, heading]})
+                # positions
+                waypointDict["robot_{}".format(d)]["positions"].update({"position_{}".format(i) : [i, offset[0], offset[1], baseAltitude + height + d*altitudeIncrement, heading]})
+                theta = theta + thetaStep
+                height = height + heightStep
+
+            # do this betta
+            waypointDict["robot_{}".format(d)]["numWaypoints"] = totalPoints
+            waypointDict["robot_{}".format(d)]["numPositions"] = totalPoints
+    rospy.set_param("/robot_list", waypointDict)
+    return
+    # print(waypointDict)
+
+if __name__ == "__main__":
+    generateWaypoints()

Onboard-SDK-ROS/dji_sdk/setup.py

+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+setup_args = generate_distutils_setup(
+    packages=['dji_sdk'],
+    package_dir={'':'src'},
+)
+
+setup(**setup_args)
+

Onboard-SDK-ROS/dji_sdk/src/main.cpp

+/** @file main.cpp
+ *  @version 3.7
+ *  @date July, 2018
+ *
+ *  @brief
+ *  DJISDKNode
+ *
+ *  @copyright 2018 DJI. All rights reserved.
+ *
+ */
+
+#include <dji_sdk/dji_sdk_node.h>
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "dji_sdk");
+  ros::NodeHandle nh;
+  ros::NodeHandle nh_private("~");
+
+  DJISDKNode* dji_sdk_node = new DJISDKNode(nh, nh_private);
+
+  ros::AsyncSpinner spinner(4); // Use 4 threads
+  spinner.start();
+  ros::waitForShutdown();
+
+  delete dji_sdk_node;
+  dji_sdk_node = NULL;
+
+  return 0;
+}

Onboard-SDK-ROS/dji_sdk/src/modules/dji_sdk_node_control.cpp

+/** @file dji_sdk_node_control.cpp
+ *  @version 3.7
+ *  @date July, 2018
+ *
+ *  @brief
+ *  Implementation of the control functions of DJISDKNode
+ *
+ *  @copyright 2018 DJI. All rights reserved.
+ *
+ */
+
+#include <dji_sdk/dji_sdk_node.h>
+
+/*!
+ * @brief The flight controller takes control signals with frame convention:
+ *   body:   Forward-Right for horizontal
+ *           CW = + for yaw
+ *           U  = + for z
+ *   ground: North-East for horizontal,
+ *           CW = + for yaw ,
+ *           U  = + for z ,
+ *
+ *  However, the telemetry data in our node follows REP 103:
+ *    body: FLU
+ *    ground: ENU
+ *  The client code is encouraged to follow the REP 103, which means that
+ *  the control signal the client generates is FLU or ENU, and needs to be
+ *  transformed to the convention of the flight controller.
+ */
+void DJISDKNode::flightControl(uint8_t flag, float xSP, float ySP, float zSP, float yawSP)
+{
+  uint8_t HORI  = (flag & 0xC0);
+  uint8_t VERT  = (flag & 0x30);
+  uint8_t YAW   = (flag & 0x08);
+  uint8_t FRAME = (flag & 0x06);
+  uint8_t HOLD  = (flag & 0x01);
+
+  double xCmd, yCmd, zCmd, yawCmd;
+  if (FRAME == Control::HORIZONTAL_GROUND)
+  {
+    // 1.1 Horizontal channels
+    if ( (HORI == Control::HORIZONTAL_VELOCITY) || (HORI == Control::HORIZONTAL_POSITION) )
+    {
+      xCmd = ySP;
+      yCmd = xSP;
+    }
+    else
+    {
+      //ROS_DEBUG("GROUND frame is specified, but angle and rate command is generated in body frame");
+      xCmd = RAD2DEG(xSP);
+      yCmd = RAD2DEG(-ySP);
+    }
+
+    // 1.2 Verticle Channel
+    if ( (VERT == Control::VERTICAL_VELOCITY) || (VERT == Control::VERTICAL_POSITION) )
+    {
+      zCmd = zSP;
+    }
+    else
+    {
+      //ROS_WARN_THROTTLE(1.0, "GROUND frame is specified, but thrust command is generated in body frame");
+      zCmd = zSP;
+    }
+  }
+  else if(FRAME == Control::HORIZONTAL_BODY)
+  {
+    // 2.1 Horizontal channels
+    if ( (HORI == Control::HORIZONTAL_VELOCITY) || (HORI == Control::HORIZONTAL_POSITION) )
+    {
+      // The X and Y Vel and Pos should be only based on rotation after Yaw,
+      // whithout roll and pitch. Otherwise the behavior will be weird.
+
+      // Transform from F-R to F-L
+      xCmd = xSP;
+      yCmd = -ySP;
+    }
+    else
+    {
+      xCmd = RAD2DEG(xSP);
+      yCmd = RAD2DEG(-ySP);
+    }
+
+    // 2.2 Vertical channel
+    if ( (VERT == Control::VERTICAL_VELOCITY) || (VERT == Control::VERTICAL_POSITION)  )
+    {
+      //ROS_WARN_THROTTLE(1.0, "BODY frame is specified, but hight and z-velocity is generated in ground frame");
+      zCmd = zSP;
+    }
+    else
+    {
+      zCmd = zSP;
+    }
+  }
+
+  // The behavior of yaw should be the same in either frame
+  if ( YAW == Control::YAW_ANGLE )
+  {
+    tf::Matrix3x3 rotationSrc;
+    rotationSrc.setRPY(0.0, 0.0, yawSP);
+
+    //The last term should be transpose, but since it's symmetric ...
+    tf::Matrix3x3 rotationDes (R_ENU2NED * rotationSrc * R_FLU2FRD);
+
+    double temp1, temp2;
+    rotationDes.getRPY(temp1, temp2, yawCmd);
+
+    yawCmd = RAD2DEG(yawCmd);
+  }
+  else if (YAW == Control::YAW_RATE)
+  {
+    yawCmd = RAD2DEG(-yawSP);
+  }
+
+  Control::CtrlData ctrlData(flag, xCmd, yCmd, zCmd, yawCmd);
+  vehicle->control->flightCtrl(ctrlData);
+}
+
+void
+DJISDKNode::flightControlSetpointCallback(
+  const sensor_msgs::Joy::ConstPtr& pMsg)
+{ 
+  float xSP    = pMsg->axes[0];
+  float ySP    = pMsg->axes[1];
+  float zSP    = pMsg->axes[2];
+  float yawSP  = pMsg->axes[3];
+  uint8_t flag = (uint8_t)(pMsg->axes[4]);
+
+  flightControl(flag, xSP, ySP, zSP, yawSP);
+}
+
+void
+DJISDKNode::flightControlPxPyPzYawCallback(
+  const sensor_msgs::Joy::ConstPtr& pMsg)
+{
+  uint8_t flag = (Control::VERTICAL_POSITION |
+                  Control::HORIZONTAL_POSITION |
+                  Control::YAW_ANGLE |
+                  Control::HORIZONTAL_GROUND |
+                  Control::STABLE_ENABLE);
+
+  float px    = pMsg->axes[0];
+  float py    = pMsg->axes[1];
+  float pz    = pMsg->axes[2];
+  float yaw   = pMsg->axes[3];
+
+  flightControl(flag, px, py, pz, yaw);
+}
+
+void
+DJISDKNode::flightControlVxVyVzYawrateCallback(
+  const sensor_msgs::Joy::ConstPtr& pMsg)
+{
+  uint8_t flag = (Control::VERTICAL_VELOCITY |
+                  Control::HORIZONTAL_VELOCITY |
+                  Control::YAW_RATE |
+                  Control::HORIZONTAL_GROUND |
+                  Control::STABLE_ENABLE);
+  float vx        = pMsg->axes[0];
+  float vy        = pMsg->axes[1];
+  float vz        = pMsg->axes[2];
+  float yawRate   = pMsg->axes[3];
+
+  flightControl(flag, vx, vy, vz, yawRate);
+}
+
+void
+DJISDKNode::flightControlRollPitchPzYawrateCallback(
+  const sensor_msgs::Joy::ConstPtr& pMsg)
+{
+  uint8_t flag = (Control::VERTICAL_POSITION |
+                  Control::HORIZONTAL_ANGLE |
+                  Control::YAW_RATE |
+                  Control::HORIZONTAL_BODY |
+                  Control::STABLE_ENABLE);
+
+  float roll      = pMsg->axes[0];
+  float pitch     = pMsg->axes[1];
+  float pz        = pMsg->axes[2];
+  float yawRate   = pMsg->axes[3];
+
+  flightControl(flag, roll, pitch, pz, yawRate);
+}
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/src/modules/dji_sdk_node_mobile_comm.cpp

+/** @file dji_sdk_node_mobile_comm.cpp
+ *  @version 3.7
+ *  @date July, 2018
+ *
+ *  @brief
+ *  Implementation of the mobile communication functions of DJISDKNode
+ *
+ *  @copyright 2018 DJI. All rights reserved.
+ *
+ */
+
+#include <dji_sdk/dji_sdk_node.h>
+
+void DJISDKNode::SDKfromMobileDataCallback(Vehicle *vehicle, RecvContainer recvFrame, DJI::OSDK::UserData userData) {
+  ((DJISDKNode*)userData)->fromMobileDataCallback(recvFrame);
+}
+
+void DJISDKNode::fromMobileDataCallback(RecvContainer recvFrame) {
+  int dataLength = recvFrame.recvInfo.len - OpenProtocol::PackageMin - 2;
+  if (dataLength <= 100) {
+    DSTATUS( "Received mobile Data of len %d\n", recvFrame.recvInfo.len);
+    dji_sdk::MobileData mobile_data;
+    mobile_data.data.resize(dataLength);
+    for (int i=0; i<dataLength; i++)
+    {
+      mobile_data.data[i] = recvFrame.recvData.raw_ack_array[i];
+    }
+    from_mobile_data_publisher.publish(mobile_data);
+  }
+}
+
+bool DJISDKNode::sendToMobileCallback(dji_sdk::SendMobileData::Request& request,
+                                      dji_sdk::SendMobileData::Response& response){
+  vehicle->moc->sendDataToMSDK(&request.data[0], request.data.size());
+  response.result = true;
+  return true;
+}

Onboard-SDK-ROS/dji_sdk/src/modules/dji_sdk_node_subscriber.cpp

+/** @file dji_sdk_node_subscriber.cpp
+ *  @version 3.7
+ *  @date July, 2018
+ *
+ *  @brief
+ *  Implementation of the subscribers of DJISDKNode
+ *
+ *  @copyright 2018 DJI. All rights reserved.
+ *
+ */
+
+#include <dji_sdk/dji_sdk_node.h>
+
+void
+DJISDKNode::gimbalAngleCtrlCallback(const dji_sdk::Gimbal::ConstPtr& msg)
+{
+  ROS_DEBUG("called gimbalAngleCtrlCallback");
+
+  DJI::OSDK::Gimbal::AngleData angle_data;
+  //! OSDK takes 0.1 sec as unit
+  angle_data.duration = msg->ts*10;
+  angle_data.mode     = msg->mode;
+  //! OSDK takes 0.1 deg as unit
+  angle_data.roll     = RAD2DEG(msg->roll)*10;
+  angle_data.pitch    = RAD2DEG(msg->pitch)*10;
+  angle_data.yaw      = RAD2DEG(msg->yaw)*10;
+  vehicle->gimbal->setAngle(&angle_data);
+}
+
+void
+DJISDKNode::gimbalSpeedCtrlCallback(
+  const geometry_msgs::Vector3Stamped::ConstPtr& msg)
+{
+  ROS_DEBUG("called gimbalAngleCtrlCallback");
+
+  DJI::OSDK::Gimbal::SpeedData speed_data;
+  //! OSDK takes 0.1 deg as unit
+  speed_data.gimbal_control_authority = 1;
+  speed_data.roll  = RAD2DEG(msg->vector.x)*10;
+  speed_data.pitch = RAD2DEG(msg->vector.y)*10;
+  speed_data.yaw   = RAD2DEG(msg->vector.z)*10;
+  vehicle->gimbal->setSpeed(&speed_data);
+}

Onboard-SDK-ROS/dji_sdk/srv/Activation.srv

+---
+bool result
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/srv/CameraAction.srv

+#constant for camera_action
+uint8 CAMERA_ACTION_TAKE_PICTURE = 0
+uint8 CAMERA_ACTION_START_RECORD = 1
+uint8 CAMERA_ACTION_STOP_RECORD  = 2
+
+#request
+uint8 camera_action     # see constants above for possible camera_actions
+---
+bool result

Onboard-SDK-ROS/dji_sdk/srv/DroneArmControl.srv

+#constant for arm
+uint8 DISARM_COMMAND = 0
+uint8 ARM_COMMAND    = 1
+
+#request
+uint8 arm    # see constants
+---
+bool result
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data

Onboard-SDK-ROS/dji_sdk/srv/DroneTaskControl.srv

+#constant for tasks
+uint8 TASK_GOHOME = 1
+uint8 TASK_TAKEOFF = 4
+uint8 TASK_LAND = 6
+
+#request
+uint8 task    # see constants above for possible tasks
+---
+bool result
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data

Onboard-SDK-ROS/dji_sdk/srv/MFIOConfig.srv

+#constant for mode
+uint8 MODE_PWM_OUT  = 0
+#uint8 MODE_PWM_IN  = 1 #PWM_IN is not functioning correctly
+uint8 MODE_GPIO_OUT = 2
+uint8 MODE_GPIO_IN  = 3
+uint8 MODE_ADC      = 4
+
+#request
+uint8 mode              # see constants above for possible modes
+uint8 channel           # 0-7
+uint32 init_on_time_us  # on time for pwm duty cycle in micro-seconds
+uint16 pwm_freq         # set pwm frequency in Hz
+---

Onboard-SDK-ROS/dji_sdk/srv/MFIOSetValue.srv

+uint8 channel   # 0-7
+uint32 init_on_time_us    # on time for pwm duty cycle in micro-seconds
+---

Onboard-SDK-ROS/dji_sdk/srv/MissionHpAction.srv

+#constant for action
+uint8 ACTION_START  = 0
+uint8 ACTION_STOP   = 1
+uint8 ACTION_PAUSE  = 2
+uint8 ACTION_RESUME = 3
+
+#response
+uint8 action          # see constants above for possible actions
+---
+bool result
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data

Onboard-SDK-ROS/dji_sdk/srv/MissionHpGetInfo.srv

+---
+MissionHotpointTask hotpoint_task
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/srv/MissionHpResetYaw.srv

+---
+bool result
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/srv/MissionHpUpdateRadius.srv

+float32 radius      # in meters
+---
+bool result
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data

Onboard-SDK-ROS/dji_sdk/srv/MissionHpUpdateYawRate.srv

+float32 yaw_rate    # degree/s
+uint8 direction     # 0: counter-clockwise, 1: clockwise
+---
+bool result
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data

Onboard-SDK-ROS/dji_sdk/srv/MissionHpUpload.srv

+MissionHotpointTask hotpoint_task
+---
+bool result
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/srv/MissionStatus.srv

+---
+uint8 waypoint_mission_count
+uint8 hotpoint_mission_count
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/srv/MissionWpAction.srv

+#constant for action
+uint8 ACTION_START  = 0
+uint8 ACTION_STOP   = 1
+uint8 ACTION_PAUSE  = 2
+uint8 ACTION_RESUME = 3
+
+#request
+uint8 action    # see constants for possible actions
+---
+bool result
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data

Onboard-SDK-ROS/dji_sdk/srv/MissionWpGetInfo.srv

+---
+MissionWaypointTask waypoint_task
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/srv/MissionWpGetSpeed.srv

+---
+float32 speed
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/srv/MissionWpSetSpeed.srv

+float32 speed
+---
+bool result
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/srv/MissionWpUpload.srv

+MissionWaypointTask waypoint_task   # see msg/MissionWaypointTask.msg
+---
+bool result
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data

Onboard-SDK-ROS/dji_sdk/srv/QueryDroneVersion.srv

+---
+uint32 version
+string hardware
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/srv/SDKControlAuthority.srv

+#constant for control_enable
+uint8 RELEASE_CONTROL = 0
+uint8 REQUEST_CONTROL = 1
+
+#request
+uint8 control_enable    # see constants above
+---
+bool result
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data

Onboard-SDK-ROS/dji_sdk/srv/SendMobileData.srv

+uint8[] data  #length(data) <= 100
+---
+bool result

Onboard-SDK-ROS/dji_sdk/srv/SetHardSync.srv

+uint32 frequency  # frequency in Hz
+uint16 tag        # the tag is to distinguish between different call
+----
+uint8 result

Onboard-SDK-ROS/dji_sdk/srv/SetLocalPosRef.srv

+#set local position
+# true: set Local position reference
+
+#request
+---
+bool result

Onboard-SDK-ROS/dji_sdk/srv/SetupCameraStream.srv

+#constant for vga image frequency
+uint8 FPV_CAM  = 0
+uint8 MAIN_CAM = 1
+
+# use above constants to config freq.
+uint8 cameraType
+
+# 1 for start camera stream, 0 for stop
+uint8 start
+
+---
+bool result
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/srv/Stereo240pSubscription.srv

+# assign value to 1 to subscribe
+uint8 front_right_240p
+uint8 front_left_240p
+uint8 down_front_240p
+uint8 down_back_240p
+
+# if unsubscribe_240p is 1,
+# service will unsubscribe no matter what
+uint8 unsubscribe_240p
+
+---
+bool result

Onboard-SDK-ROS/dji_sdk/srv/StereoDepthSubscription.srv

+# assign value to 1 to subscribe
+uint8 front_depth_240p
+
+# if unsubscribe_240p is 1,
+# service will unsubscribe no matter what
+uint8 unsubscribe_240p
+
+---
+bool result
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/srv/StereoVGASubscription.srv

+#constant for vga image frequency
+uint8 VGA_20_HZ = 0
+uint8 VGA_10_HZ = 1
+
+# use above constants to config freq.
+uint8 vga_freq
+
+# assign value to 1 to subscribe
+uint8 front_vga
+
+# if unsubscribe_vga is 1,
+# service will unsubscribe no matter what
+uint8 unsubscribe_vga
+
+---
+bool result
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk/srv/WaypointControl.srv

+#constant for action
+uint8 TAKEOFF = 0
+uint8 LAND = 1
+uint8 START_WAYPOINTS  = 2
+uint8 STOP_WAYPOINTS = 3
+
+#request
+uint8 data # see constants for possible actions
+---
+bool result
+# for debugging usage
+uint8 cmd_set
+uint8 cmd_id
+uint32 ack_data

Onboard-SDK-ROS/dji_sdk_demo/cmake-modules/FindAdvancedSensing.cmake

+# Once done this will define
+#
+#  FOUND_ADVANCED_SENSING - system has advanced-sensing
+
+if (NOT FOUND_ADVANCED_SENSING)
+
+    find_path(ADVANCED_SENSING_LIB
+            NAMES
+            libadvanced-sensing.a
+            PATHS
+            /usr/lib
+            /usr/local/lib
+            /opt/local/lib
+            /sw/lib
+            PATH_SUFFIXES
+            lib
+            )
+
+    if(NOT ADVANCED_SENSING_LIB STREQUAL ADVANCED_SENSING_LIB-NOTFOUND)
+        message(STATUS "Found Advanced Sensing in the system")
+        set(FOUND_ADVANCED_SENSING TRUE)
+    endif()
+
+endif (NOT FOUND_ADVANCED_SENSING)
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk_demo/cmake-modules/FindOpenCVContribImgProc.cmake

+# Once done this will define
+#
+#  FOUND_OPENCV_CONTRIB_IMG_PROC - system has opencv contrib img proc module
+#  This boolean is currently not used in the system.
+#  It's nice to have and not required.
+
+if (NOT FOUND_OPENCV_CONTRIB_IMG_PROC)
+
+    find_path(OPENCV_CONTRIB_IMG_PROC
+            NAMES
+            disparity_filter.hpp
+            PATHS
+            /usr/include/opencv2/ximgproc
+            /usr/local/include/opencv2/ximgproc
+            /opt/local/include/opencv2/ximgproc
+            /sw/include/opencv2/ximgproc
+            PATH_SUFFIXES
+            ximgproc
+            )
+
+    if(NOT OPENCV_CONTRIB_IMG_PROC STREQUAL OPENCV_CONTRIB_IMG_PROC-NOTFOUND)
+        message(STATUS "Found ximgproc module in OpenCV, will use it to filter disparity map in depth perception sample")
+        set(FOUND_OPENCV_CONTRIB_IMG_PROC TRUE)
+        add_definitions(-DUSE_OPEN_CV_CONTRIB)
+    endif()
+
+endif (NOT FOUND_OPENCV_CONTRIB_IMG_PROC)
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk_demo/cmake-modules/FindOpenCVViz.cmake

+# Once done this will define
+#
+#  FOUND_OPENCV_VIZ - system has advanced-sensing
+
+if (NOT FOUND_OPENCV_VIZ)
+
+    find_path(OPENCV_VIZ
+            NAMES
+            viz3d.hpp
+            PATHS
+            /usr/include/opencv2/viz
+            /usr/local/include/opencv2/viz
+            /opt/local/include/opencv2/viz
+            /sw/include/opencv2/viz
+            PATH_SUFFIXES
+            opencv2
+            )
+
+    if(NOT OPENCV_VIZ STREQUAL OPENCV_VIZ-NOTFOUND)
+        message(STATUS "Found viz3d in OpenCV, will use it to visualize point cloud")
+        set(FOUND_OPENCV_VIZ TRUE)
+    else()
+        message(STATUS "Did not find viz3d in OpenCV")
+    endif()
+
+endif (NOT FOUND_OPENCV_VIZ)
\ No newline at end of file

Onboard-SDK-ROS/dji_sdk_demo/include/dji_sdk_demo/demo_camera_gimbal.h

+/** @file demo_camera_gimbal.h
+ *  @version 3.3
+ *  @date May, 2017
+ *
+ *  @brief
+ *  demo sample of how to use camera and gimbal APIs
+ *
+ *  @copyright 2017 DJI. All rights reserved.
+ *
+ */
+
+#ifndef DEMO_CAMERA_GIMBAL_H
+#define DEMO_CAMERA_GIMBAL_H
+
+// System includes
+#include "unistd.h"
+#include <cstdint>
+#include <iostream>
+
+// DJI SDK includes
+#include <dji_sdk/Activation.h>
+#include <dji_sdk/CameraAction.h>
+#include <dji_sdk/Gimbal.h>
+
+// ROS includes
+#include <ros/ros.h>
+#include <geometry_msgs/Vector3Stamped.h>
+
+#define C_PI (double)3.141592653589793
+#define DEG2RAD(DEG) ((DEG) * ((C_PI) / (180.0)))
+#define RAD2DEG(RAD) ((RAD) * (180.0) / (C_PI))
+
+typedef struct ServiceAck{
+  bool result;
+  int cmd_set;
+  int cmd_id;
+  unsigned int ack_data;
+  ServiceAck(bool res, int set, int id, unsigned int ack):
+    result(res), cmd_set(set), cmd_id(id), ack_data(ack) {};
+  ServiceAck() {};
+}ServiceAck;
+
+// Be precise here
+struct RotationAngle{
+  float roll;
+  float pitch;
+  float yaw;
+};
+
+struct GimbalContainer{
+  int roll = 0;
+  int pitch = 0;
+  int yaw = 0;
+  int duration = 0;
+  int isAbsolute = 0;
+  bool yaw_cmd_ignore = false;
+  bool pitch_cmd_ignore = false;
+  bool roll_cmd_ignore = false;
+  RotationAngle initialAngle;
+  RotationAngle currentAngle;
+  GimbalContainer( int roll = 0,
+                   int pitch = 0,
+                   int yaw = 0,
+                   int duration = 0,
+                   int isAbsolute = 0,
+                   RotationAngle initialAngle = {},
+                   RotationAngle currentAngle = {}):
+    roll(roll), pitch(pitch), yaw(yaw),
+    duration(duration),isAbsolute(isAbsolute),
+    initialAngle(initialAngle), currentAngle(currentAngle){}
+};
+
+struct ResultContainer{
+  int angle[3] = {0};
+  int error[3] = {0};
+};
+
+void doSetGimbalAngle(GimbalContainer *gimbal);
+
+bool gimbalCameraControl();
+
+void displayResult(RotationAngle *currentAngle);
+
+ServiceAck activate();
+
+bool takePicture();
+
+bool startVideo();
+
+bool stopVideo();
+
+void gimbalAngleCallback(const geometry_msgs::Vector3Stamped::ConstPtr& msg);
+
+#endif //DEMO_CAMERA_GIMBAL_H

image_pipeline @ c4527fdf

+Subproject commit c4527fdf0de81241f6809ee2694bb3b3b7b49e07