From 8c2e8ab4df628ff02a8c7907b460d3d6fc92267a Mon Sep 17 00:00:00 2001 From: snktshrma Date: Thu, 21 Nov 2024 13:30:05 +0530 Subject: [PATCH] AP_DDS: Added takeoff DDS service --- libraries/AP_DDS/AP_DDS_Client.cpp | 34 ++++++++++++++++++- libraries/AP_DDS/AP_DDS_Service_Table.h | 15 ++++++++ libraries/AP_DDS/AP_DDS_config.h | 4 +++ .../AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl | 20 +++++++++++ libraries/AP_DDS/README.md | 14 +++++++- 5 files changed, 85 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index a0c9255b25..0a7a81689b 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -29,6 +29,9 @@ #if AP_DDS_ARM_CHECK_SERVER_ENABLED #include "std_srvs/srv/Trigger.h" #endif // AP_DDS_ARM_CHECK_SERVER_ENABLED +#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED +#include "ardupilot_msgs/srv/Takeoff.h" +#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_DDS_ExternalControl.h" @@ -915,6 +918,35 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u break; } #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED +#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED + case services[to_underlying(ServiceIndex::TAKEOFF)].rep_id: { + ardupilot_msgs_srv_Takeoff_Request takeoff_request; + ardupilot_msgs_srv_Takeoff_Response takeoff_response; + const bool deserialize_success = ardupilot_msgs_srv_Takeoff_Request_deserialize_topic(ub, &takeoff_request); + if (deserialize_success == false) { + break; + } + takeoff_response.status = AP::vehicle()->start_takeoff(takeoff_request.alt); + + const uxrObjectId replier_id = { + .id = services[to_underlying(ServiceIndex::TAKEOFF)].rep_id, + .type = UXR_REPLIER_ID + }; + + uint8_t reply_buffer[8] {}; + ucdrBuffer reply_ub; + + ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer)); + const bool serialize_success = ardupilot_msgs_srv_Takeoff_Response_serialize_topic(&reply_ub, &takeoff_response); + if (serialize_success == false) { + break; + } + + uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub)); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Takeoff : %s", msg_prefix, takeoff_response.status ? "SUCCESS" : "FAIL"); + break; + } +#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED #if AP_DDS_ARM_CHECK_SERVER_ENABLED case services[to_underlying(ServiceIndex::PREARM_CHECK)].rep_id: { std_srvs_srv_Trigger_Request prearm_check_request; @@ -943,7 +975,7 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub)); break; } -#endif //AP_DDS_ARM_CHECK_SERVER_ENABLED +#endif //AP_DDS_ARM_CHECK_SERVER_ENABLED #if AP_DDS_PARAMETER_SERVER_ENABLED case services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id: { const bool deserialize_success = rcl_interfaces_srv_SetParameters_Request_deserialize_topic(ub, &set_parameter_request); diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index ba3adc6f07..7f04ab3248 100644 --- a/libraries/AP_DDS/AP_DDS_Service_Table.h +++ b/libraries/AP_DDS/AP_DDS_Service_Table.h @@ -11,6 +11,9 @@ enum class ServiceIndex: uint8_t { #if AP_DDS_ARM_CHECK_SERVER_ENABLED PREARM_CHECK, #endif // AP_DDS_ARM_CHECK_SERVER_ENABLED +#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED + TAKEOFF, +#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED #if AP_DDS_PARAMETER_SERVER_ENABLED SET_PARAMETERS, GET_PARAMETERS @@ -66,6 +69,18 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { .reply_topic_name = "rr/ap/prearm_checkReply", }, #endif // AP_DDS_ARM_CHECK_SERVER_ENABLED +#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED + { + .req_id = to_underlying(ServiceIndex::TAKEOFF), + .rep_id = to_underlying(ServiceIndex::TAKEOFF), + .service_rr = Service_rr::Replier, + .service_name = "rs/ap/experimental/takeoffService", + .request_type = "ardupilot_msgs::srv::dds_::Takeoff_Request_", + .reply_type = "ardupilot_msgs::srv::dds_::Takeoff_Response_", + .request_topic_name = "rq/ap/experimental/takeoffRequest", + .reply_topic_name = "rr/ap/experimental/takeoffReply", + }, +#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED #if AP_DDS_PARAMETER_SERVER_ENABLED { .req_id = to_underlying(ServiceIndex::SET_PARAMETERS), diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index 7ac9864625..022bce3d39 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -141,6 +141,10 @@ #define AP_DDS_MODE_SWITCH_SERVER_ENABLED 1 #endif +#ifndef AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED +#define AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED 1 +#endif + #ifndef AP_DDS_PARAMETER_SERVER_ENABLED #define AP_DDS_PARAMETER_SERVER_ENABLED 1 #endif diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl new file mode 100644 index 0000000000..2e16f5cd62 --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl @@ -0,0 +1,20 @@ +// generated from rosidl_adapter/resource/srv.idl.em +// with input from ardupilot_msgs/srv/Takeoff.srv +// generated code does not contain a copyright notice + + +module ardupilot_msgs { + module srv { + struct Takeoff_Request { + @verbatim (language="comment", text= + "This service requests the vehicle to takeoff" "\n" + "float : Set the takeoff altitude [m] above home, or above terrain if rangefinder is healthy") + float alt; + }; + @verbatim (language="comment", text= + "status : True if the request for takeoff was successful, False otherwise") + struct Takeoff_Response { + boolean status; + }; + }; +}; diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 3a60b88190..ad323ba173 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -210,6 +210,7 @@ $ ros2 service list /ap/arm_motors /ap/mode_switch /ap/prearm_check +/ap/experimental/takeoff --- ``` @@ -236,6 +237,7 @@ $ ros2 service list -t /ap/arm_motors [ardupilot_msgs/srv/ArmMotors] /ap/mode_switch [ardupilot_msgs/srv/ModeSwitch] /ap/prearm_check [std_srvs/srv/Trigger] +/ap/experimental/takeoff [ardupilot_msgs/srv/Takeoff] ``` Call the arm motors service: @@ -272,6 +274,16 @@ or std_srvs.srv.Trigger_Response(success=True, message='Vehicle is Armable') ``` +Call the takeoff service: + +```bash +$ ros2 service call /ap/experimental/takeoff ardupilot_msgs/srv/Takeoff "{alt: 10.5}" +requester: making request: ardupilot_msgs.srv.Takeoff_Request(alt=10.5) + +response: +ardupilot_msgs.srv.Takeoff_Response(status=True) +``` + ## Commanding using ROS 2 Topics The following topic can be used to control the vehicle. @@ -296,7 +308,7 @@ ros2 topic pub /ap/cmd_gps_pose ardupilot_msgs/msg/GlobalPosition "{latitude: 34 publisher: beginning loop publishing #1: ardupilot_msgs.msg.GlobalPosition(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), coordinate_frame=0, type_mask=0, latitude=34.0, longitude=118.0, altitude=1000.0, velocity=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)), acceleration_or_force=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)), yaw=0.0) ``` - + ## Contributing to `AP_DDS` library ### Adding DDS messages to Ardupilot