From 380e9aa36b0505ac8d4cefb4604d97144382019a Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Sat, 19 Oct 2024 20:21:00 -0600 Subject: [PATCH] AP_DDS: Wrap services in defines Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- libraries/AP_DDS/AP_DDS_Client.cpp | 17 ++++++++++++++--- libraries/AP_DDS/AP_DDS_Client.h | 4 ++-- libraries/AP_DDS/AP_DDS_Service_Table.h | 9 +++++++++ libraries/AP_DDS/AP_DDS_Topic_Table.h | 8 ++++---- libraries/AP_DDS/AP_DDS_config.h | 14 +++++++++++--- 5 files changed, 40 insertions(+), 12 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index a3ae6506f0..fca7d2f614 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -1,5 +1,6 @@ #include +#include "AP_DDS_config.h" #if AP_DDS_ENABLED #include @@ -12,16 +13,22 @@ #include #include #include +#if AP_DDS_ARM_SERVER_ENABLED #include +# endif // AP_DDS_ARM_SERVER_ENABLED #include #include +#if AP_DDS_ARM_SERVER_ENABLED #include "ardupilot_msgs/srv/ArmMotors.h" +#endif // AP_DDS_ARM_SERVER_ENABLED +#if AP_DDS_MODE_SWITCH_SERVER_ENABLED #include "ardupilot_msgs/srv/ModeSwitch.h" +#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_DDS_ExternalControl.h" -#endif +#endif // AP_EXTERNAL_CONTROL_ENABLED #include "AP_DDS_Frames.h" #include "AP_DDS_Client.h" @@ -667,7 +674,7 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin break; } #endif // AP_DDS_JOY_SUB_ENABLED -#if AP_DDS_DYNAMIC_TF_SUB +#if AP_DDS_DYNAMIC_TF_SUB_ENABLED case topics[to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB)].dr_id.id: { const bool success = tf2_msgs_msg_TFMessage_deserialize_topic(ub, &rx_dynamic_transforms_topic); if (success == false) { @@ -684,7 +691,7 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin } break; } -#endif // AP_DDS_DYNAMIC_TF_SUB +#endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED #if AP_DDS_VEL_CTRL_ENABLED case topics[to_underlying(TopicIndex::VELOCITY_CONTROL_SUB)].dr_id.id: { const bool success = geometry_msgs_msg_TwistStamped_deserialize_topic(ub, &rx_velocity_control_topic); @@ -732,6 +739,7 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u (void) request_id; (void) length; switch (object_id.id) { +#if AP_DDS_ARM_SERVER_ENABLED case services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id: { ardupilot_msgs_srv_ArmMotors_Request arm_motors_request; ardupilot_msgs_srv_ArmMotors_Response arm_motors_response; @@ -761,6 +769,8 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Arming/Disarming : %s", msg_prefix, arm_motors_response.result ? "SUCCESS" : "FAIL"); break; } +#endif // AP_DDS_ARM_SERVER_ENABLED +#if AP_DDS_MODE_SWITCH_SERVER_ENABLED case services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id: { ardupilot_msgs_srv_ModeSwitch_Request mode_switch_request; ardupilot_msgs_srv_ModeSwitch_Response mode_switch_response; @@ -789,6 +799,7 @@ 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; } +#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED } } diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 005b87f30a..92bdbf2a4b 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -195,10 +195,10 @@ private: // incoming REP147 goal interface global position static ardupilot_msgs_msg_GlobalPosition rx_global_position_control_topic; #endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED -#if AP_DDS_DYNAMIC_TF_SUB +#if AP_DDS_DYNAMIC_TF_SUB_ENABLED // incoming transforms static tf2_msgs_msg_TFMessage rx_dynamic_transforms_topic; -#endif // AP_DDS_DYNAMIC_TF_SUB +#endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED HAL_Semaphore csem; // connection parametrics diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index 22f6716bef..0e86496dd5 100644 --- a/libraries/AP_DDS/AP_DDS_Service_Table.h +++ b/libraries/AP_DDS/AP_DDS_Service_Table.h @@ -1,8 +1,13 @@ #include "uxr/client/client.h" +#include enum class ServiceIndex: uint8_t { +#if AP_DDS_ARM_SERVER_ENABLED ARMING_MOTORS, +#endif // #if AP_DDS_ARM_SERVER_ENABLED +#if AP_DDS_MODE_SWITCH_SERVER_ENABLED MODE_SWITCH +#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED }; static inline constexpr uint8_t to_underlying(const ServiceIndex index) @@ -12,6 +17,7 @@ static inline constexpr uint8_t to_underlying(const ServiceIndex index) } constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { +#if AP_DDS_ARM_SERVER_ENABLED { .req_id = to_underlying(ServiceIndex::ARMING_MOTORS), .rep_id = to_underlying(ServiceIndex::ARMING_MOTORS), @@ -28,6 +34,8 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { .depth = 5, }, }, +#endif // AP_DDS_ARM_SERVER_ENABLED +#if AP_DDS_MODE_SWITCH_SERVER_ENABLED { .req_id = to_underlying(ServiceIndex::MODE_SWITCH), .rep_id = to_underlying(ServiceIndex::MODE_SWITCH), @@ -38,4 +46,5 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { .request_topic_name = "rq/ap/mode_switchRequest", .reply_topic_name = "rr/ap/mode_switchReply", }, +#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED }; diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 92d546dd51..8e3823bc23 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -51,9 +51,9 @@ enum class TopicIndex: uint8_t { #if AP_DDS_JOY_SUB_ENABLED JOY_SUB, #endif // AP_DDS_JOY_SUB_ENABLED -#if AP_DDS_DYNAMIC_TF_SUB +#if AP_DDS_DYNAMIC_TF_SUB_ENABLED DYNAMIC_TRANSFORMS_SUB, -#endif // AP_DDS_DYNAMIC_TF_SUB +#endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED #if AP_DDS_VEL_CTRL_ENABLED VELOCITY_CONTROL_SUB, #endif // AP_DDS_VEL_CTRL_ENABLED @@ -286,7 +286,7 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { }, }, #endif // AP_DDS_JOY_SUB_ENABLED -#if AP_DDS_DYNAMIC_TF_SUB +#if AP_DDS_DYNAMIC_TF_SUB_ENABLED { .topic_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), .pub_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), @@ -303,7 +303,7 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, -#endif // AP_DDS_DYNAMIC_TF_SUB +#endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED #if AP_DDS_VEL_CTRL_ENABLED { .topic_id = to_underlying(TopicIndex::VELOCITY_CONTROL_SUB), diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index 4b82ed7465..b8f31e368c 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -114,15 +114,23 @@ #define AP_DDS_GLOBAL_POS_CTRL_ENABLED 1 #endif -#ifndef AP_DDS_DYNAMIC_TF_SUB -#define AP_DDS_DYNAMIC_TF_SUB 1 +#ifndef AP_DDS_DYNAMIC_TF_SUB_ENABLED +#define AP_DDS_DYNAMIC_TF_SUB_ENABLED 1 +#endif + +#ifndef AP_DDS_ARM_SERVER_ENABLED +#define AP_DDS_ARM_SERVER_ENABLED 1 +#endif + +#ifndef AP_DDS_MODE_SWITCH_SERVER_ENABLED +#define AP_DDS_MODE_SWITCH_SERVER_ENABLED 1 #endif // Whether to include Twist support #define AP_DDS_NEEDS_TWIST AP_DDS_VEL_CTRL_ENABLED || AP_DDS_LOCAL_VEL_PUB_ENABLED // Whether to include Transform support -#define AP_DDS_NEEDS_TRANSFORMS AP_DDS_DYNAMIC_TF_SUB || AP_DDS_STATIC_TF_PUB_ENABLED +#define AP_DDS_NEEDS_TRANSFORMS AP_DDS_DYNAMIC_TF_SUB_ENABLED || AP_DDS_STATIC_TF_PUB_ENABLED #ifndef AP_DDS_DEFAULT_UDP_IP_ADDR #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS