feat:add joystick server

OSDK-2808

CMakeLists.txt

@@ -118,6 +118,7 @@ add_service_files(
   GetDroneType.srv
   GetM300StereoParams.srv
   FlightTaskControl.srv
+  SetJoystickMode.srv
   ObtainControlAuthority.srv
   GimbalAction.srv
   CameraEV.srv

include/dji_osdk_ros/common_type.h

@@ -53,6 +53,15 @@ namespace dji_osdk_ros
     double yaw_threshold;
   };
 
+  struct JoystickMode
+  {
+    uint8_t horizontalLogic;
+    uint8_t verticalLogic;
+    uint8_t yawLogic;
+    uint8_t horizontalCoordinate;
+    uint8_t stableMode;
+  };
+
   struct RotationAngle
   {
     DJI::OSDK::float32_t roll;

include/dji_osdk_ros/dji_vehicle_node.h

@@ -59,6 +59,7 @@
 
 /*! services */
 #include <dji_osdk_ros/FlightTaskControl.h>
+#include <dji_osdk_ros/SetJoystickMode.h>
 #include <dji_osdk_ros/SetGoHomeAltitude.h>
 #include <dji_osdk_ros/GetGoHomeAltitude.h>
 #include <dji_osdk_ros/SetHomePoint.h>
@@ -193,13 +194,14 @@ namespace dji_osdk_ros
       ros::ServiceServer task_control_server_;
       ros::ServiceServer set_home_altitude_server_;
       ros::ServiceServer get_home_altitude_server_;
+      ros::ServiceServer set_home_point_server_;
       ros::ServiceServer set_current_aircraft_point_as_home_server_;
       ros::ServiceServer set_local_pos_reference_server_;
       ros::ServiceServer set_horizon_avoid_enable_server_;
       ros::ServiceServer get_avoid_enable_status_server_;
       ros::ServiceServer set_upwards_avoid_enable_server_;
 
-      ros::ServiceServer set_home_point_server_;
+
       ros::ServiceServer turn_on_off_motors_server_;
       ros::ServiceServer set_joystick_mode_server_;
       ros::ServiceServer joystick_action_server_;
@@ -331,11 +333,12 @@ namespace dji_osdk_ros
                                 dji_osdk_ros::GetDroneType::Response &response);
       /*! for flight control */
       bool taskCtrlCallback(FlightTaskControl::Request& request, FlightTaskControl::Response& response);
+      bool setJoystickModeCallback(SetJoystickMode::Request& request, SetJoystickMode::Response& response);
       bool setGoHomeAltitudeCallback(SetGoHomeAltitude::Request& request, SetGoHomeAltitude::Response& response);
       bool getGoHomeAltitudeCallback(GetGoHomeAltitude::Request& request, GetGoHomeAltitude::Response& response);
       bool setCurrentAircraftLocAsHomeCallback(SetCurrentAircraftLocAsHomePoint::Request& request, 
                                                SetCurrentAircraftLocAsHomePoint::Response& response);
-      bool SetHomePointCallback(SetHomePoint::Request& request, SetHomePoint::Response& response);
+      bool setHomePointCallback(SetHomePoint::Request& request, SetHomePoint::Response& response);
       bool setLocalPosRefCallback(dji_osdk_ros::SetLocalPosRef::Request &request,
                                   dji_osdk_ros::SetLocalPosRef::Response &response);
       bool setHorizonAvoidCallback(SetAvoidEnable::Request& request, SetAvoidEnable::Response& response);

include/dji_osdk_ros/vehicle_wrapper.h

@@ -107,6 +107,7 @@ namespace dji_osdk_ros
       bool monitoredTakeoff(ACK::ErrorCode& ack, int timeout);
       bool monitoredLanding(ACK::ErrorCode& ack, int timeout);
       bool moveByPositionOffset(ACK::ErrorCode& ack, int timeout, MoveOffset& p_offset);
+      bool setJoystickMode(const JoystickMode &joystickMode);
       bool obtainReleaseCtrlAuthority(bool enableObtain, int timeout);
 
       /*! Parts of Battery */

src/dji_osdk_ros/dji_vehicle_node.cpp

@@ -191,11 +191,12 @@ void VehicleNode::initService()
    *  @platforms M210V2, M300
    */
   task_control_server_ = nh_.advertiseService("flight_task_control", &VehicleNode::taskCtrlCallback, this);
+  set_joystick_mode_server_ = nh_.advertiseService("set_joystick_mode", &VehicleNode::setJoystickModeCallback, this);
   set_home_altitude_server_ = nh_.advertiseService("set_go_home_altitude", &VehicleNode::setGoHomeAltitudeCallback,this);
   get_home_altitude_server_ = nh_.advertiseService("get_go_home_altitude", &VehicleNode::getGoHomeAltitudeCallback,this);
   set_current_aircraft_point_as_home_server_ = nh_.advertiseService("set_current_aircraft_point_as_home",
                                                &VehicleNode::setCurrentAircraftLocAsHomeCallback,this);
-  set_home_point_server_ = nh_.advertiseService("set_home_point", &VehicleNode::SetHomePointCallback, this);
+  set_home_point_server_ = nh_.advertiseService("set_home_point", &VehicleNode::setHomePointCallback, this);
   set_local_pos_reference_server_ = nh_.advertiseService("set_local_pos_reference", &VehicleNode::setLocalPosRefCallback,this);
   set_horizon_avoid_enable_server_ = nh_.advertiseService("set_horizon_avoid_enable", &VehicleNode::setHorizonAvoidCallback,this);
   set_upwards_avoid_enable_server_ = nh_.advertiseService("set_upwards_avoid_enable", &VehicleNode::setUpwardsAvoidCallback, this);
@@ -1069,6 +1070,28 @@ bool VehicleNode::taskCtrlCallback(FlightTaskControl::Request&  request, FlightT
   }
 }
 
+bool VehicleNode::setJoystickModeCallback(SetJoystickMode::Request& request, SetJoystickMode::Response& response)
+{
+  ROS_DEBUG("called setJoystickModeCallback");
+  if(ptr_wrapper_ == nullptr)
+  {
+    ROS_ERROR_STREAM("Vehicle modules is nullptr");
+    return false;
+  }
+
+  dji_osdk_ros::JoystickMode joystickMode;
+  joystickMode.horizontalLogic = request.horizontal_mode;
+  joystickMode.verticalLogic   = request.vertical_mode;
+  joystickMode.yawLogic        = request.yaw_mode;
+  joystickMode.horizontalCoordinate = request.horizontal_coordinate;
+  joystickMode.stableMode      = request.stable_mode;
+
+  ptr_wrapper_->setJoystickMode(joystickMode);
+
+  response.result = true;
+  return true;
+}
+
 bool VehicleNode::gimbalCtrlCallback(GimbalAction::Request& request, GimbalAction::Response& response)
 {
   if(ptr_wrapper_ == nullptr)
@@ -1479,7 +1502,7 @@ bool VehicleNode::setCurrentAircraftLocAsHomeCallback(SetCurrentAircraftLocAsHom
   return true;
 }
 
-bool VehicleNode::SetHomePointCallback(SetHomePoint::Request& request, SetHomePoint::Response& response)
+bool VehicleNode::setHomePointCallback(SetHomePoint::Request& request, SetHomePoint::Response& response)
 {
   ROS_INFO_STREAM("Set home point callback");
   if(ptr_wrapper_ == nullptr)

src/dji_osdk_ros/modules/vehicle_wrapper.cpp

@@ -1519,6 +1519,15 @@ static T_OsdkOsalHandler osalHandler = {
     return false;
   }
 
+  bool VehicleWrapper::setJoystickMode(const JoystickMode &joystickMode)
+  {
+    DJI::OSDK::FlightController::JoystickMode interJoystickMode;
+    memcpy(&interJoystickMode, &joystickMode, sizeof(interJoystickMode));
+    vehicle->flightController->setJoystickMode(interJoystickMode);
+
+    return true;
+  }
+
   bool VehicleWrapper::moveByPositionOffset(ACK::ErrorCode& ack, int timeout, MoveOffset& p_offset)
   {
     using namespace Telemetry;

srv/JoystickAction.srv

@@ -0,0 +1,11 @@
+#request
+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.
+
+---
+#response
+bool result

srv/SetJoystickMode.srv

@@ -0,0 +1,79 @@
+#request
+#contant for horizontal_mode
+# Set the control-mode to control pitch & roll angle of the vehicle.
+# Need to be referenced to either the ground or body frame 
+# by HorizontalCoordinate setting.
+# Limit: 35 degree
+uint8 HORIZONTAL_ANGLE = 0
+# Set the control-mode to control horizontal vehicle velocities.
+# Need to be referenced to either the ground or body frame by
+# HorizontalCoordinate setting.
+# Limit: 30 m/s
+uint8 HORIZONTAL_VELOCITY = 1
+# Set the control-mode to control position offsets of pitch & roll directions
+# Need to be referenced to either the ground r body frame by HorizontalCoordinate setting.
+# Limit: N/A
+uint8 HORIZONTAL_POSITION = 2
+# Set the control-mode to control rate of change of the vehicle's attitude
+# Need to be referenced to either the ground or body frame by HorizontalCoordinate setting.
+# Limit: 150.0 deg/s
+uint8 HORIZONTAL_ANGULAR_RATE = 3
+
+#contant for vertical_mode
+# Set the control-mode to control the vertical
+# speed of UAV, upward is positive
+# Limit: -5 to 5 m/s
+uint8 VERTICAL_VELOCITY = 0
+# Set the control-mode to control the height of UAV
+# Limit: 0 to 120 m
+uint8 VERTICAL_POSITION = 1
+# Set the control-mode to directly control the thrust
+# Range: 0% to 100%
+uint8 VERTICAL_THRUST = 2
+
+#contant for yaw_mode
+# Set the control-mode to control yaw angle.
+# Yaw angle is referenced to the ground frame.
+# In this control mode, Ground frame is enforeced in Autopilot.
+uint8 YAW_ANGLE = 0
+# Set the control-mode to control yaw angular velocity.
+# Same reference frame as YAW_ANGLE.
+# Limite: 150 deg/s
+uint8 YAW_RATE = 1
+#contant for horizontal_coordinate
+# Set the x-y of ground frame as the horizontal frame (NEU) */
+uint8 HORIZONTAL_GROUND = 0
+# Set the x-y of body frame as the horizontal frame (FRU) */
+uint8 HORIZONTAL_BODY = 1
+#contant for stable_mode
+# Disable the stable mode
+uint8 STABLE_DISABLE = 0
+# Enable the stable mode
+uint8 STABLE_ENABLE = 1
+
+# Only when the GPS signal is good (health_flag >=3)，horizontal
+# position control (HORIZONTAL_POSITION) related control modes can be used.
+# Only when GPS signal is good (health_flag >=3)，or when AdvancedSensing
+# system is working properly with Autopilot，horizontal velocity control
+# （HORIZONTAL_VELOCITY）related control modes can be used.
+uint8 horizontal_mode
+
+# We suggest developers do not use VERTICAL_POSITION control mode indoor
+# when your UAV flight height is larger than 3 meters.
+# This is because in indoor environments, barometer can be inaccurate, and
+# the vertical controller may fail to keep the height of the UAV.
+uint8 vertical_mode
+uint8 yaw_mode
+uint8 horizontal_coordinate
+
+# Only works in Horizontal velocity control mode
+# In velocity stable mode, drone will brake and hover at one position once
+# the input command is zero.
+# Drone will try to stay in position once in hover state.
+# In velocity non-stable mode, drone will follow the velocity command and
+# doesn’t hover when the command is zero.
+# That’s to say drone will drift with the wind.
+uint8 stable_mode
+---
+#response
+bool result