feat:add get avoid(upwards/collision) enable status server

OSDK-2808
bhaskar-glitch · Jan 6, 2021 · 04ef096 · 04ef096
1 parent abc2204
commit 04ef096
Show file tree

Hide file tree

Showing 9 changed files with 102 additions and 21 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -141,6 +141,7 @@ add_service_files(
   SetNewHomePoint.srv
   SetupCameraH264.srv
   AvoidEnable.srv
+  GetAvoidEnable.srv
   InitWaypointV2Setting.srv
   UploadWaypointV2Mission.srv
   UploadWaypointV2Action.srv

diff --git a/ReadMe.md b/ReadMe.md
@@ -33,8 +33,8 @@ This update mainly includes:
 |flight_control_node                   |flight_task_control                               |                                       |
 |                                      |set_go_home_altitude                              |                                       |
 |                                      |set_current_point_as_home                         |                                       |
-|                                      |enable_avoid                                      |                                       |
-|                                      |enable_upwards_avoid                              |                                       |
+|                                      |set_collision_avoid_enable                                      |                                       |
+|                                      |set_upwards_avoid_enable                              |                                       |
 |gimbal_camera_control_node            |gimbal_task_control                               |                                       |
 |                                      |camera_task_set_EV                                |                                       |
 |                                      |camera_task_set_shutter_speed                     |                                       |

diff --git a/include/dji_osdk_ros/dji_vehicle_node.h b/include/dji_osdk_ros/dji_vehicle_node.h
@@ -63,6 +63,7 @@
 #include <dji_osdk_ros/SetNewHomePoint.h>
 #include <dji_osdk_ros/SetLocalPosRef.h>
 #include <dji_osdk_ros/AvoidEnable.h>
+#include <dji_osdk_ros/GetAvoidEnable.h>
 #include <dji_osdk_ros/ObtainControlAuthority.h>
 #include <dji_osdk_ros/GimbalAction.h>
 #include <dji_osdk_ros/CameraEV.h>
@@ -186,13 +187,24 @@ namespace dji_osdk_ros
       /*! for general */
       ros::ServiceServer get_drone_type_server_;
       /*! for flight control */
+      ros::ServiceServer obtain_releae_control_authority_server_;
       ros::ServiceServer task_control_server_;
       ros::ServiceServer set_home_altitude_server_;
       ros::ServiceServer set_current_point_as_home_server_;
       ros::ServiceServer set_local_pos_reference_server_;
-      ros::ServiceServer avoid_enable_server_;
-      ros::ServiceServer upwards_avoid_enable_server_;
-      ros::ServiceServer obtain_releae_control_authority_server_;
+
+      ros::ServiceServer get_home_altitude_server_;
+      ros::ServiceServer set_collision_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 set_current_aircraft_point_as_home_server_;
+      ros::ServiceServer turn_on_off_motors_server_;
+      ros::ServiceServer set_joystick_mode_server_;
+      ros::ServiceServer joystick_action_server_;
+      ros::ServiceServer emergency_brake_action_server_;
+      ros::ServiceServer kill_switch_server_;
+
       /*! for gimbal */
       ros::ServiceServer gimbal_control_server_;
       /*! for camera */
@@ -322,8 +334,9 @@ namespace dji_osdk_ros
       bool setHomeCallback(SetNewHomePoint::Request& request, SetNewHomePoint::Response& response);
       bool setLocalPosRefCallback(dji_osdk_ros::SetLocalPosRef::Request &request,
                                   dji_osdk_ros::SetLocalPosRef::Response &response);
-      bool setAvoidCallback(AvoidEnable::Request& request, AvoidEnable::Response& response);
+      bool setCollisionAvoidCallback(AvoidEnable::Request& request, AvoidEnable::Response& response);
       bool setUpwardsAvoidCallback(AvoidEnable::Request& request, AvoidEnable::Response& response);
+      bool getAvoidEnableStatusCallback(GetAvoidEnable::Request& request, GetAvoidEnable::Response& response);
       bool obtainReleaseControlAuthorityCallback(ObtainControlAuthority::Request& request, ObtainControlAuthority::Response& reponse);
       /*! for gimbal control */
       bool gimbalCtrlCallback(GimbalAction::Request& request, GimbalAction::Response& response);

diff --git a/include/dji_osdk_ros/vehicle_wrapper.h b/include/dji_osdk_ros/vehicle_wrapper.h
@@ -97,8 +97,10 @@ namespace dji_osdk_ros
       bool setHomeAltitude(uint16_t altitude, int timeout = 1);
       bool goHome(ACK::ErrorCode& ack, int timeout);
       bool goHomeAndConfirmLanding(int timeout);
-      bool setAvoid(bool enable);
+      bool setCollisionAvoidance(bool enable);
+      bool getCollisionAvoidance(uint8_t& enable);
       bool setUpwardsAvoidance(bool enable);
+      bool getUpwardsAvoidance(uint8_t& enable);
 
       bool monitoredTakeoff(ACK::ErrorCode& ack, int timeout);
       bool monitoredLanding(ACK::ErrorCode& ack, int timeout);

diff --git a/src/dji_osdk_ros/dji_vehicle_node.cpp b/src/dji_osdk_ros/dji_vehicle_node.cpp
@@ -194,8 +194,9 @@ void VehicleNode::initService()
   set_home_altitude_server_ = nh_.advertiseService("set_go_home_altitude", &VehicleNode::setGoHomeAltitudeCallback,this);
   set_current_point_as_home_server_ = nh_.advertiseService("set_current_point_as_home", &VehicleNode::setHomeCallback,this);
   set_local_pos_reference_server_ = nh_.advertiseService("set_local_pos_reference", &VehicleNode::setLocalPosRefCallback,this);
-  avoid_enable_server_ = nh_.advertiseService("enable_avoid", &VehicleNode::setAvoidCallback,this);
-  upwards_avoid_enable_server_ = nh_.advertiseService("enable_upwards_avoid", &VehicleNode::setUpwardsAvoidCallback, this);
+  set_collision_avoid_enable_server_ = nh_.advertiseService("set_collision_avoid_enable", &VehicleNode::setCollisionAvoidCallback,this);
+  set_upwards_avoid_enable_server_ = nh_.advertiseService("set_upwards_avoid_enable", &VehicleNode::setUpwardsAvoidCallback, this);
+  get_avoid_enable_status_server_ = nh_.advertiseService("get_avoid_enable_status", &VehicleNode::getAvoidEnableStatusCallback, this);
   obtain_releae_control_authority_server_ = nh_.advertiseService("obtain_release_control_authority", &VehicleNode::obtainReleaseControlAuthorityCallback, this);
 
   /*! @brief
@@ -1486,28 +1487,46 @@ bool VehicleNode::setLocalPosRefCallback(dji_osdk_ros::SetLocalPosRef::Request &
   return true;
 }
 
-bool VehicleNode::setAvoidCallback(AvoidEnable::Request& request, AvoidEnable::Response& response)
+bool VehicleNode::setCollisionAvoidCallback(AvoidEnable::Request& request, AvoidEnable::Response& response)
 {
-  ROS_INFO_STREAM("Set avoid function callback");
+  ROS_INFO_STREAM("Set collision avoid function callback");
   if(ptr_wrapper_ == nullptr)
   {
     ROS_ERROR_STREAM("Vehicle modules is nullptr");
     return false;
   }
 
-  if(ptr_wrapper_->setAvoid(request.enable) == true)
+  if (!(ptr_wrapper_->setCollisionAvoidance(request.enable)))
   {
-    response.result = true;
+    response.result = false;
+    return false;
   }
-  else
+
+  response.result = true;
+  return true;
+}
+
+bool VehicleNode::setUpwardsAvoidCallback(AvoidEnable::Request& request, AvoidEnable::Response& response)
+{
+  ROS_INFO_STREAM("Set upwards avoid function callback");
+  if(ptr_wrapper_ == nullptr)
+  {
+    ROS_ERROR_STREAM("Vehicle modules is nullptr");
+    return false;
+  }
+
+  if(!(ptr_wrapper_->setUpwardsAvoidance(request.enable)))
   {
     response.result = false;
+    return false;
   }
 
+
+  response.result = true;
   return true;
 }
 
-bool VehicleNode::setUpwardsAvoidCallback(AvoidEnable::Request& request, AvoidEnable::Response& response)
+bool VehicleNode::getAvoidEnableStatusCallback(GetAvoidEnable::Request& request, GetAvoidEnable::Response& response)
 {
   ROS_INFO_STREAM("Set upwards avoid function callback");
   if(ptr_wrapper_ == nullptr)
@@ -1516,15 +1535,25 @@ bool VehicleNode::setUpwardsAvoidCallback(AvoidEnable::Request& request, AvoidEn
     return false;
   }
 
-  if(ptr_wrapper_->setUpwardsAvoidance(request.enable) == true)
+  uint8_t get_collision_avoid_enable_status = 0xF;
+  uint8_t get_upwards_avoid_enable_status = 0xF;
+
+  if (!(ptr_wrapper_->getCollisionAvoidance(get_collision_avoid_enable_status)))
   {
-    response.result = true;
+    response.result = false;
+    return false;
   }
-  else
+  response.collision_avoid_enable_status = get_collision_avoid_enable_status;
+
+  if(!(ptr_wrapper_->getUpwardsAvoidance(get_upwards_avoid_enable_status)))
   {
     response.result = false;
+    return false;
   }
 
+  response.upwards_avoid_enable_status = get_collision_avoid_enable_status;
+
+  response.result = true;
   return true;
 }
 

diff --git a/src/dji_osdk_ros/modules/vehicle_wrapper.cpp b/src/dji_osdk_ros/modules/vehicle_wrapper.cpp
@@ -1436,7 +1436,7 @@ static T_OsdkOsalHandler osalHandler = {
     }
   }
 
-  bool VehicleWrapper::setAvoid(bool enable)
+  bool VehicleWrapper::setCollisionAvoidance(bool enable)
   {
     auto enum_enable = enable ? FlightController::AvoidEnable::AVOID_ENABLE : FlightController::AvoidEnable::AVOID_DISABLE;
     ErrorCode::ErrorCodeType ack = vehicle->flightController->setCollisionAvoidanceEnabledSync(enum_enable, 1);
@@ -1447,6 +1447,20 @@ static T_OsdkOsalHandler osalHandler = {
     return false;
   }
 
+  bool VehicleWrapper::getCollisionAvoidance(uint8_t& enable)
+  {
+    FlightController::AvoidEnable enum_enable;
+    ErrorCode::ErrorCodeType ack = vehicle->flightController->getCollisionAvoidanceEnabledSync(enum_enable, 1);
+    enable = static_cast<uint8_t>(enum_enable);
+    if (ack == ErrorCode::SysCommonErr::Success)
+    {
+      return true;
+    }
+
+    enable = 0xF;
+    return false;
+  }
+
   bool VehicleWrapper::setUpwardsAvoidance(bool enable)
   {
     auto enum_enable = enable ? FlightController::UpwardsAvoidEnable::UPWARDS_AVOID_DISABLE : FlightController::UpwardsAvoidEnable::UPWARDS_AVOID_ENABLE;
@@ -1458,6 +1472,20 @@ static T_OsdkOsalHandler osalHandler = {
     return false;
   }
 
+  bool VehicleWrapper::getUpwardsAvoidance(uint8_t& enable)
+  {
+    FlightController::UpwardsAvoidEnable enum_enable;
+    ErrorCode::ErrorCodeType ack = vehicle->flightController->getUpwardsAvoidanceEnabledSync(enum_enable, 1);
+    enable = static_cast<uint8_t>(enum_enable);
+    if (ack == ErrorCode::SysCommonErr::Success)
+    {
+      return true;
+    }
+
+    enable = 0xF;
+    return false;
+  }
+
   bool VehicleWrapper::moveByPositionOffset(ACK::ErrorCode& ack, int timeout, MoveOffset& p_offset)
   {
     using namespace Telemetry;

diff --git a/src/dji_osdk_ros/samples/flight_control_node.cpp b/src/dji_osdk_ros/samples/flight_control_node.cpp
@@ -51,8 +51,8 @@ int main(int argc, char** argv)
   task_control_client = nh.serviceClient<FlightTaskControl>("/flight_task_control");
   auto set_go_home_altitude_client = nh.serviceClient<SetGoHomeAltitude>("/set_go_home_altitude");
   auto set_current_point_as_home_client = nh.serviceClient<SetNewHomePoint>("/set_current_point_as_home");
-  auto enable_avoid_client = nh.serviceClient<AvoidEnable>("/enable_avoid");
-  auto enable_upward_avoid_client = nh.serviceClient<AvoidEnable>("/enable_upwards_avoid");
+  auto enable_avoid_client = nh.serviceClient<AvoidEnable>("/set_collision_avoid_enable");
+  auto enable_upward_avoid_client = nh.serviceClient<AvoidEnable>("/set_upwards_avoid_enable");
   auto obtain_ctrl_authority_client = nh.serviceClient<dji_osdk_ros::ObtainControlAuthority>("obtain_release_control_authority");
   std::cout
       << "| Available commands:                                            |"

diff --git a/srv/AvoidEnable.srv b/srv/AvoidEnable.srv
@@ -1,3 +1,5 @@
+#request
 bool enable
 ---
+#response
 bool result
diff --git a/srv/GetAvoidEnable.srv b/srv/GetAvoidEnable.srv
@@ -0,0 +1,6 @@
+#request
+---
+#response
+bool result
+uint8 collision_avoid_enable_status #0:disable 1:enable 0xF:invalid
+uint8 upwards_avoid_enable_status