diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 2e9fe20239269..d5315e829705e 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 a9dbd97cce9f4..de6a7e54acba8 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 { @@ -207,9 +203,6 @@ class AP_DDS_Client //! @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; diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index c5ffb5c737e14..50611276a5539 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,13 @@ 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 = "", .req_profile_label = "", .rep_profile_label = "arm_motors__replier", - } + }, + { + .req_id = to_underlying(ServiceIndex::MODE_SWITCH), + .rep_id = to_underlying(ServiceIndex::MODE_SWITCH), + .req_profile_label = "", + .rep_profile_label = "mode_switch__replier", + }, }; diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 0a82ac1ab4ad7..f4bad30b96633 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -238,6 +238,7 @@ nanosec: 729410000 ```bash $ ros2 service list /ap/arm_motors +/ap/mode_switch --- ``` @@ -262,6 +263,7 @@ List the available services: ```bash $ ros2 service list -t /ap/arm_motors [ardupilot_msgs/srv/ArmMotors] +/ap/mode_switch [ardupilot_msgs/srv/ModeSwitch] ``` Call the arm motors service: @@ -273,6 +275,16 @@ requester: making request: ardupilot_msgs.srv.ArmMotors_Request(arm=True) response: ardupilot_msgs.srv.ArmMotors_Response(result=True) ``` + +Call the mode switch service: + +```bash +$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 4}" +requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=4) + +response: +ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4) +``` ## Contributing to `AP_DDS` library diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index 03baed3bbbe70..225c8f9610f40 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -268,13 +268,12 @@ geometry_msgs::msg::dds_::TwistStamped_ - + rq/ap/arm_motorsRequest rr/ap/arm_motorsReply - + + rq/ap/mode_switchRequest + rr/ap/mode_switchReply + diff --git a/libraries/AP_Vehicle/ModeReason.h b/libraries/AP_Vehicle/ModeReason.h index daa5453e0abcd..d1e124426f28e 100644 --- a/libraries/AP_Vehicle/ModeReason.h +++ b/libraries/AP_Vehicle/ModeReason.h @@ -69,4 +69,5 @@ enum class ModeReason : uint8_t { QLAND_INSTEAD_OF_RTL = 49, DEADRECKON_FAILSAFE = 50, MODE_TAKEOFF_FAILSAFE = 51, + DDS_COMMAND = 52, };