init mission controller stuff

Onboard-SDK-ROS/launch/local_test.launch

@@ -36,19 +36,18 @@
           <param name="trackName" type="string" value="M100_CarbonFiber"/>
         </node> -->
 
-    <node name="waypointGenerator" type="waypointGeneratorRemote.py" pkg="dji_osdk_ros">
+    <!-- <node name="waypointGenerator" type="waypointGeneratorRemote.py" pkg="dji_osdk_ros">
       <rosparam command="load" param="anchorPoint"> [37.196941, -80.578335] </rosparam>
       <rosparam command="load" param="pathPoints">
-	[
+        [
         [37.197879, -80.578022],
         [37.198037, -80.577689],
         [37.198161, -80.577359],
         [37.198161, -80.577072],
         [37.197879, -80.576637]
         ]
+      </rosparam>
       <rosparam command="load" param="altitude"> 10.0 </rosparam>
       <rosparam command="load" param="droneNum"> 3 </rosparam>
-    </node>
-
-    <!-- <node pkg="dji_osdk_ros" type="image_saver.py" name="image_saver" output="screen"/> -->
+    </node> -->
 </launch>

misson_controller/CMakeLists.txt → mission_controller/CMakeLists.txt

 cmake_minimum_required(VERSION 3.0.2)
-project(misson_controller)
+project(mission_controller)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
@@ -13,6 +13,8 @@ find_package(catkin REQUIRED COMPONENTS
   rospy
   sensor_msgs
   std_msgs
+  message_generation
+  dji_osdk_ros
 )
 
 ## System dependencies are found with CMake's conventions
@@ -48,19 +50,133 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * 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
-#   Message1.msg
-#   Message2.msg
-# )
+## Generate msg in the 'msg' folder
+add_message_files(
+  FILES
+
+  FCTimeInUTC.msg
+  JoystickParams.msg
+  GPSUTC.msg
+  Gimbal.msg
+  Waypoint.msg
+  WaypointList.msg
+  WaypointV2AircraftControlActuatorFlying.msg
+  WaypointV2AircraftControlActuatorRotateHeading.msg
+  WaypointV2AircraftControlActuator.msg
+  WaypointV2AssociateTrigger.msg
+  WaypointV2CameraActuatorFocalLengthParam.msg
+  WaypointV2CameraActuatorFocusParam.msg
+  WaypointV2CameraActuator.msg
+  WaypointV2Config.msg
+  WaypointV2GimbalActuatorRotationParam.msg
+  WaypointV2GimbalActuator.msg
+  WaypointV2InitSetting.msg
+  WaypointV2IntervalTrigger.msg
+  WaypointV2ReachpointTrigger.msg
+  WaypointV2SampleReachPointTrigger.msg
+  WaypointV2TrajectoryTrigger.msg
+  WaypointV2Action.msg
+  WaypointV2.msg
+  WaypointV2MissionEventPush.msg
+  WaypointV2MissionStatePush.msg
+
+  MobileData.msg
+  PayloadData.msg
+  MissionWaypointAction.msg
+  MissionWaypoint.msg
+  MissionWaypointTask.msg
+  MissionHotpointTask.msg
+  FlightAnomaly.msg
+  VOPosition.msg
+  BatteryState.msg
+  BatteryWholeInfo.msg
+  SmartBatteryState.msg
+  SmartBatteryDynamicInfo.msg
+  HMSPushInfo.msg
+)
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+
+  GetDroneType.srv
+  GetM300StereoParams.srv
+  FlightTaskControl.srv
+  SetJoystickMode.srv
+  JoystickAction.srv
+  ObtainControlAuthority.srv
+  KillSwitch.srv
+  EmergencyBrake.srv
+  GimbalAction.srv
+  CameraEV.srv
+  CameraShutterSpeed.srv
+  CameraAperture.srv
+  CameraISO.srv
+  CameraFocusPoint.srv
+  CameraTapZoomPoint.srv
+  CameraSetZoomPara.srv
+  CameraZoomCtrl.srv
+  CameraStartShootSinglePhoto.srv
+  CameraStartShootBurstPhoto.srv
+  CameraStartShootAEBPhoto.srv
+  CameraStartShootIntervalPhoto.srv
+  CameraStopShootPhoto.srv
+  CameraRecordVideoAction.srv
+  GetWholeBatteryInfo.srv
+  GetSingleBatteryDynamicInfo.srv
+  MFIO.srv
+  SetGoHomeAltitude.srv
+  GetGoHomeAltitude.srv
+  SetCurrentAircraftLocAsHomePoint.srv
+  SetHomePoint.srv
+  SetupCameraH264.srv
+  SetAvoidEnable.srv
+  GetAvoidEnable.srv
+  InitWaypointV2Setting.srv
+  UploadWaypointV2Mission.srv
+  UploadWaypointV2Action.srv
+  DownloadWaypointV2Mission.srv
+  StartWaypointV2Mission.srv
+  StopWaypointV2Mission.srv
+  PauseWaypointV2Mission.srv
+  ResumeWaypointV2Mission.srv
+  GenerateWaypointV2Action.srv
+  SetGlobalCruisespeed.srv
+  GetGlobalCruisespeed.srv
+  GetHMSData.srv
+  SubscribeWaypointV2Event.srv
+  SubscribeWaypointV2State.srv
+
+  Activation.srv
+  CameraAction.srv
+  DroneTaskControl.srv
+  SDKControlAuthority.srv
+  SetLocalPosRef.srv
+  MFIOConfig.srv
+  MFIOSetValue.srv
+  DroneArmControl.srv
+  MissionStatus.srv
+  MissionWpAction.srv
+  MissionHpAction.srv
+  MissionWpUpload.srv
+  MissionWpSetSpeed.srv
+  MissionWpGetSpeed.srv
+  MissionWpGetInfo.srv
+  MissionHpUpload.srv
+  MissionHpGetInfo.srv
+  MissionHpUpdateYawRate.srv
+  MissionHpUpdateRadius.srv
+  MissionHpResetYaw.srv
+  SendMobileData.srv
+  SendPayloadData.srv
+  SetHardSync.srv
+  QueryDroneVersion.srv
+  Stereo240pSubscription.srv
+  StereoVGASubscription.srv
+  StereoDepthSubscription.srv
+  SetupCameraStream.srv
+  Overwatch.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -70,10 +186,12 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   geometry_msgs#   sensor_msgs#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  geometry_msgs
+  sensor_msgs
+  std_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -105,10 +223,10 @@ find_package(catkin REQUIRED COMPONENTS
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES misson_controller
-#  CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs
-#  DEPENDS system_lib
+  INCLUDE_DIRS include
+  LIBRARIES mission_controller
+  CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs message_runtime
+  DEPENDS system_lib
 )
 
 ###########
@@ -117,14 +235,11 @@ catkin_package(
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-include_directories(
-# include
-  ${catkin_INCLUDE_DIRS}
-)
+include_directories(include ${catkin_INCLUDE_DIRS})
 
 ## Declare a C++ library
 # add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/misson_controller.cpp
+#   src/${PROJECT_NAME}/mission_controller.cpp
 # )
 
 ## Add cmake target dependencies of the library
@@ -135,7 +250,7 @@ include_directories(
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/misson_controller_node.cpp)
+add_executable(${PROJECT_NAME}_node src/mission_controller_node.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -145,7 +260,7 @@ include_directories(
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} dji_osdk_ros)
 
 ## Specify libraries to link a library or executable target against
 # target_link_libraries(${PROJECT_NAME}_node
@@ -181,11 +296,11 @@ include_directories(
 # )
 
 ## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
+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
@@ -199,7 +314,7 @@ include_directories(
 #############
 
 ## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_misson_controller.cpp)
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_mission_controller.cpp)
 # if(TARGET ${PROJECT_NAME}-test)
 #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 # endif()

mission_controller/include/misson_controller/mission_controller_node.h

+
+#ifndef MISSION_CONTROLLER_H
+#define MISSION_CONTROLLER_H
+
+
+// System includes
+#include "unistd.h"
+
+// DJI SDK includes
+#include <mission_controller/MissionWpAction.h>
+#include <mission_controller/MissionWpUpload.h>
+#include <mission_controller/Overwatch.h>
+
+// ROS includes
+#include <ros/ros.h>
+#include <sensor_msgs/NavSatFix.h>
+//#include <dji_vehicle.hpp>
+
+#include <math.h>       /* atan2 */
+#include <mission_controller/FlightTaskControl.h>
+#include <mission_controller/ObtainControlAuthority.h>
+#include <mission_controller/Overwatch.h>
+#include <geometry_msgs/Transform.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Quaternion.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <string>
+#include <fstream>
+#include <iostream>
+
+#include <Eigen3/Eigen/Geometry>
+#include <Eigen3/Eigen/Dense>
+
+// header
+int main(int argc, char** argv);

mission_controller/msg/BatteryState.msg

+uint8 voltageNotSafety # Generally caused by low temperature, the battery has electricity,
+                       # but the battery voltage is too low.
+uint8 veryLowVoltageAlarm
+uint8 LowVoltageAlarm
+uint8 seriousLowCapacityAlarm
+uint8 LowCapacityAlarm
\ No newline at end of file

mission_controller/msg/BatteryWholeInfo.msg

+uint16  remainFlyTime             # remain fly time(s)
+uint16  goHomeNeedTime            # Time required for the gohome flight (s)
+uint16  landNeedTime              # Time required for the land flight (s).max value:100*/
+uint16  goHomeNeedCapacity        # Capacity required for the gohome flight (%).max value:100*/
+uint16  landNeedCapacity          # Capacity required for the land flight (%).max value:100*/
+float32 safeFlyRadius             # Safe flight area radius (m)*/
+float32 capacityConsumeSpeed      # (mAh/sec)*/
+BatteryState batteryState
+uint8   goHomeCountDownState      # Countdown status of smart battery gohome
+                                   # 0/2:not in gohome state
+                                   # 1  :in gohome state
+                                   #
+uint8   gohomeCountDownvalue      # uint:s.max value:10
+uint16  voltage                   # mv
+uint8   batteryCapacityPercentage # uint:%.max value:100
+uint8   lowBatteryAlarmThreshold
+uint8   lowBatteryAlarmEnable
+uint8   seriousLowBatteryAlarmThreshold
+uint8   seriousLowBatteryAlarmEnable  
\ No newline at end of file

mission_controller/msg/FCTimeInUTC.msg

+# the time stamp of getting this data in the ROS system
+time stamp
+
+# the time stamp in FC
+uint32 fc_timestamp_us
+uint32 fc_utc_yymmdd
+uint32 fc_utc_hhmmss

mission_controller/msg/FlightAnomaly.msg

+# Here is an example how to use FlightAnomaly msg:
+#
+# void flightAnomalyCallback(const dji_osdk_ros::FlightAnomaly::ConstPtr & msg)
+# {
+#   uint32_t flightAnomalyData = msg->data;
+#   if(flightAnomalyData)
+#   {
+#     ROS_INFO("Flight Anomaly Reported by Flight Controller. Here are details:");
+#     if(flightAnomalyData && dji_osdk_ros::FlightAnomaly::COMPASS_INSTALLATION_ERROR)
+#     {
+#         ROS_INFO("COMPASS_INSTALLATION_ERROR");
+#     }
+#
+#     if(flightAnomalyData && dji_osdk_ros::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
+

mission_controller/msg/GPSUTC.msg

+# the time stamp of getting this data in the ROS system
+time stamp
+
+# the time stamp from GPS/RTK
+string UTCTimeData

mission_controller/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

mission_controller/msg/HMSPushInfo.msg

+uint32 alarmID     #/*! error code*/
+uint8  sensorIndex #/*! fault sensor's index*/
+uint8  reportLevel #/*! fault level ,0-4,0 is no error,4 is highest*/
\ No newline at end of file

mission_controller/msg/JoystickParams.msg

+float32 x   # Control with respect to the x axis of the
+            # DJI::OSDK::Control::HorizontalCoordinate.
+float32 y   # Control with respect to the y axis of the
+            # DJI::OSDK::Control::HorizontalCoordinate.
+float32 z   # Control with respect to the z axis, up is positive.
+float32 yaw #Yaw position/velocity control w.r.t. the ground frame.
\ No newline at end of file

mission_controller/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

mission_controller/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

mission_controller/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

mission_controller/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
+

mission_controller/msg/MobileData.msg

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

mission_controller/msg/PayloadData.msg

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

mission_controller/msg/SmartBatteryDynamicInfo.msg

+uint8  batteryIndex
+int32  currentVoltage          # uint:mV
+int32  currentElectric         # uint:mA
+uint32 fullCapacity            # uint:mAh
+uint32 remainedCapacity        # uint:mAh
+int16  batteryTemperature      # uint:0.1℃
+uint8  cellCount
+uint8  batteryCapacityPercent  # uint:%
+SmartBatteryState batteryState
+uint8  SOP                     # Relative power percentage
\ No newline at end of file

mission_controller/msg/SmartBatteryState.msg

+uint8 cellBreak                  # 0:normal;other:Undervoltage core index(0x01-0x1F)
+uint8 selfCheckError             # enum-type: DJISmartBatterySelfCheck
+uint8 batteryClosedReason        # enum-type: DJI_BETTERY_CLOSED_REASON
+uint8 batSOHState                # enum-type: DJISmartBatterySohState*/
+uint8 maxCycleLimit              # APP:cycle_limit*10*/
+uint8 batteryCommunicationAbnormal
+uint8 hasCellBreak
+uint8 heatState                  # enum-type: DJISmartBatteryHeatState
\ No newline at end of file

mission_controller/msg/VOPosition.msg

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

mission_controller/msg/Waypoint.msg

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

mission_controller/msg/WaypointList.msg

+Waypoint[] waypoint_list

mission_controller/msg/WaypointV2.msg

+# The struct represents a target point in the waypoint mission. For a waypoint
+
+#constant for flightpathMode
+uint8 DJIWaypointV2FlightPathModeGoToPointAlongACurve = 0  #In the mission, the aircraft will go to the waypoint along a curve and fly past the waypoint.
+uint8 DJIWaypointV2FlightPathModeGoToPointAlongACurveAndStop = 1 #In the mission, the aircraft will go to the waypoint along a curve and stop at the waypoint.
+uint8 DJIWaypointV2FlightPathModeGoToPointInAStraightLineAndStop = 2  #In the mission, the aircraft will go to the waypoint along a straight line and stop at the waypoint.
+uint8 DJIWaypointV2FlightPathModeCoordinateTurn = 3   #In the mission, the aircraft will fly from the previous waypoint to the next waypoint along a smooth curve without stopping at this waypoint.
+                                                      #the next in a curved motion,  adhering to the ``DJIWaypointV2_dampingDistance``, which is
+                                                      #set in ``DJIWaypointV2``.
+uint8 DJIWaypointV2FlightPathModeGoToFirstPointAlongAStraightLine = 4  # In the mission, the aircraft will go to the first waypoint along a straight line.
+                                                                       # This is only valid for the first waypoint.
+uint8 DJIWaypointV2FlightPathModeStraightOut = 5   # Straight exit the Last waypoint, Only valid for last waypoint.
+uint8 DJIWaypointV2FlightPathModeUnknown = 255    # Unknown
+
+#constant for headMode
+uint8 DJIWaypointV2HeadingModeAuto = 0    # Aircraft's heading will always be in the direction of flight.
+uint8 DJIWaypointV2HeadingFixed    = 1    # Aircraft's heading will be set to the heading when reaching the first waypoint.
+                                          # Before reaching the first waypoint, the aircraft's heading can be controlled by
+                                          # the remote controller. When the aircraft reaches the first waypoint, its
+                                          # heading will be fixed.
+uint8 DJIWaypointV2HeadingManual   = 2    # The aircraft's heading in the mission can be controlled by the remote controller.
+uint8 DJIWaypointV2HeadingWaypointCustom = 3  # In the mission, the aircraft's heading will change dynamically and adapt to the heading set at the next waypoint.
+                                               # See ``DJIWaypointV2_heading`` to preset the heading.
+uint8 DJIWaypointV2HeadingTowardPointOfInterest = 4 # Aircraft's heading will always toward point of interest.
+                                                    # using ``DJIWaypointV2_pointOfInterest`` setting point of interest coordiate and ``DJIWaypointV2_pointOfInterestAltitude``
+                                                    # setting point of interset altitute.
+uint8 DJIWaypointV2HeadingGimbalYawFollow  = 5   # The aircraft's heading rotate simultaneously with its gimbal's yaw.
+uint8 DJIWaypointV2HeadingUnknown = 255         # Unknown.
+
+#constant for turnMode
+uint8 DJIWaypointV2TurnModeClockwise = 0  # The aircraft's heading rotates clockwise.
+uint8 DJIWaypointV2TurnModeCounterClockwise = 1   # The aircraft's heading rotates counterclockwise.
+uint8 DJIWaypointV2TurnModeUnknown = 255    # Changes the heading of the aircraft by rotating the aircraft anti-clockwise.
+
+
+
+#  mission, a flight route  consists of multiple `WaypointV2` objects.
+float64 longitude  # waypoint position relative to WayPointV2InitSettings's reference point.unit: m
+float64 latitude
+float32 relativeHeight  # relative to takeoff height
+uint8   waypointType    # Waypoint flight path mode
+uint8   headingMode     # Represents current aircraft's heading mode on current waypoint.
+WaypointV2Config  config          # Represents current waypoint's speed config.
+uint16  dampingDistance
+float32 heading         # The heading to which the aircraft will rotate by the time it reaches the
+                        # waypoint. The aircraft heading  will gradually change between two waypoints with
+                        # different headings if the waypoint  mission's `headingMode` is set  to
+                        # 'DJIWaypointV2_DJIWaypointV2HeadingMode_WaypointCustom`. A heading has a range of
+                        # [-180, 180] degrees, where 0 represents True North.
+uint8 turnMode          # Determines whether the aircraft will turn clockwise or anticlockwise when
+                        # changing its heading.
+# Property is used when ``DJIWaypointV2_headingMode`` is
+# ``DJIWaypointV2_DJIWaypointV2HeadingMode_TowardPointOfInterest``.
+# Aircraft will always be heading to point while executing mission. Default is
+# "kCLLocationCoordinate2DInvalid".
+float32 positionX       # X distance to reference point, North is positive
+float32 positionY       # Y distance to reference point, East is positive
+float32 positionZ       # Z distance to reference point, UP is positive
+
+# While the aircraft is travelling between waypoints, you can offset its speed by
+# using the throttle joystick on the remote controller. "maxFlightSpeed" is this
+# offset when the joystick is pushed to maximum deflection. For example, If
+# maxFlightSpeed is 10 m/s, then pushing the throttle joystick all the way up will
+# add 10 m/s to the aircraft speed, while pushing down will subtract 10 m/s from
+# the aircraft speed. If the remote controller stick is not at maximum deflection,
+# then the offset speed will be interpolated between "[0, maxFlightSpeed]"" with a
+# resolution of 1000 steps. If the offset speed is negative, then the aircraft
+# will fly backwards to previous waypoints. When it reaches the first waypoint, it
+# will then hover in place until a positive speed is applied. "maxFlightSpeed" has
+# a range of [2,15] m/s.
+float32 maxFlightSpeed
+
+# The base automatic speed of the aircraft as it moves between waypoints with
+# range [-15, 15] m/s. The aircraft's actual speed is a combination of the base
+# automatic speed, and the speed control given by the throttle joystick on the
+# remote controller. If "autoFlightSpeed >0": Actual speed is "autoFlightSpeed" +
+# Joystick Speed (with combined max of "maxFlightSpeed") If "autoFlightSpeed =0":
+# Actual speed is controlled only by the remote controller joystick. If
+# autoFlightSpeed <0" and the aircraft is at the first waypoint, the aircraft
+# will hover in place until the speed is made positive by the remote controller
+# joystick. In flight controller firmware 3.2.10.0 or above, different speeds
+# between individual waypoints can also be set in waypoint objects which will
+# overwrite "autoFlightSpeed".
+float32 autoFlightSpeed
\ No newline at end of file

mission_controller/msg/WaypointV2Action.msg

+# This class represents an action for ``DJIWaypointV2Mission``. It
+# determines how action is performed when a waypoint mission is executed.
+
+# The action will be triggered when action associated executes.
+# The parameters should be defined by ``DJIWaypointV2Action_DJIWaypointV2AssociateTriggerParam``.
+uint8 DJIWaypointV2ActionTriggerTypeActionAssociated = 2
+
+# The action will be triggered when the aircraft flies from one waypoint to the next.
+# The parameters should be defined by ``DJIWaypointV2Action_DJIWaypointV2TrajectoryTriggerParam``.
+uint8 DJIWaypointV2ActionTriggerTypeTrajectory = 3
+
+# The action will be triggered when the aircraft flies between two waypoints
+# The parameters should be defined by ``DJIWaypointV2Action_DJIWaypointV2IntervalTriggerParam``.
+uint8 DJIWaypointV2ActionTriggerTypeInterval = 4
+uint8 DJIWaypointV2ActionTriggerTypeSampleReachPoint = 5
+
+# Unknown
+uint8 DJIWaypointV2ActionTriggerTypeUnknown = 255
+
+#contant for waypointV2ActuatorTriggerType
+# The action will be executed by the camera.
+# The parameters should be defined by ``DJIWaypointV2Action_DJIWaypointV2CameraActuatorParam``.
+uint8 DJIWaypointV2ActionActuatorTypeCamera = 1
+
+# The action will be executed by the gimbal.
+# The parameters should be defined by ``DJIWaypointV2Action_DJIWaypointV2GimbalActuatorParam``.
+uint8 DJIWaypointV2ActionActuatorTypeGimbal = 2
+
+# The action will executes by control aircraft.
+# The parameters should be setting by ``DJIWaypointV2Action_DJIWaypointV2CameraActuatorParam``.
+uint8 DJIWaypointV2ActionActuatorTypeAircraftControl = 4
+
+# Unknown actuator type.
+uint8 DJIWaypointV2ActionActuatorTypeUnknown = 255
+
+uint16 actionId # The ID of Action.
+
+uint8 waypointV2ActionTriggerType
+uint8 waypointV2ACtionActuatorType
+
+# The trigger of action.You can only choose one to config
+WaypointV2AssociateTrigger waypointV2AssociateTrigger
+WaypointV2IntervalTrigger waypointV2IntervalTrigger
+WaypointV2TrajectoryTrigger waypointV2TrajectoryTrigger
+WaypointV2SampleReachPointTrigger waypointV2SampleReachPointTrigger
+
+ # The actuator of action.You can only choose one to config
+WaypointV2CameraActuator waypointV2CameraActuator
+WaypointV2GimbalActuator waypointV2GimbalActuator
+WaypointV2AircraftControlActuator waypointV2AircraftControlActuator
\ No newline at end of file

mission_controller/msg/WaypointV2AircraftControlActuator.msg

+# Parameters for aircraft control actuator. It is valid only when the
+# ``DJIWaypointV2Action_DJIWaypointV2Actuator_type`` is
+# ``DJIWaypointV2MissionV2_DJIWaypointV2ActionActuatorType_AircraftControl``.
+#contant for DJIWaypointV2ActionActuatorAircraftControlOperationType
+# Rotates the aircraft's yaw.
+uint8 DJIWaypointV2ActionActuatorAircraftControlOperationTypeRotateYaw = 1
+# Keeps the aircraft stop flying or start flying.
+uint8 DJIWaypointV2ActionActuatorAircraftControlOperationTypeFlyingControl = 2
+# Unknown
+uint8 DJIWaypointV2ActionActuatorAircraftControlOperationTypeUnknown = 255
+
+uint8 actuatorIndex   # The index of actuator. It is valid when the diagnostics is related
+                      # to camera or gimbal and the connected product has multiple gimbals and cameras.
+uint16 DJIWaypointV2ActionActuatorAircraftControlOperationType 
+WaypointV2AircraftControlActuatorFlying waypointV2AircraftControlActuatorFlying
+WaypointV2AircraftControlActuatorRotateHeading waypointV2AircraftControlActuatorRotateHeading
\ No newline at end of file

mission_controller/msg/WaypointV2AircraftControlActuatorFlying.msg

+# This class defines if the aircraft starts or stops the flight.s.
+uint8  isStartFlying  # Determines the aircraft start flying or stop flying.
+                        # ``TRUE`` for the aircraft to start flying.
\ No newline at end of file

mission_controller/msg/WaypointV2AircraftControlActuatorRotateHeading.msg

+# This class defines how the aircraft rotates on the yaw axis.
+  uint8 isRelative # Determines the aircraft rotate heading relative.
+                     # if ``TRUE``, when the aircraft is rotating, relative to the current angle.
+  float32 yaw # Determines the direction how aircraft changes its heading.
\ No newline at end of file

mission_controller/msg/WaypointV2AssociateTrigger.msg

+#contant for actionAssociatedType
+uint8 DJIWaypointV2TriggerAssociatedTimingTypeSimultaneously = 1  # The trigger starts simultaneously with the trigger that is associated.
+uint8 DJIWaypointV2TriggerAssociatedTimingTypeAfterFinised = 2    # The trigger starts after the trigger associated has finished.
+uint8 DJIWaypointV2TriggerAssociatedTimingTypeUnknown = 255       # Unkown timing type.
+
+uint8 actionAssociatedType  # The type of assciate trigger.
+uint8 waitingTime           # Waiting time in seconds after ActionTrigger starts.
+uint16 actionIdAssociated # Associated action ID.
\ No newline at end of file

mission_controller/msg/WaypointV2CameraActuator.msg

+# This determines how the camera will be performed when a waypoint mission is executing.
+#constant for DJIWaypointV2ActionActuatorCameraOperationType
+# Starts to shoot a photo.
+uint16 DJIWaypointV2ActionActuatorCameraOperationTypeTakePhoto = 1
+# Starts to record a video.
+uint16 DJIWaypointV2ActionActuatorCameraOperationTypeStartRecordVideo = 2
+# Stops to record a video.
+uint16 DJIWaypointV2ActionActuatorCameraOperationTypeStopRecordVideo = 3
+# Starts focus.
+uint16 DJIWaypointV2ActionActuatorCameraOperationTypeFocus = 4
+# Starts focal lenth. Only support those support flocal lenth cameras.
+uint16 DJIWaypointV2ActionActuatorCameraOperationTypeFocalLength = 5
+
+uint16 actuatorIndex   # The index of actuator. It is valid when the diagnostics is related
+                      # to camera or gimbal and the connected product has multiple gimbals and cameras.
+
+uint16 DJIWaypointV2ActionActuatorCameraOperationType
+# you can only choose one to config.
+WaypointV2CameraActuatorFocusParam focusParam  # The parameters for camera focus operation. It is valid only when
+                                       # ``DJIWaypointV2Action_DJIWaypointV2CameraActuatorParam_operationType`` is
+                                       # ``DJIWaypointV2MissionV2_DJIWaypointV2ActionActuatorCameraOperationType_Focus``
+WaypointV2CameraActuatorFocalLengthParam zoomParam # The parameters for camera focus length operation. It is valid only when
+                                           # ``DJIWaypointV2Action_DJIWaypointV2CameraActuatorParam_operationType`` is
+                                           # ``DJIWaypointV2MissionV2_DJIWaypointV2ActionActuatorCameraOperationType_FocalLength``
\ No newline at end of file

mission_controller/msg/WaypointV2CameraActuatorFocalLengthParam.msg

+# This class defines a camera focal length operation for  ``DJIWaypointV2Action_DJIWaypointV2CameraActuatorParam``.
+# Focal length of zoom lens. Valid range is [``DJICamera_DJICameraOpticalZoomSpec_minFocalLength``,
+# ``DJICamera_DJICameraOpticalZoomSpec_minFocalLength``] and must be a multiple of
+# ``DJICamera_DJICameraOpticalZoomSpec_focalLengthStep``.
+#  Only support by those camera ``DJICamera_CameraSettings_isOpticalZoomSupported`` return ``TRUE``.
+  uint16 focalLength
+
+  uint8 retryTimes = 1
\ No newline at end of file

mission_controller/msg/WaypointV2CameraActuatorFocusParam.msg

+# This class defines a camera focus operation for ``DJIWaypointV2Action_DJIWaypointV2CameraActuatorParam``.
+# The lens focus target point. When the focus mode is auto, the target point
+# is the focal point. When the focus mode is manual, the target point is the zoom
+# out area if the focus assistant is enabled for the manual mode.
+#  The range for x and y is from 0.0 to 1.0. The point [0.0, 0.0] represents the top-left angle of the screen.
+  
+  float32 x # x axis focus point value.range: [0,1]
+  float32 y # y axis focus point value.range: [0,1]
+  uint8 regionType #focus type:0:point focus,1:rectangle focus
+  float32 width #Normalized focus area width(0,1)
+  float32 height # Normalized focus area height(0,1)
+  uint8 retryTimes = 1
\ No newline at end of file

mission_controller/msg/WaypointV2Config.msg

+# Represents current waypoint's speed config.
+# 0: set local waypoint's cruise speed,
+# 1: unset global waypoint's cruise speed*/
+uint8  useLocalCruiseVel
+# 0: set local waypoint's max speed,
+# 1: unset global waypoint's max speed*/
+uint8  useLocalMaxVel
\ No newline at end of file

mission_controller/msg/WaypointV2GimbalActuator.msg

+# gimbal actuator will be performed when a waypoint mission is executed.
+#constant for DJIWaypointV2ActionActuatorGimbalOperationType
+# Rotates the gimbal. Only valid when the trigger type is
+# ``DJIWaypointV2MissionV2_DJIWaypointV2TriggerAssociatedTimingType_ReachPoint``.
+uint8 DJIWaypointV2ActionActuatorGimbalOperationTypeRotateGimbal = 1
+# Unknown
+uint8 DJIWaypointV2ActionActuatorGimbalOperationTypeUnknown = 255
+
+uint8 DJIWaypointV2ActionActuatorGimbalOperationType
+uint16 actuatorIndex   # The index of actuator. It is valid when the diagnostics is related
+                      # to camera or gimbal and the connected product has multiple gimbals and cameras.
+WaypointV2GimbalActuatorRotationParam waypointV2GimbalActuatorRotationParam # The operation type of gimbal actuator.
\ No newline at end of file

mission_controller/msg/WaypointV2GimbalActuatorRotationParam.msg

+  int16 x             # gimbal roll angle,  unit: 0.1 deg,range:[-3600, 3600]*/
+  int16 y             # gimbal pitch angle, unit: 0.1 deg,range:[-3600, 3600]*/
+  int16 z             # gimbal yaw angle,   unit: 0.1 deg,range:[-3600, 3600]*/
+  uint8 ctrl_mode     # 0: absolute position control, 1:relative position control*/
+  uint8 rollCmdIgnore # 0: roll command normal,  1: roll command ignore*/
+  uint8 pitchCmdIgnore# 0: pitch command normal,  1: pitch command ignore*/
+  uint8 yawCmdIgnore  # 0: yaw command normal,  1: yaw command ignore*/
+  uint8 absYawModeRef # 0: absoluate rotate yaw relative to aircraft,
+                      #  1: absoluate rotate yaw relative to North*/
+  uint8 duationTime    # 0: rotate time,unit:0.1s, range[1,255]*/
\ No newline at end of file

mission_controller/msg/WaypointV2InitSetting.msg

+# @brief Waypoint V2 Mission Initialization settings
+# @details This is one of the few structs in the OSDK codebase that
+# is used in both a sending and a receiving API.
+
+#constant for finishedAction
+uint8 DJIWaypointV2MissionFinishedNoAction = 0  # No further action will be taken.
+                                                # The aircraft can be controlled by the remote controller.
+uint8 DJIWaypointV2MissionFinishedGoHome = 1    # Goes home when the mission is finished. The aircraft will
+                                                # land directly if it is within 20 meters away from the home point.
+uint8 DJIWaypointV2MissionFinishedAutoLanding = 2  # The aircraft will land automatically at the last waypoint.
+uint8 DJIWaypointV2MissionFinishedGoToFirstWaypoint = 3  # The aircraft will go back to the first waypoint and hover.
+uint8 DJIWaypointV2MissionFinishedContinueUntilStop = 4  #  When the aircraft reaches its final waypoint, it will hover without ending the
+                                                         # mission. The joystick  can still be used to pull the aircraft back along its
+                                                         # previous waypoints. The only way this mission  can end is if stopMission is
+                                                         # called
+
+#constant for gotoFirstWaypintMode
+uint8 DJIWaypointV2MissionGotoFirstWaypointModeSafely = 0 # Go to the waypoint safely. The aircraft will rise to the same altitude of the
+                                                          # waypoint if the current  altitude is lower then the waypoint altitude. It then
+                                                          # goes to the waypoint coordinate from the current  altitude, and proceeds to the
+                                                          # altitude of the waypoint.
+uint8 DJIWaypointV2MissionGotoFirstWaypointModePointToPoint = 1 # Go to the waypoint from the current aircraft point to the waypoint directly.
+
+
+uint32 missionID    # The Mission ID. Use to distinguish　different mission
+uint16 missTotalLen # The Mission waypoint total length, could not exceed 65535
+uint8 repeatTimes   # Mission execution can be repeated more than once. A value of 0 means the mission
+                    # only executes once, and does not repeat. A value of 1 means the mission will
+                    # execute a total of two times.
+uint8 finishedAction # Action the aircraft will take when the waypoint mission is complete.
+
+# While the aircraft is travelling between waypoints, you can offset its speed by
+# using the throttle joystick  on the remote controller. `maxFlightSpeed` is this
+# offset when the joystick is pushed  to maximum deflection. For example, If
+# maxFlightSpeed is 10 m/s, then pushing the throttle joystick all the  way up
+# will add 10 m/s to the aircraft speed, while pushing down will subtract 10 m/s
+# from the aircraft  speed. If the remote controller stick is not at maximum
+# deflection, then the offset speed will be  interpolated between [0,
+# `maxFlightSpeed`] with a resolution of 1000 steps. If the  offset speed is
+# negative, then the aircraft will fly backwards to previous waypoints. When it
+# reaches the  first waypoint, it will then hover in place until a positive speed
+# is  applied. `maxFlightSpeed` has a range of [2,15] m/s.
+# unit: m/s
+float32 maxFlightSpeed
+
+# The base automatic speed of the aircraft as it moves between waypoints with
+# range [-15, 15] m/s. The  aircraft's actual speed is a combination of the base
+# automatic speed, and the speed control given by  the throttle joystick on the
+# remote controller. If `autoFlightSpeed` >0: Actual  speed is `autoFlightSpeed` +
+# Joystick Speed (with combined max  of `maxFlightSpeed`) If `autoFlightSpeed` =0:
+# Actual speed is  controlled only by the remote controller joystick. If
+# `autoFlightSpeed` <0 and the  aircraft is at the first waypoint, the aircraft
+# will hover in place until the speed is made positive by  the remote controller
+# joystick.
+# unit: m/s
+float32 autoFlightSpeed
+
+uint8 exitMissionOnRCSignalLost    # Determines whether the mission should stop when connection between the  aircraft
+                                   # and remote controller is lost. Default is `NO`.
+
+uint8 gotoFirstWaypointMode        # Defines how the aircraft will go to the first waypoint from its current
+                                   # position. Default  is ``DJIWaypointV2MissionV2_DJIWaypointV2MissionGotoWaypointMode_Safely``.
+
+WaypointV2[] mission
\ No newline at end of file

mission_controller/msg/WaypointV2IntervalTrigger.msg

+#constant for actionIntervalType
+uint8 DJIWaypointV2ActionIntervalTypeTime     = 1  # The action will be repeated after a particular period of time.
+uint8 DJIWaypointV2ActionIntervalTypeDistance = 2  # The action will be repeated after a particular distance.
+uint8 DJIWaypointV2ActionIntervalTypeUnknown  = 255 # Unknown action trigger type.
+
+uint16 startIndex # It determines the index of the waypoint at which the trigger starts.
+# If the  ``DJIWaypointV2Action_DJIWaypointV2IntervalTriggerParam_actionIntervalType``
+# is ``DJIWaypointV2MissionV2_DJIWaypointV2TriggerAssociatedTimingType_Time``
+# The time interval in seconds when two action are executed as the aircraft moves
+# from the current waypoint to the next waypoint.
+# If the ``DJIWaypointV2Action_DJIWaypointV2IntervalTriggerParam_actionIntervalType`` is
+# ``DJIWaypointV2MissionV2_DJIWaypointV2TriggerAssociatedTimingType_Distance``
+# The distance interval in meters when two action are executed as the aircraft moves
+# from the current waypoint to the next waypoint.
+uint16 interval
+uint8 actionIntervalType # The type of interval trigger.
+                         # See ``DJIWaypointV2MissionV2_DJIWaypointV2ActionIntervalType``.
\ No newline at end of file

mission_controller/msg/WaypointV2MissionEventPush.msg

+uint8 event
+uint32 FCTimestamp
+#ID:0x01
+uint8 interruptReason #0x00:rc triggered interrupt
+
+#ID:0x02
+uint8 recoverProcess #0x00:finished pause recover
+
+#ID:0x03
+uint8 finishReason #0x00:finished normally; 0x10:External user trigger ended successfully
+
+#ID:0x10
+uint16 waypointIndex
+
+#ID:0x11
+uint8 currentMissionExecNum
+uint8 finishedAllExecNum  #0:not finished; 1:finished all exec num
\ No newline at end of file

mission_controller/msg/WaypointV2MissionStatePush.msg

+int8   commonDataVersion
+uint16 commonDataLen
+uint16 curWaypointIndex
+uint8  state  #0x0:ground station not start. 0x1:mission prepared. 0x2:enter mission.
+               #0x3:execute flying route mission.
+               #0x4:pause state. 0x5:enter mission after ending pause.
+               #0x6:exit mission.
+uint16 velocity #uint:0.01m/s
\ No newline at end of file

mission_controller/msg/WaypointV2ReachpointTrigger.msg

+#  It describes an action will be triggered when the aircraft reach the certain waypoint.
+  uint16 startIndex   # It determines the index of the waypoint at which the action will be triggered.
+  uint16 endIndex
+  uint16 intervalWPNum
+  uint16 waypointCountToTerminate  # It determines the waypoint count till the action triggered stops.
\ No newline at end of file

mission_controller/msg/WaypointV2SampleReachPointTrigger.msg

+uint16 waypointIndex # It determines the index of the waypoint at which the action will be triggered.
+uint16 terminateNum

mission_controller/msg/WaypointV2TrajectoryTrigger.msg

+# This class represents a trajectory trigger action when should be trigger.
+
+uint16 startIndex # It determines the index of the waypoint at which the trigger starts.
+uint16 endIndex   # It determines the waypoint when the trigger stops.
+

misson_controller/package.xml → mission_controller/package.xml

 <?xml version="1.0"?>
 <package format="2">
-  <name>misson_controller</name>
+  <name>mission_controller</name>
   <version>0.0.0</version>
-  <description>The misson_controller package</description>
+  <description>The mission_controller package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
@@ -19,7 +19,7 @@
   <!-- Url tags are optional, but multiple are allowed, one per tag -->
   <!-- Optional attribute type can be: website, bugtracker, or repository -->
   <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/misson_controller</url> -->
+  <!-- <url type="website">http://wiki.ros.org/mission_controller</url> -->
 
 
   <!-- Author tags are optional, multiple are allowed, one per tag -->
@@ -54,16 +54,22 @@
   <build_depend>rospy</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>message_filters</build_depend>
   <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>message_generation</build_export_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>sensor_msgs</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>message_generation</exec_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_filters</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

mission_controller/src/mission_controller_node.cpp

+// mission controller node all-in-one for DJI M210 (v1 or v2)
+
+#include <mission_controller_node.h>
+
+// #include <dji_osdk_ros/vehicle_wrapper.h>
+// #include <dji_osdk_ros/dji_vehicle_node.h>
+
+// global variables
+ros::ServiceClient     waypoint_upload_client;
+ros::ServiceClient     waypoint_action_client;
+ros::ServiceClient     flight_control_client;
+ros::ServiceClient     mission_waypoint_client;
+ros::ServiceClient     obtain_ctrl_authority_client;
+// need to take another look at synchronization...
+// ros::Publisher         syncPub; // thing to publish to sync topic "positionSync"
+
+sensor_msgs::NavSatFix gps_pos; // gps position from drone
+geometry_msgs::Transform ex_pos; // position from external source (qualisys or some such)
+geometry_msgs::Transform track_pos; // target position
+geometry_msgs::Transform previous_ex_pos;
+geometry_msgs::Transform previous_track_pos;
+
+geometry_msgs::Transform pd_pos; // desired pose from pd relative to qual center (only x on rotation)
+
+std::vector<float> expectedVel; // expected position after applying relative position move
+float feedbackYaw; // yaw as measured by pos ref
+float feedbackPitch; // pitch as measured by pos ref
+float feedbackRoll; // roll as measured by pos ref
+float trackingYaw; // yaw to track
+float trackingPitch; // pitch to track
+float trackingRoll; // roll to track
+int calibrationWindow = 3; // the last _ position calls that are included in coordinate system calibration
+std::vector<float> calibrationWindowValues;
+float angleOffset;
+float controlLoopTime;
+float updateRateParam;
+std::vector<std::vector<float>> poseRotationMatrix; // need to run calibration to get
+std::vector<std::vector<std::vector<float>>> poseRotationMatrixValues; // need to run calibration to get
+float poseMagnitudeScaler; // need to run calibration to get
+float poseRotationRadians;
+bool rotationMatrixCalibratedFlag = false;
+
+bool                   gps_home_flag = false;
+bool                   pos_home_flag = false;
+bool                   localMissionFlag = false; // whether running position mission currently
+bool                   gpsMissionFlag = false; // whether running gps mission currently
+std::string                 missionTypeParam;
+ros::Subscriber        gps_pos_subscriber;
+ros::Subscriber        pos_feedback_subscriber;
+ros::Subscriber        pos_tracking_subscriber;
+ros::Subscriber        pd_position_subscriber;
+ros::Subscriber        pd_heading_subscriber;
+
+// temp subscribers to pick up puredata parameter information, will be removed once the right parameters are settled on
+ros::Subscriber        pd_parameter_MinSpeed_subscriber;
+ros::Subscriber        pd_parameter_MaxSpeed_subscriber;
+ros::Subscriber        pd_parameter_MaxYawSpeed_subscriber;
+ros::Subscriber        pd_parameter_SpeedScale_subscriber;
+ros::Subscriber        pd_parameter_UpdateRate_subscriber;
+ros::Subscriber        pd_parameter_Damping_subscriber;
+ros::Subscriber        pd_parameter_Frequency_subscriber;
+ros::Subscriber        pd_parameter_AngleOffset_subscriber;
+
+int                    droneId; // stupid but will work for now
+
+int                    waypointMissionUpdateCounter = 0; // counts iterations before we check waypoints again
+int                    waypointMissionUpdateLimit = 10; // number of counts before we check ...
+int                    positionMissionUpdateCounter = 0; // counts iterations before we check positions again
+int                    positionMissionUpdateLimit = 10; // ...
+
+                                    // std::vector<WayPointSettings> generatedWaypts; // waypoints to be generated, used to check if we're at waypoint or not
+int                    waypointNumber;
+int                    currentWaypoint = 0; // how many waypoints have we reached
+long double            initialWaypointDistance; // distance to first waypoint
+long double            initialPositionDistance; // distance to first position
+double                 waypointSpeedParam;
+double                 positionThresholdParam;
+double                 angleOffsetParam;
+double		             maxSpeedParam;
+double                 speedScalarParam;
+double                 maxYawSpeedParam;
+
+// inverse controller variables
+double                 dt;
+double                 damping;
+double                 frequency;
+double                 kp;
+double                 kd;
+double                 ksg;
+double                 kdg;
+double                 g;
+
+
+int main(int argc, char** argv)
+{
+
+  ros::init(argc, argv, "mission_controller");
+  ros::NodeHandle nh;
+  //
+  // ros::param::get("base_speed", waypointSpeedParam);
+  // ros::param::get("max_speed", maxSpeedParam);
+  // ros::param::get("max_yaw_speed", maxYawSpeedParam);
+  // ros::param::get("angle_offset", angleOffsetParam);
+  // ros::param::get("speed_scalar", speedScalarParam);
+  // ros::param::get("drone_id", droneId);
+  // ros::param::get("mission_type", missionTypeParam);
+  // ros::param::get("position_threshold", positionThresholdParam);
+  // ros::param::get("loop_rate", updateRateParam);
+  // controlLoopTime = 1.0/updateRateParam;
+  // // additions
+  // ros::param::get("controller_damping", damping);
+  // ros::param::get("controller_frequency", frequency);
+  // kp = (6.0*frequency)*(6.0*frequency)* 0.25;
+  // kd = 4.5*frequency*damping;
+  // expectedVel = std::vector<float> {0.0, 0.0, 0.0};
+  // angleOffset = 0.0;
+  // calibrationWindowValues = std::vector<float> {};
+  // for (int i = 0; i < calibrationWindow; i++)
+  // {
+  //   calibrationWindowValues.push_back(0.0);
+  // }
+  //
+  // // set puredata position to zero zero so it doesn't immediately fly away
+  // pd_pos.translation.x = 0.0;
+  // pd_pos.translation.y = 0.0;
+  // pd_pos.translation.z = 1.5;
+  // pd_pos.rotation.x = 0.0;
+  //
+  // // ROS stuff
+  // // obtain control authority?
+  // obtain_ctrl_authority_client = nh.serviceClient<dji_osdk_ros::ObtainControlAuthority>(
+  //   "obtain_release_control_authority");
+  // waypoint_upload_client = nh.serviceClient<dji_osdk_ros::MissionWpUpload>(
+  //   "dji_osdk_ros/mission_waypoint_upload");
+  // waypoint_action_client = nh.serviceClient<dji_osdk_ros::MissionWpAction>(
+  //   "dji_osdk_ros/mission_waypoint_action");
+  // flight_control_client = nh.serviceClient<dji_osdk_ros::FlightTaskControl>(
+  //   "flight_task_control");
+  //
+  // // syncPub = nh.advertise<std_msgs::Float32>("sync_signal", 1);
+  // gps_pos_subscriber = nh.subscribe<sensor_msgs::NavSatFix>("dji_osdk_ros/gps_position", 1, &gpsPosCallback);
+  // pos_feedback_subscriber = nh.subscribe<geometry_msgs::Transform>("feedback", 1, &posFeedbackCallback);
+  // pos_tracking_subscriber = nh.subscribe<geometry_msgs::Transform>("tracking", 1, &posTrackingCallback);
+  // pd_position_subscriber = nh.subscribe<geometry_msgs::Transform>("pd/target", 1, &pdPositionCallback);
+  //
+  // // pd_parameter_MinSpeed_subscriber = nh.subscribe<std_msgs::Float32>("pd/parameters/minSpeed", 1, &pdMinSpeedCallback);
+  // pd_parameter_MaxSpeed_subscriber = nh.subscribe<std_msgs::Float32>("pd/parameters/maxSpeed", 1, &pdMaxSpeedCallback);
+  // pd_parameter_MaxYawSpeed_subscriber = nh.subscribe<std_msgs::Float32>("pd/parameters/maxYawSpeed", 1, &pdMaxYawSpeedCallback);
+  // pd_parameter_SpeedScale_subscriber = nh.subscribe<std_msgs::Float32>("pd/parameters/speedScale", 1, &pdSpeedScaleCallback);
+  // pd_parameter_UpdateRate_subscriber = nh.subscribe<std_msgs::Float32>("pd/parameters/updateRate", 1, &pdUpdateRateCallback);
+  // pd_parameter_AngleOffset_subscriber = nh.subscribe<std_msgs::Float32>("pd/parameters/angleOffset", 1, &pdAngleOffsetCallback);
+  // pd_parameter_Damping_subscriber = nh.subscribe<std_msgs::Float32>("pd/parameters/controllerDamping", 1, &pdDampingCallback);
+  // pd_parameter_Frequency_subscriber = nh.subscribe<std_msgs::Float32>("pd/parameters/controllerFrequency", 1, &pdFrequencyCallback);
+  //
+  // ros::ServiceServer overwatchService = nh.advertiseService("overwatch", overwatchFunction);
+  //
+  // // wait for services to be up
+  // obtain_ctrl_authority_client.waitForExistence();
+  // waypoint_upload_client.waitForExistence();
+  // waypoint_action_client.waitForExistence();
+  // flight_control_client.waitForExistence();
+  // // wait for waypoints to be up as well
+  ROS_INFO("mission controller ready to go");
+
+  ros::spin();
+
+  return 0;
+}

mission_controller/srv/Activation.srv

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

mission_controller/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

mission_controller/srv/CameraAperture.srv

+#request
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+uint8 exposure_mode #see enum class ExposureMode in common_type.h
+uint16 aperture      #see enum class Aperture in common_type.h
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/CameraEV.srv

+#request
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+uint8 exposure_mode #see enum class ExposureMode in common_type.h
+uint8 exposure_compensation #see enum class ExposureCompensation in common_type.h
+---
+#reponse
+bool result
+

mission_controller/srv/CameraFocusPoint.srv

+#request
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+float32 x #range from 0~1, top left corner is origin point
+float32 y #range from 0~1, top left corner is origin point
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/CameraISO.srv

+#request
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+uint8 exposure_mode #see enum class ExposureMode in common_type.h
+uint8 iso_data #see enum class ISO in common_type.h
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/CameraRecordVideoAction.srv

+#request
+uint8 start_stop
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/CameraSetZoomPara.srv

+#request
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+float32 factor
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/CameraShutterSpeed.srv

+#request
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+uint8 exposure_mode #see enum class ExposureMode in common_type.h
+uint8 shutter_speed #see enum class ShutterSpeed in common_type.h
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/CameraStartShootAEBPhoto.srv

+#request
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+uint8 photo_aeb_count #see enum class PhotoAEBCount in common_type.h
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/CameraStartShootBurstPhoto.srv

+#request
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+uint8 photo_burst_count #see enum class PhotoBurstCount in common_type.h
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/CameraStartShootIntervalPhoto.srv

+#request
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+uint8 photo_num_conticap # 0:reserve 1~254:number 255:keep capturing till stop
+int16 time_interval #/ time interval (second)
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/CameraStartShootSinglePhoto.srv

+#request
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/CameraStopShootPhoto.srv

+#request
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/CameraTapZoomPoint.srv

+#request
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+uint8 multiplier #Data struct of tap zoom multiplier used by user. Limit: 1~5
+float32 x  # range from 0~1, top left corner is origin point
+float32 y  # range from 0~1, top left corner is origin point
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/CameraZoomCtrl.srv

+#request
+uint8 start_stop
+uint8 payload_index #see enum class PayloadIndex in common_type.h
+uint8 direction #see enum class ZoomDirection in common_type.h
+uint8 speed #see enum class ZoomSpeed in common_type.h
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/DownloadWaypointV2Mission.srv

+#request
+---
+#response
+WaypointV2[] mission
+bool result
\ No newline at end of file

mission_controller/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

mission_controller/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

mission_controller/srv/EmergencyBrake.srv

+#request
+---
+#reponse
+bool result
\ No newline at end of file

mission_controller/srv/FlightTaskControl.srv

+#constant for tasks
+uint8 TASK_GOHOME = 1
+uint8 TASK_POSITION_AND_YAW_CONTROL   = 2
+uint8 TASK_GOHOME_AND_CONFIRM_LANDING = 3
+uint8 TASK_TAKEOFF = 4
+uint8 TASK_VELOCITY_AND_YAWRATE_CONTROL = 5
+uint8 TASK_LAND          = 6
+uint8 START_MOTOR        = 7
+uint8 STOP_MOTOR         = 8
+uint8 TASK_EXIT_GO_HOME  = 12
+uint8 TASK_EXIT_LANDING  = 14
+uint8 TASK_FORCE_LANDING_AVOID_GROUND = 30 #/*!< confirm landing */
+uint8 TASK_FORCE_LANDING              = 31 #/*!< force landing */
+
+#request
+uint8 task    # see constants above for possible tasks
+JoystickParams joystickCommand  #Provide Position and Velocity control
+uint32 velocityControlTimeMs    #Velocity control time
+float32 posThresholdInM  #(Meter)
+float32 yawThresholdInDeg  #(Degree)
+---
+#response
+bool result
+

mission_controller/srv/GenerateWaypointV2Action.srv

+#request
+WaypointV2Action[] actions
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/GetAvoidEnable.srv

+#request
+---
+#response
+bool result
+uint8 horizon_avoid_enable_status #0:disable 1:enable 0xF:invalid
+uint8 upwards_avoid_enable_status
\ No newline at end of file

mission_controller/srv/GetDroneType.srv

+#request
+---
+#response
+uint8 drone_type
\ No newline at end of file

mission_controller/srv/GetGlobalCruisespeed.srv

+#request
+---
+#response
+# Global cruise speed of mission
+# unit: m/s
+# range:[0,WayPointV2InitSettings::maxFlightSpeed]
+float32 global_cruisespeed
+bool result
\ No newline at end of file

mission_controller/srv/GetGoHomeAltitude.srv

+#request
+---
+#response
+bool result
+uint16 altitude
\ No newline at end of file

mission_controller/srv/GetHMSData.srv

+#request
+bool enable
+---
+#reponse
+bool result
+uint8 deviceIndex         # When the error code is related to camera or gimbal device,
+                          # it will tell you which device it is.
+HMSPushInfo[] errList     # error code list in each pushing
+uint32 timeStamp
\ No newline at end of file

mission_controller/srv/GetM300StereoParams.srv

+#request
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/GetSingleBatteryDynamicInfo.srv

+#request
+uint8 first_smart_battery = 1
+uint8 second_smart_battery = 2
+
+uint8 batteryIndex
+---
+#response
+SmartBatteryDynamicInfo smartBatteryDynamicInfo
\ No newline at end of file

mission_controller/srv/GetWholeBatteryInfo.srv

+#request
+---
+#response
+BatteryWholeInfo battery_whole_info
\ No newline at end of file

mission_controller/srv/GimbalAction.srv

+Header header
+# ts is the time it takes to achieve the desired angle,
+# so the shorter the time, the faster the gimbal rotates.
+
+#request
+bool is_reset
+
+uint8 payload_index
+
+# rotation cooradiration
+# 0 = execute angle command based on the previously set reference point
+# 1 = execute angle command based on the current point
+uint8 rotationMode
+
+# pitch angle in degree, unit : deg
+float32 pitch
+
+# roll angle in degree, unit : deg
+float32 roll
+
+# yaw angle in degree, unit : deg
+float32 yaw
+
+#execution time, unit : second
+float64 time
+
+---
+#response
+bool result

mission_controller/srv/InitWaypointV2Setting.srv

+#request
+WaypointV2InitSetting waypointV2InitSettings
+uint16 polygonNum
+float32 radius
+uint16 actionNum
+
+---
+bool result
\ No newline at end of file

mission_controller/srv/JoystickAction.srv

+#request
+JoystickParams joystickCommand
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/KillSwitch.srv

+#request
+bool enable
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/MFIO.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
+
+uint8 CHANNEL_0 = 0
+uint8 CHANNEL_1 = 1
+uint8 CHANNEL_2 = 2
+uint8 CHANNEL_3 = 3
+uint8 CHANNEL_4 = 4
+uint8 CHANNEL_5 = 5
+uint8 CHANNEL_6 = 6
+uint8 CHANNEL_7 = 7
+
+uint8 TURN_ON   = 0     # Control PWM on/off
+uint8 TURN_OFF  = 1
+
+#request
+uint8 action            # Turn On/Off, 0: ON    1: OFF
+uint8 mode              # see constants above for possible modes
+bool block              # Block/Non-block Ouput
+uint8 channel           # 0-7
+uint32 init_on_time_us  # on time for pwm duty cycle in micro-seconds, 0-20000(0%-100%)
+uint8 gpio_value        # 0: Low, 1:High just for GPIO_OUT
+uint16 pwm_freq         # set pwm frequency in Hz
+---
+uint32 read_value

mission_controller/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
+---

mission_controller/srv/MFIOSetValue.srv

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

mission_controller/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

mission_controller/srv/MissionHpGetInfo.srv

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

mission_controller/srv/MissionHpResetYaw.srv

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

mission_controller/srv/MissionHpUpdateRadius.srv

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

mission_controller/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

mission_controller/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

mission_controller/srv/MissionStatus.srv

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

mission_controller/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

mission_controller/srv/MissionWpGetInfo.srv

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

mission_controller/srv/MissionWpGetSpeed.srv

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

mission_controller/srv/MissionWpSetSpeed.srv

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

mission_controller/srv/MissionWpUpload.srv

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

mission_controller/srv/ObtainControlAuthority.srv

+#request
+bool enable_obtain
+---
+#reponse
+bool result
\ No newline at end of file

mission_controller/srv/Overwatch.srv

+# constants for different actions
+uint8 START_WAYPOINT_PATH = 1
+uint8 START_POSITION_PATH = 2
+uint8 STOP_MOTION = 3
+uint8 LAND = 4
+uint8 GO_HOME = 5
+uint8 TAKEOFF = 6
+uint8 CALIBRATE_POS_REF = 7
+
+# request
+bool start
+uint8 action
+---
+# response
+bool result

mission_controller/srv/PauseWaypointV2Mission.srv

+#request
+---
+#response
+bool result
\ No newline at end of file

mission_controller/srv/QueryDroneVersion.srv

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

mission_controller/srv/ResumeWaypointV2Mission.srv

+#request
+---
+#response
+bool result
\ No newline at end of file

mission_controller/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

mission_controller/srv/SendMobileData.srv

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

mission_controller/srv/SendPayloadData.srv

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