diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 2e9fe202392698..d5315e829705e1 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #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; + } } } diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index a9dbd97cce9f42..18aef2322528aa 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/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 { diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index 962fa9197d96de..93579e7a6f2be6 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) @@ -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", + }, }; 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 51% 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..80d1ea33c33c5b 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 } @@ -26,7 +37,7 @@ services: route: dds_server, remap: { dds: { - topic: ArmMotorsService, + topic: ArmMotors_Service, }, ros2: { request_type: "ardupilot_msgs/ArmMotors:request", @@ -34,3 +45,17 @@ services: } } } + 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" + } + } + } diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 819396257bc121..c1ad4efe361d59 100644 --- a/libraries/AP_DDS/README.md +++ b/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 diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index 2ff5207f6870eb..9736ef098e2ecd 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -268,6 +268,8 @@ geometry_msgs::msg::dds_::TwistStamped_ - + + +