AP_DDS: use generated types for service serialisation

Signed-off-by: Rhys Mainwaring <[email protected]>
tridge · Oct 18, 2023 · fbdd4d6 · fbdd4d6
1 parent 091c315
commit fbdd4d6
Showing 1 changed file with 25 additions and 23 deletions.
diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp
@@ -12,6 +12,10 @@
 #include <AP_Arming/AP_Arming.h>
 #include <AP_Vehicle/AP_Vehicle.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h>
+
+#include "ardupilot_msgs/srv/ArmMotors.h"
+#include "ardupilot_msgs/srv/ModeSwitch.h"
+
 #if AP_EXTERNAL_CONTROL_ENABLED
 #include "AP_DDS_ExternalControl.h"
 #endif
@@ -501,75 +505,72 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
     (void) length;
     switch (object_id.id) {
     case services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id: {
-        bool arm;
-        bool result;
-        const bool deserialize_success = ucdr_deserialize_bool(ub,&arm);
+        ardupilot_msgs_srv_ArmMotors_Request arm_motors_request;
+        ardupilot_msgs_srv_ArmMotors_Response arm_motors_response;
+        const bool deserialize_success = ardupilot_msgs_srv_ArmMotors_Request_deserialize_topic(ub, &arm_motors_request);
         if (deserialize_success == false) {
             break;
         }
 
-        if (arm) {
+        if (arm_motors_request.arm) {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Request for arming received");
-            result = AP::arming().arm(AP_Arming::Method::DDS);
+            arm_motors_response.result = AP::arming().arm(AP_Arming::Method::DDS);
         } else {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Request for disarming received");
-            result = AP::arming().disarm(AP_Arming::Method::DDS);
+            arm_motors_response.result = AP::arming().disarm(AP_Arming::Method::DDS);
         }
 
         const uxrObjectId replier_id = {
             .id = services[to_underlying(ServiceIndex::ARMING_MOTORS)].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));
-        const bool serialize_success = ucdr_serialize_bool(&reply_ub,result);
+        const bool serialize_success = ardupilot_msgs_srv_ArmMotors_Response_serialize_topic(&reply_ub, &arm_motors_response);
         if (serialize_success == false) {
             break;
         }
 
         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");
+        if (arm_motors_response.result) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DDS Client: Request for Arming/Disarming : SUCCESS");
         } else {
-            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Arming/Disarming : FAIL");
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DDS Client: Request for Arming/Disarming : FAIL");
         }
         break;
     }
     case services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id: {
-        uint8_t mode;
-        const bool deserialize_success = ucdr_deserialize_uint8_t(ub,&mode);
+        ardupilot_msgs_srv_ModeSwitch_Request mode_switch_request;
+        ardupilot_msgs_srv_ModeSwitch_Response mode_switch_response;
+        const bool deserialize_success = ardupilot_msgs_srv_ModeSwitch_Request_deserialize_topic(ub, &mode_switch_request);
         if (deserialize_success == false) {
             break;
         }
-        bool status = AP::vehicle()->set_mode(mode, ModeReason::DDS_COMMAND);
-        uint8_t curr_mode = AP::vehicle()->get_mode();
+        mode_switch_response.status = AP::vehicle()->set_mode(mode_switch_request.mode, ModeReason::DDS_COMMAND);
+        mode_switch_response.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) {
+        const bool serialize_success = ardupilot_msgs_srv_ModeSwitch_Response_serialize_topic(&reply_ub, &mode_switch_response);
+        if (serialize_success == false) {
             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");
+        if (mode_switch_response.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");
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "DDS Client: Request for Mode Switch : FAIL");
         }
         break;
     }
@@ -839,6 +840,7 @@ void AP_DDS_Client::write_battery_state_topic()
         }
     }
 }
+
 void AP_DDS_Client::write_local_pose_topic()
 {
     WITH_SEMAPHORE(csem);