From 644b75d9bc31bdb251391713fb487d8d46b9a961 Mon Sep 17 00:00:00 2001
From: arshPratap <arshpratapofficial@gmail.com>
Date: Sat, 19 Aug 2023 18:52:41 +0530
Subject: [PATCH] AP_DDS: Added Mode Switch Service [skip_ci]

---
 libraries/AP_DDS/AP_DDS_Client.cpp            | 37 +++++++++++++++++++
 libraries/AP_DDS/AP_DDS_Service_Table.h       | 12 +++++-
 ...config.yaml => DDS_Service_IS_config.yaml} | 25 +++++++++++++
 libraries/AP_DDS/dds_xrce_profile.xml         |  2 +
 4 files changed, 74 insertions(+), 2 deletions(-)
 rename libraries/AP_DDS/Is-Config/{Arm_Motors_DDS_IS_config.yaml => DDS_Service_IS_config.yaml} (54%)

diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp
index 5c2933f2d54460..ba08811c654121 100644
--- a/libraries/AP_DDS/AP_DDS_Client.cpp
+++ b/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_DDS_Client.h"
 #include "AP_DDS_Topic_Table.h"
@@ -538,6 +539,42 @@ 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;
+        }
+
+        request_sample_count++;
+
+        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;
+    }
     }
 }
 
diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h
index 962fa9197d96de..3dd57ce5c6cb1e 100644
--- a/libraries/AP_DDS/AP_DDS_Service_Table.h
+++ b/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)
@@ -17,5 +18,12 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
         .srv_profile_label = "ArmMotorsService",
         .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 = "ModeSwitchService",
+        .req_profile_label = "",
+        .rep_profile_label = "ModeSwitch_Replier",
+    },
 };
diff --git a/libraries/AP_DDS/Is-Config/Arm_Motors_DDS_IS_config.yaml b/libraries/AP_DDS/Is-Config/DDS_Service_IS_config.yaml
similarity index 54%
rename from libraries/AP_DDS/Is-Config/Arm_Motors_DDS_IS_config.yaml
rename to libraries/AP_DDS/Is-Config/DDS_Service_IS_config.yaml
index ef46bf6aa943b5..f040729d04824a 100644
--- a/libraries/AP_DDS/Is-Config/Arm_Motors_DDS_IS_config.yaml
+++ b/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 }
@@ -34,3 +45,17 @@ services:
             }
         }
     }
+    mode_switch: {
+        request_type: ModeSwitch_Request,
+        reply_type: ModeSwitch_Response,
+        route: dds_server,
+        remap: {
+            dds: {
+                topic: ModeSwitchService,
+            },
+            ros2: {
+                request_type: "ardupilot_msgs/ModeSwitch:request",
+                reply_type: "ardupilot_msgs/ModeSwitch:response"
+            }
+        }
+    }
diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml
index dd76da5c2ee506..ce828992cecb48 100644
--- a/libraries/AP_DDS/dds_xrce_profile.xml
+++ b/libraries/AP_DDS/dds_xrce_profile.xml
@@ -270,4 +270,6 @@
   </data_reader>
   <replier profile_name="ArmMotors_Replier" service_name="ArmMotorsService" request_type="ArmMotors_Request" reply_type="ArmMotors_Response">
   </replier>
+  <replier profile_name="ModeSwitch_Replier" service_name="ModeSwitchService" request_type="ModeSwitch_Request" reply_type="ModeSwitch_Response">
+  </replier>
 </profiles>