AP_DDS: Mode Switch Service

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -10,6 +10,7 @@
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Arming/AP_Arming.h>
+#include <AP_Vehicle/AP_Vehicle.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h>
 #if AP_EXTERNAL_CONTROL_ENABLED
 #include "AP_DDS_ExternalControl.h"
@@ -443,7 +444,6 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
             break;
         }
 
-        subscribe_sample_count++;
         if (rx_joy_topic.axes_size >= 4) {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: %f, %f, %f, %f",
                           rx_joy_topic.axes[0], rx_joy_topic.axes[1], rx_joy_topic.axes[2], rx_joy_topic.axes[3]);
@@ -459,7 +459,6 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
             break;
         }
 
-        subscribe_sample_count++;
         if (rx_dynamic_transforms_topic.transforms_size > 0) {
 #if AP_DDS_VISUALODOM_ENABLED
             AP_DDS_External_Odom::handle_external_odom(rx_dynamic_transforms_topic);
@@ -476,7 +475,6 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
             break;
         }
 
-        subscribe_sample_count++;
 #if AP_EXTERNAL_CONTROL_ENABLED
         if (!AP_DDS_External_Control::handle_velocity_control(rx_velocity_control_topic)) {
             // TODO #23430 handle velocity control failure through rosout, throttled.
@@ -533,8 +531,6 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
             break;
         }
 
-        request_sample_count++;
-
         uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
         if (result) {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Arming/Disarming : SUCCESS");
@@ -543,6 +539,40 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
         }
         break;
     }
+    case services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id: {
+        uint8_t mode;
+        const bool deserialize_success = ucdr_deserialize_uint8_t(ub,&mode);
+        if (deserialize_success == false) {
+            break;
+        }
+        bool status = AP::vehicle()->set_mode(mode, ModeReason::DDS_COMMAND);
+        uint8_t curr_mode = AP::vehicle()->get_mode();
+
+        const uxrObjectId replier_id = {
+            .id = services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id,
+            .type = UXR_REPLIER_ID
+        };
+
+        //Todo : Fix the size-handling of services with the help of the functions autogenerated via Micro-XRCE-DDS Gen
+        uint8_t reply_buffer[8] {};
+        ucdrBuffer reply_ub;
+
+        ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
+        bool serialize_success = true;
+        serialize_success &= ucdr_serialize_bool(&reply_ub, status);
+        serialize_success &= ucdr_serialize_uint8_t(&reply_ub, curr_mode);
+        if (serialize_success == false || reply_ub.error) {
+            break;
+        }
+
+        uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
+        if (status) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Mode Switch : SUCCESS");
+        } else {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Mode Switch : FAIL");
+        }
+        break;
+    }
     }
 }
 

libraries/AP_DDS/AP_DDS_Client.h

@@ -87,14 +87,10 @@ class AP_DDS_Client
     // subscription callback function
     static void on_topic_trampoline(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length, void* args);
     void on_topic(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length);
-    // count of subscribed samples
-    uint32_t subscribe_sample_count;
 
     // service replier callback function
     static void on_request_trampoline(uxrSession* session, uxrObjectId object_id, uint16_t request_id, SampleIdentity* sample_id, ucdrBuffer* ub, uint16_t length, void* args);
     void on_request(uxrSession* session, uxrObjectId object_id, uint16_t request_id, SampleIdentity* sample_id, ucdrBuffer* ub, uint16_t length);
-    // count of request samples
-    uint32_t request_sample_count;
 
     // delivery control parameters
     uxrDeliveryControl delivery_control {
@@ -207,9 +203,6 @@ class AP_DDS_Client
         //! @brief Reply ID for the service
         const uint8_t rep_id;
 
-        //! @brief Profile Label for the service
-        const char* srv_profile_label;
-
         //! @brief Profile Label for the service requester
         const char* req_profile_label;
 

libraries/AP_DDS/AP_DDS_Service_Table.h

@@ -1,7 +1,8 @@
 #include "uxr/client/client.h"
 
 enum class ServiceIndex: uint8_t {
-    ARMING_MOTORS
+    ARMING_MOTORS,
+    MODE_SWITCH
 };
 
 static inline constexpr uint8_t to_underlying(const ServiceIndex index)
@@ -14,8 +15,13 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
     {
         .req_id = to_underlying(ServiceIndex::ARMING_MOTORS),
         .rep_id = to_underlying(ServiceIndex::ARMING_MOTORS),
-        .srv_profile_label = "",
         .req_profile_label = "",
         .rep_profile_label = "arm_motors__replier",
-    }
+    },
+    {
+        .req_id = to_underlying(ServiceIndex::MODE_SWITCH),
+        .rep_id = to_underlying(ServiceIndex::MODE_SWITCH),
+        .req_profile_label = "",
+        .rep_profile_label = "mode_switch__replier",
+    },
 };

libraries/AP_DDS/README.md

@@ -238,6 +238,7 @@ nanosec: 729410000
 ```bash
 $ ros2 service list
 /ap/arm_motors
+/ap/mode_switch
 ---
 ```
 
@@ -262,6 +263,7 @@ List the available services:
 ```bash
 $ ros2 service list -t
 /ap/arm_motors [ardupilot_msgs/srv/ArmMotors]
+/ap/mode_switch [ardupilot_msgs/srv/ModeSwitch]
 ```
 
 Call the arm motors service:
@@ -273,6 +275,16 @@ requester: making request: ardupilot_msgs.srv.ArmMotors_Request(arm=True)
 response:
 ardupilot_msgs.srv.ArmMotors_Response(result=True)
 ```
+
+Call the mode switch service:
+
+```bash
+$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 4}"
+requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=4)
+
+response:
+ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4)
+```
 
 ## Contributing to `AP_DDS` library
 

libraries/AP_DDS/dds_xrce_profile.xml

@@ -268,13 +268,12 @@
       <dataType>geometry_msgs::msg::dds_::TwistStamped_</dataType>
     </topic>
   </data_reader>
-  <replier
-      profile_name="arm_motors__replier"
-      service_name="rs/ap/arm_motorsService"
-      request_type="ardupilot_msgs::srv::dds_::ArmMotors_Request_"
-      reply_type="ardupilot_msgs::srv::dds_::ArmMotors_Response_">
+  <replier profile_name="arm_motors__replier" service_name="rs/ap/arm_motorsService" request_type="ardupilot_msgs::srv::dds_::ArmMotors_Request_" reply_type="ardupilot_msgs::srv::dds_::ArmMotors_Response_">
     <request_topic_name>rq/ap/arm_motorsRequest</request_topic_name>
     <reply_topic_name>rr/ap/arm_motorsReply</reply_topic_name>
   </replier>
-
+  <replier profile_name="mode_switch__replier" service_name="rs/ap/mode_switchService" request_type="ardupilot_msgs::srv::dds_::ModeSwitch_Request_" reply_type="ardupilot_msgs::srv::dds_::ModeSwitch_Response_">
+    <request_topic_name>rq/ap/mode_switchRequest</request_topic_name>
+    <reply_topic_name>rr/ap/mode_switchReply</reply_topic_name>
+  </replier>
 </profiles>

libraries/AP_Vehicle/ModeReason.h

@@ -69,4 +69,5 @@ enum class ModeReason : uint8_t {
   QLAND_INSTEAD_OF_RTL = 49,
   DEADRECKON_FAILSAFE = 50,
   MODE_TAKEOFF_FAILSAFE = 51,
+  DDS_COMMAND = 52,
 };