diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index a39240143f..9547008c08 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -817,7 +817,13 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u } GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for %sing received", msg_prefix, arm_motors_request.arm ? "arm" : "disarm"); - arm_motors_response.result = arm_motors_request.arm ? AP::arming().arm(AP_Arming::Method::DDS) : AP::arming().disarm(AP_Arming::Method::DDS); +#if AP_EXTERNAL_CONTROL_ENABLED + const bool do_checks = true; + arm_motors_response.result = arm_motors_request.arm ? AP_DDS_External_Control::arm(AP_Arming::Method::DDS, do_checks) : AP_DDS_External_Control::disarm(AP_Arming::Method::DDS, do_checks); + if (!arm_motors_response.result) { + // TODO #23430 handle arm failure through rosout, throttled. + } +#endif // AP_EXTERNAL_CONTROL_ENABLED const uxrObjectId replier_id = { .id = services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id, diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp index 490ce519de..259dcb5160 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp @@ -85,6 +85,26 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta return false; } +bool AP_DDS_External_Control::arm(AP_Arming::Method method, bool do_arming_checks) +{ + auto *external_control = AP::externalcontrol(); + if (external_control == nullptr) { + return false; + } + + return external_control->arm(method, do_arming_checks); +} + +bool AP_DDS_External_Control::disarm(AP_Arming::Method method, bool do_disarm_checks) +{ + auto *external_control = AP::externalcontrol(); + if (external_control == nullptr) { + return false; + } + + return external_control->disarm(method, do_disarm_checks); +} + bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in, Location::AltFrame& frame_out) { diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.h b/libraries/AP_DDS/AP_DDS_ExternalControl.h index 3986ef6377..eeb86e5e9c 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.h +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.h @@ -3,7 +3,7 @@ #if AP_DDS_ENABLED #include "ardupilot_msgs/msg/GlobalPosition.h" #include "geometry_msgs/msg/TwistStamped.h" - +#include #include class AP_DDS_External_Control @@ -13,6 +13,9 @@ public: // https://ros.org/reps/rep-0147.html#goal-interface static bool handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos); static bool handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel); + static bool arm(AP_Arming::Method method, bool do_arming_checks); + static bool disarm(AP_Arming::Method method, bool do_disarm_checks); + private: static bool convert_alt_frame(const uint8_t frame_in, Location::AltFrame& frame_out); };