AP_DDS: Added 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 {

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,15 @@ 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 = "ArmMotorsService",
+        .srv_profile_label = "ArmMotors_Service",
         .req_profile_label = "",
         .rep_profile_label = "ArmMotors_Replier",
-    }
+    },
+    {
+        .req_id = to_underlying(ServiceIndex::MODE_SWITCH),
+        .rep_id = to_underlying(ServiceIndex::MODE_SWITCH),
+        .srv_profile_label = "ModeSwitch_Service",
+        .req_profile_label = "",
+        .rep_profile_label = "ModeSwitch_Replier",
+    },
 };

libraries/AP_DDS/Is-Config/Arm_Motors_DDS_IS_config.yaml → libraries/AP_DDS/Is-Config/DDS_Service_IS_config.yaml

@@ -10,6 +10,17 @@ types:
             {
                 boolean result;
             };
+
+            struct ModeSwitch_Request
+            {
+                octet mode;
+            };
+
+            struct ModeSwitch_Response
+            {
+                boolean status;
+                octet curr_mode;
+            };
 systems:
     dds: { type: fastdds }
     ros2: { type: ros2 }
@@ -26,11 +37,25 @@ services:
         route: dds_server,
         remap: {
             dds: {
-                topic: ArmMotorsService,
+                topic: ArmMotors_Service,
             },
             ros2: {
                 request_type: "ardupilot_msgs/ArmMotors:request",
                 reply_type: "ardupilot_msgs/ArmMotors:response"
             }
         }
     }
+    mode_switch: {
+        request_type: ModeSwitch_Request,
+        reply_type: ModeSwitch_Response,
+        route: dds_server,
+        remap: {
+            dds: {
+                topic: ModeSwitch_Service,
+            },
+            ros2: {
+                request_type: "ardupilot_msgs/ModeSwitch:request",
+                reply_type: "ardupilot_msgs/ModeSwitch:response"
+            }
+        }
+    }

libraries/AP_DDS/README.md

@@ -257,14 +257,15 @@ Next, follow the associated section for your chosen transport, and finally you c
 ### Terminal 2 (Integration Service)
 - Source ROS 2 installation
 - Source Integration Service installation
-- Move to the **AP_DDS** folder and run the following command `integration-service Is-Config/Arm_Motors_DDS_IS_config.yaml`
+- Move to the **AP_DDS** folder and run the following command `integration-service Is-Config/DDS_Service_IS_config.yaml`
 
 ### Terminal 3 (Ardupilot)
 - Make sure you have successfully setup Ardupilot and the `DDS_ENABLE` param is set to 1
 - Run SITL with the following command `sim_vehicle.py -v ArduPlane -DG --console --enable-dds`
 
 ### Terminal 4 (ROS 2 Client)
-- Run the following command : `ros2 service call /arm_motors ardupilot_msgs/srv/ArmMotors "{arm: True}"`
+- Run the following command (for arming/disarming motors): `ros2 service call /arm_motors ardupilot_msgs/srv/ArmMotors "{arm: True}"`
+- Run the following command (for switching drive modes): `ros2 service call /mode_select ardupilot_msgs/srv/ModeSelect "{mode: 4}"`
  
 ## Contributing to AP_DDS library
 ### Adding DDS messages to Ardupilot

libraries/AP_DDS/dds_xrce_profile.xml

@@ -268,6 +268,8 @@
       <dataType>geometry_msgs::msg::dds_::TwistStamped_</dataType>
     </topic>
   </data_reader>
-  <replier profile_name="ArmMotors_Replier" service_name="ArmMotorsService" request_type="ArmMotors_Request" reply_type="ArmMotors_Response">
+  <replier profile_name="ArmMotors_Replier" service_name="ArmMotors_Service" request_type="ArmMotors_Request" reply_type="ArmMotors_Response">
+  </replier>
+  <replier profile_name="ModeSwitch_Replier" service_name="ModeSwitch_Service" request_type="ModeSwitch_Request" reply_type="ModeSwitch_Response">
   </replier>
 </profiles>