AP_DDS: Added copter takeoff service

ArduCopter/Copter.cpp

@@ -272,7 +272,8 @@ void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
 
 constexpr int8_t Copter::_failsafe_priorities[7];
 
-#if AP_SCRIPTING_ENABLED
+
+#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 #if MODE_GUIDED_ENABLED == ENABLED
 // start takeoff to given altitude (for use by scripting)
 bool Copter::start_takeoff(float alt)
@@ -289,6 +290,7 @@ bool Copter::start_takeoff(float alt)
     return false;
 }
 
+
 // set target location (for use by scripting)
 bool Copter::set_target_location(const Location& target_loc)
 {

ArduCopter/Copter.h

@@ -666,7 +666,7 @@ class Copter : public AP_Vehicle {
     void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
                              uint8_t &task_count,
                              uint32_t &log_bit) override;
-#if AP_SCRIPTING_ENABLED
+#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 #if MODE_GUIDED_ENABLED == ENABLED
     bool start_takeoff(float alt) override;
     bool set_target_location(const Location& target_loc) override;

ArduCopter/mode.cpp

@@ -248,6 +248,7 @@ bool Copter::gcs_mode_enabled(const Mode::Number mode_num)
 // optional force parameter used to force the flight mode change (used only first time mode is set)
 // returns true if mode was successfully set
 // ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
+
 bool Copter::set_mode(Mode::Number mode, ModeReason reason)
 {
     // update last reason

Tools/ros2/ardupilot_msgs/srv/Takeoff.srv

@@ -0,0 +1,10 @@
+
+# This service requests the vehicle to takeoff
+
+# alt : Set the takeoff altitude
+
+uint8 alt
+---
+# status    : True if the request for mode switch was successful, False otherwise
+
+bool status

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -17,6 +17,7 @@
 
 #include "ardupilot_msgs/srv/ArmMotors.h"
 #include "ardupilot_msgs/srv/ModeSwitch.h"
+#include "ardupilot_msgs/srv/Takeoff.h"
 
 #if AP_EXTERNAL_CONTROL_ENABLED
 #include "AP_DDS_ExternalControl.h"
@@ -652,6 +653,33 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Mode Switch : %s", msg_prefix, mode_switch_response.status ? "SUCCESS" : "FAIL");
         break;
     }
+    case services[to_underlying(ServiceIndex::TAKEOFF)].rep_id: {
+        ardupilot_msgs_srv_Takeoff_Request takeoff_request;
+        ardupilot_msgs_srv_Takeoff_Response takeoff_response;
+        const bool deserialize_success = ardupilot_msgs_srv_Takeoff_Request_deserialize_topic(ub, &takeoff_request);
+        if (deserialize_success == false) {
+            break;
+        }
+        takeoff_response.status = AP::vehicle()->start_takeoff(takeoff_request.alt);
+
+        const uxrObjectId replier_id = {
+            .id = services[to_underlying(ServiceIndex::TAKEOFF)].rep_id,
+            .type = UXR_REPLIER_ID
+        };
+
+        uint8_t reply_buffer[8] {};
+        ucdrBuffer reply_ub;
+
+        ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
+        const bool serialize_success = ardupilot_msgs_srv_Takeoff_Response_serialize_topic(&reply_ub, &takeoff_response);
+        if (serialize_success == false) {
+            break;
+        }
+
+        uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Takeoff : %s", msg_prefix, takeoff_response.status ? "SUCCESS" : "FAIL");
+        break;
+    }
     }
 }
 

libraries/AP_DDS/AP_DDS_Service_Table.h

@@ -2,7 +2,8 @@
 
 enum class ServiceIndex: uint8_t {
     ARMING_MOTORS,
-    MODE_SWITCH
+    MODE_SWITCH,
+    TAKEOFF
 };
 
 static inline constexpr uint8_t to_underlying(const ServiceIndex index)
@@ -24,4 +25,10 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
         .req_profile_label = "",
         .rep_profile_label = "mode_switch__replier",
     },
+    {
+        .req_id = to_underlying(ServiceIndex::TAKEOFF),
+        .rep_id = to_underlying(ServiceIndex::TAKEOFF),
+        .req_profile_label = "",
+        .rep_profile_label = "takeoff__replier",
+    },
 };

libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl

@@ -0,0 +1,20 @@
+// generated from rosidl_adapter/resource/srv.idl.em
+// with input from ardupilot_msgs/srv/Takeoff.srv
+// generated code does not contain a copyright notice
+
+
+module ardupilot_msgs {
+  module srv {
+    struct Takeoff_Request {
+      @verbatim (language="comment", text=
+        "This service requests the vehicle to takeoff" "\n"
+        "alt : Set the takeoff altitude")
+      uint8 alt;
+    };
+    @verbatim (language="comment", text=
+      "status    : True if the request for takeoff was successful, False otherwise")
+    struct Takeoff_Response {
+      boolean status;
+    };
+  };
+};

libraries/AP_DDS/dds_xrce_profile.xml

@@ -327,6 +327,10 @@
     <request_topic_name>rq/ap/mode_switchRequest</request_topic_name>
     <reply_topic_name>rr/ap/mode_switchReply</reply_topic_name>
   </replier>
+  <replier profile_name="takeoff__replier" service_name="rs/ap/takeoffService" request_type="ardupilot_msgs::srv::dds_::Takeoff_Request_" reply_type="ardupilot_msgs::srv::dds_::Takeoff_Response_">
+    <request_topic_name>rq/ap/takeoffRequest</request_topic_name>
+    <reply_topic_name>rr/ap/takeoffReply</reply_topic_name>
+  </replier>
   <topic profile_name="globalposcontrol__t">
     <name>rt/ap/cmd_gps_pose</name>
     <dataType>ardupilot_msgs::msg::dds_::GlobalPosition_</dataType>