From 1d552af1e5476d8569638bfab4fb6939c24953c8 Mon Sep 17 00:00:00 2001 From: snktshrma Date: Mon, 29 Apr 2024 02:44:22 +0530 Subject: [PATCH] AP_DDS: Added copter takeoff service --- ArduCopter/Copter.cpp | 4 ++- ArduCopter/Copter.h | 2 +- ArduCopter/mode.cpp | 1 + Tools/ros2/ardupilot_msgs/srv/Takeoff.srv | 10 +++++++ libraries/AP_DDS/AP_DDS_Client.cpp | 28 +++++++++++++++++++ libraries/AP_DDS/AP_DDS_Service_Table.h | 9 +++++- .../AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl | 20 +++++++++++++ libraries/AP_DDS/dds_xrce_profile.xml | 4 +++ 8 files changed, 75 insertions(+), 3 deletions(-) create mode 100644 Tools/ros2/ardupilot_msgs/srv/Takeoff.srv create mode 100644 libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index a16286a6a4a936..e0cda18e3daac5 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -272,7 +272,8 @@ void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, constexpr int8_t Copter::_failsafe_priorities[7]; -#if AP_SCRIPTING_ENABLED + +#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED #if MODE_GUIDED_ENABLED == ENABLED // start takeoff to given altitude (for use by scripting) bool Copter::start_takeoff(float alt) @@ -289,6 +290,7 @@ bool Copter::start_takeoff(float alt) return false; } + // set target location (for use by scripting) bool Copter::set_target_location(const Location& target_loc) { diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 3285f267976e5c..98ce438ae2f0ff 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -666,7 +666,7 @@ class Copter : public AP_Vehicle { void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, uint8_t &task_count, uint32_t &log_bit) override; -#if AP_SCRIPTING_ENABLED +#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED #if MODE_GUIDED_ENABLED == ENABLED bool start_takeoff(float alt) override; bool set_target_location(const Location& target_loc) override; diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index fe900d307db9ae..51097c93074fd9 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -248,6 +248,7 @@ bool Copter::gcs_mode_enabled(const Mode::Number mode_num) // optional force parameter used to force the flight mode change (used only first time mode is set) // returns true if mode was successfully set // ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately + bool Copter::set_mode(Mode::Number mode, ModeReason reason) { // update last reason diff --git a/Tools/ros2/ardupilot_msgs/srv/Takeoff.srv b/Tools/ros2/ardupilot_msgs/srv/Takeoff.srv new file mode 100644 index 00000000000000..3a1d72856543a4 --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/srv/Takeoff.srv @@ -0,0 +1,10 @@ + +# This service requests the vehicle to takeoff + +# alt : Set the takeoff altitude + +uint8 alt +--- +# status : True if the request for mode switch was successful, False otherwise + +bool status diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 373d93bc62ecac..4d1abe4e8b79cc 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -17,6 +17,7 @@ #include "ardupilot_msgs/srv/ArmMotors.h" #include "ardupilot_msgs/srv/ModeSwitch.h" +#include "ardupilot_msgs/srv/Takeoff.h" #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_DDS_ExternalControl.h" @@ -652,6 +653,33 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Mode Switch : %s", msg_prefix, mode_switch_response.status ? "SUCCESS" : "FAIL"); break; } + 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; + } } } diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index 50611276a5539e..353067ab16a1b8 100644 --- a/libraries/AP_DDS/AP_DDS_Service_Table.h +++ b/libraries/AP_DDS/AP_DDS_Service_Table.h @@ -2,7 +2,8 @@ enum class ServiceIndex: uint8_t { ARMING_MOTORS, - MODE_SWITCH + MODE_SWITCH, + TAKEOFF }; static inline constexpr uint8_t to_underlying(const ServiceIndex index) @@ -24,4 +25,10 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { .req_profile_label = "", .rep_profile_label = "mode_switch__replier", }, + { + .req_id = to_underlying(ServiceIndex::TAKEOFF), + .rep_id = to_underlying(ServiceIndex::TAKEOFF), + .req_profile_label = "", + .rep_profile_label = "takeoff__replier", + }, }; 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 00000000000000..0d56fd474a649e --- /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" + "alt : Set the takeoff altitude") + uint8 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/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index 3dcece3f9d30cd..e7fb9d6b7291a6 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -327,6 +327,10 @@ rq/ap/mode_switchRequest rr/ap/mode_switchReply + + rq/ap/takeoffRequest + rr/ap/takeoffReply + rt/ap/cmd_gps_pose ardupilot_msgs::msg::dds_::GlobalPosition_