AP_DDS: Added ROS 2 service support [skip ci]

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -9,6 +9,7 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_AHRS/AP_AHRS.h>
+#include <AP_Arming/AP_Arming.h>
 
 #include "AP_DDS_Client.h"
 
@@ -19,6 +20,11 @@ static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = 33;
 static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33;
 static constexpr uint16_t DELAY_CLOCK_TOPIC_MS = 10;
 static char WGS_84_FRAME_ID[] = "WGS-84";
+
+// static because on_request is a static member function
+static uxrStreamId reliable_in;
+static uxrStreamId reliable_out;
+
 // https://www.ros.org/reps/rep-0105.html#base-link
 static char BASE_LINK_FRAME_ID[] = "base_link";
 
@@ -402,9 +408,8 @@ bool AP_DDS_Client::start(void)
 }
 
 // read function triggered at every subscription callback
-void AP_DDS_Client::on_topic (uxrSession* uxr_session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length, void* args)
+void AP_DDS_Client::on_topic ([[maybe_unused]] uxrSession* uxr_session, uxrObjectId object_id, [[maybe_unused]] uint16_t request_id, [[maybe_unused]] uxrStreamId stream_id, struct ucdrBuffer* ub, [[maybe_unused]] uint16_t length, void* args)
 {
-    (void) uxr_session; (void) object_id; (void) request_id; (void) stream_id; (void) length;
     /*
     TEMPLATE for reading to the subscribed topics
     1) Store the read contents into the ucdr buffer
@@ -433,6 +438,54 @@ void AP_DDS_Client::on_topic (uxrSession* uxr_session, uxrObjectId object_id, ui
 
 }
 
+#include "AP_DDS_Service_Table.h"
+
+void AP_DDS_Client::on_request([[maybe_unused]] uxrSession* uxr_session, uxrObjectId object_id, [[maybe_unused]] uint16_t request_id, SampleIdentity* sample_id, ucdrBuffer* ub, [[maybe_unused]] uint16_t length, void* args){
+    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);
+        if(deserialize_success == false){
+            break;
+        }
+
+        if(arm == true) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for arming recieved");
+            result = AP::arming().arm(AP_Arming::Method::DDS);
+        } else {
+            result = !AP::arming().disarm(AP_Arming::Method::DDS);
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for disarming recieved");
+        }
+
+        const uxrObjectId replier_id = {
+            .id = services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id,
+            .type = UXR_REPLIER_ID
+        };
+        uint8_t reply_buffer[8] = {
+            0
+        };
+        ucdrBuffer reply_ub;
+
+        ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
+        const bool serialize_success = ucdr_serialize_bool(&reply_ub,result);
+        if(serialize_success == false){
+            break;
+        }
+
+        uint32_t* count_ptr = (uint32_t*) args;
+        (*count_ptr)++;
+
+        uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, sizeof(result));
+        if(result == true){
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Reply : Armed ");
+        }else{
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Reply : Disarmed ");
+        }
+        break;
+    }    
+}
+
 /*
   main loop for DDS thread
  */
@@ -472,7 +525,10 @@ bool AP_DDS_Client::init()
     }
 
     // Register topic callbacks
-    uxr_set_topic_callback(&session, AP_DDS_Client::on_topic, &count);
+    uxr_set_topic_callback(&session, AP_DDS_Client::on_topic, &subscribe_sample_count);
+
+    // ROS-2 Service : Register service request callbacks
+    uxr_set_request_callback(&session, AP_DDS_Client::on_request, &request_sample_count);
 
     while (!uxr_create_session(&session)) {
         GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Initialization waiting...");
@@ -536,7 +592,7 @@ bool AP_DDS_Client::create()
         // Status requests
         constexpr uint8_t nRequests = 3;
         uint16_t requests[nRequests];
-        constexpr uint16_t requestTimeoutMs = (uint8_t) nRequests * maxTimeMsPerRequestMs;
+        constexpr uint16_t requestTimeoutMs = nRequests * maxTimeMsPerRequestMs;
         uint8_t status[nRequests];
 
         if (strlen(topics[i].dw_profile_label) > 0) {
@@ -586,18 +642,56 @@ bool AP_DDS_Client::create()
             requests[2] = dreader_req_id;
 
             if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) {
-                GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Topic/Sub/Reader session request failure for index '%d'",(int)i);
+                GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Topic/Sub/Reader session request failure for index '%zu'",i);
                 for (uint8_t s = 0 ; s < nRequests; s++) {
                     GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Status '%d' result '%u'", s, status[s]);
                 }
                 // TODO add a failure log message sharing the status results
                 return false;
             } else {
-                GCS_SEND_TEXT(MAV_SEVERITY_INFO,"XRCE Client: Topic/Sub/Reader session pass for index '%d'",(int)i);
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO,"XRCE Client: Topic/Sub/Reader session pass for index '%zu'",i);
                 uxr_buffer_request_data(&session, reliable_out, topics[i].dr_id, reliable_in, &delivery_control);
             }
         }
     }
+
+    // ROS-2 Service : else case for service requests
+
+    for (size_t i = 0; i < ARRAY_SIZE(services);i++){
+
+        //status requests
+        constexpr uint8_t nRequests = 1;
+        uint16_t requests[nRequests];
+        constexpr uint16_t requestTimeoutMs = nRequests * maxTimeMsPerRequestMs;
+        uint8_t status[nRequests];
+
+        if(strlen(services[i].rep_profile_label) > 0){
+            const uxrObjectId rep_id = {
+                .id = services[i].rep_id,
+                .type = UXR_REPLIER_ID
+            };
+            const char* replier_ref = services[i].rep_profile_label;    
+            const auto  replier_req_id = uxr_buffer_create_replier_ref(&session, reliable_out, rep_id, participant_id, replier_ref, UXR_REPLACE);
+
+            requests[0] = replier_req_id;
+
+            if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) {
+                GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Service/Replier session request failure for index '%zu'",i);
+                for (uint8_t s = 0 ; s < nRequests; s++) {
+                    GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Status '%d' result '%u'", s, status[s]);
+                }
+                // TODO add a failure log message sharing the status results
+                return false;
+            } else {
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO,"XRCE Client: Service/Replier session pass for index '%zu'",i);
+                uxr_buffer_request_data(&session, reliable_out, rep_id, reliable_in, &delivery_control);
+            }
+
+        } else if (strlen(services[i].req_profile_label) > 0){
+            // TODO : Add Similar Code for Requester Profile
+        }
+    }
+
     return true;
 }
 

libraries/AP_DDS/AP_DDS_Client.h

@@ -51,8 +51,6 @@ class AP_DDS_Client
     // input and output stream
     uint8_t *input_reliable_stream;
     uint8_t *output_reliable_stream;
-    uxrStreamId reliable_in;
-    uxrStreamId reliable_out;
 
     // Topic
     builtin_interfaces_msg_Time time_topic;
@@ -81,10 +79,14 @@ class AP_DDS_Client
 
     // subscription callback function
     static void on_topic(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length, void* args);
-
     // count of subscribed samples
-    uint32_t count;
+    uint32_t subscribe_sample_count;
 
+    // service replier callback function
+    static void on_request(uxrSession* session, uxrObjectId object_id, uint16_t request_id, SampleIdentity* sample_id, ucdrBuffer* ub, uint16_t length, void* args);
+    // count of request samples
+    uint32_t request_sample_count;
+
     // delivery control parameters
     uxrDeliveryControl delivery_control {
         .max_samples = UXR_MAX_SAMPLES_UNLIMITED,
@@ -188,7 +190,24 @@ class AP_DDS_Client
     };
     static const struct Topic_table topics[];
 
-
+    //! @brief Convenience grouping for a single "channel" of services
+    struct Service_table{
+        //! @brief Request ID for the service
+        const uint8_t req_id;
+
+        //! @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;
+
+        //! @brief Profile Label for the service replier
+        const char* rep_profile_label;
+    };
+    static const struct Service_table services[];
 };
 
 #endif // AP_DDS_ENABLED

libraries/AP_DDS/AP_DDS_Service_Table.h

@@ -0,0 +1,21 @@
+#include "uxr/client/client.h"
+
+enum class ServiceIndex: uint8_t {
+    ARMING_MOTORS
+};
+
+static inline constexpr uint8_t to_underlying(const ServiceIndex index)
+{
+    static_assert(sizeof(index) == sizeof(uint8_t));
+    return static_cast<uint8_t>(index);
+}
+
+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",
+        .req_profile_label = "",
+        .rep_profile_label = "ArmMotors_Replier",
+    }
+};

libraries/AP_DDS/Is-Config/Arm_Motors_DDS_IS_config.yaml

@@ -0,0 +1,36 @@
+types:
+    idls:
+        - >
+            struct ArmMotors_Request
+            {
+                boolean arm;
+            };
+
+            struct ArmMotors_Response
+            {
+                boolean result;
+            };
+systems:
+    dds: { type: fastdds }
+    ros2: { type: ros2 }
+
+routes:
+    dds_server:
+        server: dds
+        clients: ros2
+
+services:
+    arm_motors: {
+        request_type: ArmMotors_Request,
+        reply_type: ArmMotors_Response,
+        route: dds_server,
+        remap: {
+            dds: {
+                topic: ArmMotorsService,
+            },
+            ros2: {
+                request_type: "ap_custom_services/ArmMotors:request",
+                reply_type: "ap_custom_services/ArmMotors:response"
+            }
+        }
+    }

libraries/AP_DDS/README.md

@@ -230,7 +230,45 @@ Next, follow the associated section for your chosen transport, and finally you c
   ```
   In order to consume the transforms, it's highly recommended to [create and run a transform broadcaster in ROS 2](https://docs.ros.org/en/humble/Concepts/About-Tf2.html#tutorials).
 
-
+## Using ROS 2 services (with Integration Services)
+
+### Prerequisites
+- Install and setup [Micro-XRCE Agent](https://micro-xrce-dds.docs.eprosima.com/en/latest/installation.html#installing-the-agent-standalone)
+- Install and setup [Integration Services](https://micro-xrce-dds.docs.eprosima.com/en/latest/installation.html#installing-the-client-standalone) (it would be good to have a seperate workspace for this)
+  - Get System Handles for [ROS 2](https://github.com/eProsima/ROS2-SH)
+  - Get System Handles for [Fast-DDS](https://github.com/eProsima/FastDDS-SH)
+
+### Setup
+- Clone the [ardupilot_ros2](https://github.com/arshPratap/ardupilot_ros2) repo in your ROS 2 workspace
+- Switch to **ArmingClient** branch
+- Build the [ap_custom_services](https://github.com/arshPratap/ardupilot_ros2/tree/ArmingClient/ap_custom_services) package
+- Build the [ap_service_clients](https://github.com/arshPratap/ardupilot_ros2/tree/ArmingClient/ap_service_clients) package
+- In a new terminal, successfully source ROS 2 installation
+- Move to your ROS 2 workspace and source the installation (you should now see the above 2 pacakges when you run `ros2 pkg list`)
+- Move to your Integration Service workspace and use the following command to successfully link the ***ap_custom_services*** package with Integration Services `colcon build --cmake-args -DMIX_ROS_PACKAGES="ap_custom_services"`
+- Source the above Integration Service installation
+
+### Terminal 1 (XRCE Agent)
+- Move to the **AP_DDS** folder and run the XRCE Agent as follows `MicroXRCEAgent udp4 -p 2019 -r dds_xrce_profile.xml`
+
+### 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`
+
+### 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)
+- Source ROS 2 installation
+- In your ROS 2 workspace source the installation of the above packages
+- The ROS 2 client takes in an argument for the total number of requests to be made to the Arming Server
+- Run a ROS 2 client as follows : `ros2 run ap_service_clients arm_motors_client 5` (here 5 is the number of requests to be made)
+- Once the client is running , pass in the following inputs
+  - `arm` for **Arming**
+  - `disarm` for **Disarming**
+ 
 ## Contributing to AP_DDS library
 ### Adding DDS messages to Ardupilot
 

libraries/AP_DDS/dds_xrce_profile.xml

@@ -238,4 +238,9 @@
       <dataType>sensor_msgs::msg::dds_::Joy_</dataType>
     </topic>
   </data_reader>
+  <replier profile_name = "ArmMotors_Replier" 
+           service_name = "ArmMotorsService" 
+           request_type = "ArmMotors_Request" 
+           reply_type = "ArmMotors_Response">
+  </replier>
 </profiles>