Commit 7ca7c99e authored by Larkin Heintzman's avatar Larkin Heintzman

init mission controller stuff

parent c2e326c8
...@@ -36,7 +36,7 @@ ...@@ -36,7 +36,7 @@
<param name="trackName" type="string" value="M100_CarbonFiber"/> <param name="trackName" type="string" value="M100_CarbonFiber"/>
</node> --> </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="anchorPoint"> [37.196941, -80.578335] </rosparam>
<rosparam command="load" param="pathPoints"> <rosparam command="load" param="pathPoints">
[ [
...@@ -46,9 +46,8 @@ ...@@ -46,9 +46,8 @@
[37.198161, -80.577072], [37.198161, -80.577072],
[37.197879, -80.576637] [37.197879, -80.576637]
] ]
</rosparam>
<rosparam command="load" param="altitude"> 10.0 </rosparam> <rosparam command="load" param="altitude"> 10.0 </rosparam>
<rosparam command="load" param="droneNum"> 3 </rosparam> <rosparam command="load" param="droneNum"> 3 </rosparam>
</node> </node> -->
<!-- <node pkg="dji_osdk_ros" type="image_saver.py" name="image_saver" output="screen"/> -->
</launch> </launch>
cmake_minimum_required(VERSION 3.0.2) cmake_minimum_required(VERSION 3.0.2)
project(misson_controller) project(mission_controller)
## Compile as C++11, supported in ROS Kinetic and newer ## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11) # add_compile_options(-std=c++11)
...@@ -13,6 +13,8 @@ find_package(catkin REQUIRED COMPONENTS ...@@ -13,6 +13,8 @@ find_package(catkin REQUIRED COMPONENTS
rospy rospy
sensor_msgs sensor_msgs
std_msgs std_msgs
message_generation
dji_osdk_ros
) )
## System dependencies are found with CMake's conventions ## System dependencies are found with CMake's conventions
...@@ -48,19 +50,133 @@ find_package(catkin REQUIRED COMPONENTS ...@@ -48,19 +50,133 @@ find_package(catkin REQUIRED COMPONENTS
## * uncomment the generate_messages entry below ## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder ## Generate msg in the 'msg' folder
# add_message_files( add_message_files(
# FILES FILES
# Message1.msg
# Message2.msg 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 ## Generate services in the 'srv' folder
# add_service_files( add_service_files(
# FILES FILES
# Service1.srv
# Service2.srv 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 ## Generate actions in the 'action' folder
# add_action_files( # add_action_files(
...@@ -70,10 +186,12 @@ find_package(catkin REQUIRED COMPONENTS ...@@ -70,10 +186,12 @@ find_package(catkin REQUIRED COMPONENTS
# ) # )
## Generate added messages and services with any dependencies listed here ## Generate added messages and services with any dependencies listed here
# generate_messages( generate_messages(
# DEPENDENCIES DEPENDENCIES
# geometry_msgs# sensor_msgs# std_msgs geometry_msgs
# ) sensor_msgs
std_msgs
)
################################################ ################################################
## Declare ROS dynamic reconfigure parameters ## ## Declare ROS dynamic reconfigure parameters ##
...@@ -105,10 +223,10 @@ find_package(catkin REQUIRED COMPONENTS ...@@ -105,10 +223,10 @@ find_package(catkin REQUIRED COMPONENTS
## CATKIN_DEPENDS: catkin_packages dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package( catkin_package(
# INCLUDE_DIRS include INCLUDE_DIRS include
# LIBRARIES misson_controller LIBRARIES mission_controller
# CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs message_runtime
# DEPENDS system_lib DEPENDS system_lib
) )
########### ###########
...@@ -117,14 +235,11 @@ catkin_package( ...@@ -117,14 +235,11 @@ catkin_package(
## Specify additional locations of header files ## Specify additional locations of header files
## Your package locations should be listed before other locations ## Your package locations should be listed before other locations
include_directories( include_directories(include ${catkin_INCLUDE_DIRS})
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library ## Declare a C++ library
# add_library(${PROJECT_NAME} # add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/misson_controller.cpp # src/${PROJECT_NAME}/mission_controller.cpp
# ) # )
## Add cmake target dependencies of the library ## Add cmake target dependencies of the library
...@@ -135,7 +250,7 @@ include_directories( ...@@ -135,7 +250,7 @@ include_directories(
## Declare a C++ executable ## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context ## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide ## 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 ## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the ## The above recommended prefix causes long target names, the following renames the
...@@ -145,7 +260,7 @@ include_directories( ...@@ -145,7 +260,7 @@ include_directories(
## Add cmake target dependencies of the executable ## Add cmake target dependencies of the executable
## same as for the library above ## 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 ## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node # target_link_libraries(${PROJECT_NAME}_node
...@@ -181,11 +296,11 @@ include_directories( ...@@ -181,11 +296,11 @@ include_directories(
# ) # )
## Mark cpp header files for installation ## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/ install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h" FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE PATTERN ".svn" EXCLUDE
# ) )
## Mark other files for installation (e.g. launch and bag files, etc.) ## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES # install(FILES
...@@ -199,7 +314,7 @@ include_directories( ...@@ -199,7 +314,7 @@ include_directories(
############# #############
## Add gtest based cpp test target and link libraries ## 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) # if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif() # endif()
......
#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);
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
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
# 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
# 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
# the time stamp of getting this data in the ROS system
time stamp
# the time stamp from GPS/RTK
string UTCTimeData
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
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
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
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
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
# 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
# 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
uint8[] data
\ No newline at end of file
uint8[] data
\ No newline at end of file
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
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
Header header
float32 x
float32 y
float32 z
uint8 xHealth
uint8 yHealth
uint8 zHealth
\ No newline at end of file
float64 latitude # in degree
float64 longitude # in degree
float32 altitude
int16 heading #heading is in [-180,180]
uint16 staytime # in second
# 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
# 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
# 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
# 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
# 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
#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
# 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
# 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
# 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
# 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
# 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
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
# @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
#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
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
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
# 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
uint16 waypointIndex # It determines the index of the waypoint at which the action will be triggered.
uint16 terminateNum
# 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.
<?xml version="1.0"?> <?xml version="1.0"?>
<package format="2"> <package format="2">
<name>misson_controller</name> <name>mission_controller</name>
<version>0.0.0</version> <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 --> <!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: --> <!-- Example: -->
...@@ -19,7 +19,7 @@ ...@@ -19,7 +19,7 @@
<!-- Url tags are optional, but multiple are allowed, one per tag --> <!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository --> <!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: --> <!-- 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 --> <!-- Author tags are optional, multiple are allowed, one per tag -->
...@@ -54,16 +54,22 @@ ...@@ -54,16 +54,22 @@
<build_depend>rospy</build_depend> <build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend> <build_depend>sensor_msgs</build_depend>
<build_depend>std_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>geometry_msgs</build_export_depend>
<build_export_depend>message_generation</build_export_depend>
<build_export_depend>roscpp</build_export_depend> <build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend> <build_export_depend>rospy</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend> <build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend> <build_export_depend>std_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_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>roscpp</exec_depend>
<exec_depend>rospy</exec_depend> <exec_depend>rospy</exec_depend>
<exec_depend>sensor_msgs</exec_depend> <exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend> <exec_depend>std_msgs</exec_depend>
<exec_depend>message_filters</exec_depend>
<!-- The export tag contains other, unspecified, tags --> <!-- The export tag contains other, unspecified, tags -->
......
// 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;
}
---
bool result
# for debugging usage
uint8 cmd_set
uint8 cmd_id
uint32 ack_data
\ No newline at end of file
#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
#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
#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
#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
#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
#request
uint8 start_stop
uint8 payload_index #see enum class PayloadIndex in common_type.h
---
#response
bool result
\ No newline at end of file
#request
uint8 payload_index #see enum class PayloadIndex in common_type.h
float32 factor
---
#response
bool result
\ No newline at end of file
#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
#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
#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
#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
#request
uint8 payload_index #see enum class PayloadIndex in common_type.h
---
#response
bool result
\ No newline at end of file
#request
uint8 payload_index #see enum class PayloadIndex in common_type.h
---
#response
bool result
\ No newline at end of file
#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
#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
#request
---
#response
WaypointV2[] mission
bool result
\ No newline at end of file
#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
#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
#request
---
#reponse
bool result
\ No newline at end of file
#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
#request
WaypointV2Action[] actions
---
#response
bool result
\ No newline at end of file
#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
#request
---
#response
uint8 drone_type
\ No newline at end of file
#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
#request
---
#response
bool result
uint16 altitude
\ No newline at end of file
#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
#request
---
#response
bool result
\ No newline at end of file
#request
uint8 first_smart_battery = 1
uint8 second_smart_battery = 2
uint8 batteryIndex
---
#response
SmartBatteryDynamicInfo smartBatteryDynamicInfo
\ No newline at end of file
#request
---
#response
BatteryWholeInfo battery_whole_info
\ No newline at end of file
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
#request
WaypointV2InitSetting waypointV2InitSettings
uint16 polygonNum
float32 radius
uint16 actionNum
---
bool result
\ No newline at end of file
#request
JoystickParams joystickCommand
---
#response
bool result
\ No newline at end of file
#request
bool enable
---
#response
bool result
\ No newline at end of file
#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
#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
---
uint8 channel # 0-7
uint32 init_on_time_us # on time for pwm duty cycle in micro-seconds
---
#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
---
MissionHotpointTask hotpoint_task
# for debugging usage
uint8 cmd_set
uint8 cmd_id
uint32 ack_data
\ No newline at end of file
---
bool result
# for debugging usage
uint8 cmd_set
uint8 cmd_id
uint32 ack_data
\ No newline at end of file
float32 radius # in meters
---
bool result
# for debugging usage
uint8 cmd_set
uint8 cmd_id
uint32 ack_data
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
MissionHotpointTask hotpoint_task
---
bool result
# for debugging usage
uint8 cmd_set
uint8 cmd_id
uint32 ack_data
\ No newline at end of file
---
uint8 waypoint_mission_count
uint8 hotpoint_mission_count
\ No newline at end of file
#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
---
MissionWaypointTask waypoint_task
\ No newline at end of file
---
float32 speed
# for debugging usage
uint8 cmd_set
uint8 cmd_id
uint32 ack_data
\ No newline at end of file
float32 speed
---
bool result
\ No newline at end of file
MissionWaypointTask waypoint_task # see msg/MissionWaypointTask.msg
---
bool result
# for debugging usage
uint8 cmd_set
uint8 cmd_id
uint32 ack_data
#request
bool enable_obtain
---
#reponse
bool result
\ No newline at end of file
# 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
#request
---
#response
bool result
\ No newline at end of file
---
uint32 version
string hardware
\ No newline at end of file
#request
---
#response
bool result
\ No newline at end of file
#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
uint8[] data #length(data) <= 100
---
bool result
uint8[] data #length(data) <= 255
---
bool result
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment