From 88c06e07d7caa9953172c8c78007f52458029a86 Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Fri, 20 Sep 2024 22:27:52 -0600 Subject: [PATCH] AP_DDS: Wrap all topics in ifdefs * Give ability to enable/disable any topic in DDS through compile options Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- libraries/AP_DDS/AP_DDS_Client.cpp | 91 +++++++++++-- libraries/AP_DDS/AP_DDS_Client.h | 177 +++++++++++++++++--------- libraries/AP_DDS/AP_DDS_Topic_Table.h | 52 ++++++++ libraries/AP_DDS/AP_DDS_config.h | 58 +++++++++ 4 files changed, 305 insertions(+), 73 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index d224a22551..926d4304b7 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -31,26 +31,45 @@ // Enable DDS at runtime by default static constexpr uint8_t ENABLED_BY_DEFAULT = 1; +#if AP_DDS_TIME_PUB_ENABLED static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10; +#endif // AP_DDS_TIME_PUB_ENABLED +#if AP_DDS_BATTERY_STATE_PUB_ENABLED static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000; +#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED #if AP_DDS_IMU_PUB_ENABLED static constexpr uint16_t DELAY_IMU_TOPIC_MS = 5; #endif // AP_DDS_IMU_PUB_ENABLED +#if AP_DDS_LOCAL_POSE_PUB_ENABLED static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33; +#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED +#if AP_DDS_LOCAL_VEL_PUB_ENABLED static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = 33; +#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED +#if AP_DDS_GEOPOSE_PUB_ENABLED static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33; +#endif // AP_DDS_GEOPOSE_PUB_ENABLED +#if AP_DDS_CLOCK_PUB_ENABLED static constexpr uint16_t DELAY_CLOCK_TOPIC_MS = 10; +#endif // AP_DDS_CLOCK_PUB_ENABLED +#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED static constexpr uint16_t DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS = 1000; +#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED static constexpr uint16_t DELAY_PING_MS = 500; // Define the subscriber data members, which are static class scope. // If these are created on the stack in the subscriber, // the AP_DDS_Client::on_topic frame size is exceeded. +#if AP_DDS_JOY_SUB_ENABLED sensor_msgs_msg_Joy AP_DDS_Client::rx_joy_topic {}; +#endif // AP_DDS_JOY_SUB_ENABLED tf2_msgs_msg_TFMessage AP_DDS_Client::rx_dynamic_transforms_topic {}; +#if AP_DDS_VEL_CTRL_ENABLED geometry_msgs_msg_TwistStamped AP_DDS_Client::rx_velocity_control_topic {}; +#endif // AP_DDS_VEL_CTRL_ENABLED +#if AP_DDS_GLOBAL_POS_CTRL_ENABLED ardupilot_msgs_msg_GlobalPosition AP_DDS_Client::rx_global_position_control_topic {}; - +#endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED const AP_Param::GroupInfo AP_DDS_Client::var_info[] { @@ -127,6 +146,7 @@ AP_DDS_Client::~AP_DDS_Client() } } +#if AP_DDS_TIME_PUB_ENABLED void AP_DDS_Client::update_topic(builtin_interfaces_msg_Time& msg) { uint64_t utc_usec; @@ -137,7 +157,9 @@ void AP_DDS_Client::update_topic(builtin_interfaces_msg_Time& msg) msg.nanosec = (utc_usec % 1000000ULL) * 1000UL; } +#endif // AP_DDS_TIME_PUB_ENABLED +#if AP_DDS_NAVSATFIX_PUB_ENABLED bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) { // Add a lambda that takes in navsatfix msg and populates the cov @@ -224,7 +246,9 @@ bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i return true; } +#endif // AP_DDS_NAVSATFIX_PUB_ENABLED +#if AP_DDS_STATIC_TF_PUB_ENABLED void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg) { msg.transforms_size = 0; @@ -266,7 +290,9 @@ void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg) } } +#endif // AP_DDS_STATIC_TF_PUB_ENABLED +#if AP_DDS_BATTERY_STATE_PUB_ENABLED void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance) { if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) { @@ -331,7 +357,9 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_ } } } +#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED +#if AP_DDS_LOCAL_POSE_PUB_ENABLED void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg) { update_topic(msg.header.stamp); @@ -380,7 +408,9 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg) initialize(msg.pose.orientation); } } +#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED +#if AP_DDS_LOCAL_VEL_PUB_ENABLED void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg) { update_topic(msg.header.stamp); @@ -422,7 +452,9 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg) msg.twist.angular.y = -angular_velocity[1]; msg.twist.angular.z = -angular_velocity[2]; } +#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED +#if AP_DDS_GEOPOSE_PUB_ENABLED void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) { update_topic(msg.header.stamp); @@ -461,6 +493,7 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) initialize(msg.pose.orientation); } } +#endif // AP_DDS_GEOPOSE_PUB_ENABLED #if AP_DDS_IMU_PUB_ENABLED void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg) @@ -502,11 +535,14 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg) } #endif // AP_DDS_IMU_PUB_ENABLED +#if AP_DDS_CLOCK_PUB_ENABLED void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg) { update_topic(msg.clock); } +#endif // AP_DDS_CLOCK_PUB_ENABLED +#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg) { update_topic(msg.header.stamp); @@ -524,6 +560,7 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg) msg.position.altitude = ekf_origin.alt * 0.01; } } +#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED /* start the DDS thread @@ -566,6 +603,7 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin (void) stream_id; (void) length; switch (object_id.id) { +#if AP_DDS_JOY_SUB_ENABLED case topics[to_underlying(TopicIndex::JOY_SUB)].dr_id.id: { const bool success = sensor_msgs_msg_Joy_deserialize_topic(ub, &rx_joy_topic); @@ -594,6 +632,8 @@ 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 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) { @@ -610,12 +650,13 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin } break; } +#endif // AP_DDS_DYNAMIC_TF_SUB +#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); if (success == false) { break; } - #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. @@ -623,6 +664,8 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin #endif // AP_EXTERNAL_CONTROL_ENABLED break; } +#endif // AP_DDS_VEL_CTRL_ENABLED +#if AP_DDS_GLOBAL_POS_CTRL_ENABLED case topics[to_underlying(TopicIndex::GLOBAL_POSITION_SUB)].dr_id.id: { const bool success = ardupilot_msgs_msg_GlobalPosition_deserialize_topic(ub, &rx_global_position_control_topic); if (success == false) { @@ -636,6 +679,7 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin #endif // AP_EXTERNAL_CONTROL_ENABLED break; } +#endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED } } @@ -744,8 +788,10 @@ void AP_DDS_Client::main_loop(void) connected = true; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Initialization passed", msg_prefix); +#if AP_DDS_STATIC_TF_PUB_ENABLED populate_static_transforms(tx_static_transforms_topic); write_static_transforms(); +#endif // AP_DDS_STATIC_TF_PUB_ENABLED uint64_t last_ping_ms{0}; uint8_t num_pings_missed{0}; @@ -998,6 +1044,7 @@ void AP_DDS_Client::write_time_topic() } } +#if AP_DDS_NAVSATFIX_PUB_ENABLED void AP_DDS_Client::write_nav_sat_fix_topic() { WITH_SEMAPHORE(csem); @@ -1012,7 +1059,9 @@ void AP_DDS_Client::write_nav_sat_fix_topic() } } } +#endif // AP_DDS_NAVSATFIX_PUB_ENABLED +#if AP_DDS_STATIC_TF_PUB_ENABLED void AP_DDS_Client::write_static_transforms() { WITH_SEMAPHORE(csem); @@ -1027,7 +1076,9 @@ void AP_DDS_Client::write_static_transforms() } } } +#endif // AP_DDS_STATIC_TF_PUB_ENABLED +#if AP_DDS_BATTERY_STATE_PUB_ENABLED void AP_DDS_Client::write_battery_state_topic() { WITH_SEMAPHORE(csem); @@ -1042,7 +1093,9 @@ void AP_DDS_Client::write_battery_state_topic() } } } +#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED +#if AP_DDS_LOCAL_POSE_PUB_ENABLED void AP_DDS_Client::write_local_pose_topic() { WITH_SEMAPHORE(csem); @@ -1057,7 +1110,9 @@ void AP_DDS_Client::write_local_pose_topic() } } } +#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED +#if AP_DDS_LOCAL_VEL_PUB_ENABLED void AP_DDS_Client::write_tx_local_velocity_topic() { WITH_SEMAPHORE(csem); @@ -1072,6 +1127,7 @@ void AP_DDS_Client::write_tx_local_velocity_topic() } } } +#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED #if AP_DDS_IMU_PUB_ENABLED void AP_DDS_Client::write_imu_topic() @@ -1090,6 +1146,7 @@ void AP_DDS_Client::write_imu_topic() } #endif // AP_DDS_IMU_PUB_ENABLED +#if AP_DDS_GEOPOSE_PUB_ENABLED void AP_DDS_Client::write_geo_pose_topic() { WITH_SEMAPHORE(csem); @@ -1104,7 +1161,9 @@ void AP_DDS_Client::write_geo_pose_topic() } } } +#endif // AP_DDS_GEOPOSE_PUB_ENABLED +#if AP_DDS_CLOCK_PUB_ENABLED void AP_DDS_Client::write_clock_topic() { WITH_SEMAPHORE(csem); @@ -1119,7 +1178,9 @@ void AP_DDS_Client::write_clock_topic() } } } +#endif // AP_DDS_CLOCK_PUB_ENABLED +#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED void AP_DDS_Client::write_gps_global_origin_topic() { WITH_SEMAPHORE(csem); @@ -1133,66 +1194,76 @@ void AP_DDS_Client::write_gps_global_origin_topic() } } } +#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED void AP_DDS_Client::update() { WITH_SEMAPHORE(csem); const auto cur_time_ms = AP_HAL::millis64(); +#if AP_DDS_TIME_PUB_ENABLED if (cur_time_ms - last_time_time_ms > DELAY_TIME_TOPIC_MS) { update_topic(time_topic); last_time_time_ms = cur_time_ms; write_time_topic(); } - +#endif // AP_DDS_TIME_PUB_ENABLED +#if AP_DDS_NAVSATFIX_PUB_ENABLED constexpr uint8_t gps_instance = 0; if (update_topic(nav_sat_fix_topic, gps_instance)) { write_nav_sat_fix_topic(); } - +#endif // AP_DDS_NAVSATFIX_PUB_ENABLED +#if AP_DDS_BATTERY_STATE_PUB_ENABLED if (cur_time_ms - last_battery_state_time_ms > DELAY_BATTERY_STATE_TOPIC_MS) { constexpr uint8_t battery_instance = 0; update_topic(battery_state_topic, battery_instance); last_battery_state_time_ms = cur_time_ms; write_battery_state_topic(); } - +#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED +#if AP_DDS_LOCAL_POSE_PUB_ENABLED if (cur_time_ms - last_local_pose_time_ms > DELAY_LOCAL_POSE_TOPIC_MS) { update_topic(local_pose_topic); last_local_pose_time_ms = cur_time_ms; write_local_pose_topic(); } - +#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED +#if AP_DDS_LOCAL_VEL_PUB_ENABLED if (cur_time_ms - last_local_velocity_time_ms > DELAY_LOCAL_VELOCITY_TOPIC_MS) { update_topic(tx_local_velocity_topic); last_local_velocity_time_ms = cur_time_ms; write_tx_local_velocity_topic(); } +#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED #if AP_DDS_IMU_PUB_ENABLED if (cur_time_ms - last_imu_time_ms > DELAY_IMU_TOPIC_MS) { update_topic(imu_topic); last_imu_time_ms = cur_time_ms; write_imu_topic(); } -#endif - +#endif // AP_DDS_IMU_PUB_ENABLED +#if AP_DDS_GEOPOSE_PUB_ENABLED if (cur_time_ms - last_geo_pose_time_ms > DELAY_GEO_POSE_TOPIC_MS) { update_topic(geo_pose_topic); last_geo_pose_time_ms = cur_time_ms; write_geo_pose_topic(); } - +#endif // AP_DDS_GEOPOSE_PUB_ENABLED +#if AP_DDS_CLOCK_PUB_ENABLED if (cur_time_ms - last_clock_time_ms > DELAY_CLOCK_TOPIC_MS) { update_topic(clock_topic); last_clock_time_ms = cur_time_ms; write_clock_topic(); } - +#endif // AP_DDS_CLOCK_PUB_ENABLED +#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED if (cur_time_ms - last_gps_global_origin_time_ms > DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS) { update_topic(gps_global_origin_topic); last_gps_global_origin_time_ms = cur_time_ms; write_gps_global_origin_topic(); } +#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED status_ok = uxr_run_session_time(&session, 1); } diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index c020eae179..2d2ec5a27f 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -7,21 +7,42 @@ #include "uxr/client/client.h" #include "ucdr/microcdr.h" +#if AP_DDS_GLOBAL_POS_CTRL_ENABLED #include "ardupilot_msgs/msg/GlobalPosition.h" +#endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED +#if AP_DDS_TIME_PUB_ENABLED #include "builtin_interfaces/msg/Time.h" - +#endif // AP_DDS_TIME_PUB_ENABLED +#if AP_DDS_NAVSATFIX_PUB_ENABLED #include "sensor_msgs/msg/NavSatFix.h" +#endif // AP_DDS_NAVSATFIX_PUB_ENABLED +#if AP_DDS_NEEDS_TRANSFORMS #include "tf2_msgs/msg/TFMessage.h" +#endif // AP_DDS_NEEDS_TRANSFORMS +#if AP_DDS_BATTERY_STATE_PUB_ENABLED #include "sensor_msgs/msg/BatteryState.h" +#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED #if AP_DDS_IMU_PUB_ENABLED #include "sensor_msgs/msg/Imu.h" #endif // AP_DDS_IMU_PUB_ENABLED +#if AP_DDS_JOY_SUB_ENABLED #include "sensor_msgs/msg/Joy.h" +#endif // AP_DDS_JOY_SUB_ENABLED +#if AP_DDS_LOCAL_POSE_PUB_ENABLED #include "geometry_msgs/msg/PoseStamped.h" +#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED +#if AP_DDS_NEEDS_TWIST #include "geometry_msgs/msg/TwistStamped.h" +#endif // AP_DDS_NEEDS_TWIST +#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED #include "geographic_msgs/msg/GeoPointStamped.h" +#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_GEOPOSE_PUB_ENABLED #include "geographic_msgs/msg/GeoPoseStamped.h" +#endif // AP_DDS_GEOPOSE_PUB_ENABLED +#if AP_DDS_CLOCK_PUB_ENABLED #include "rosgraph_msgs/msg/Clock.h" +#endif // AP_DDS_CLOCK_PUB_ENABLED #include #include @@ -60,46 +81,119 @@ private: uxrStreamId reliable_out; // Outgoing Sensor and AHRS data + +#if AP_DDS_TIME_PUB_ENABLED builtin_interfaces_msg_Time time_topic; + // The last ms timestamp AP_DDS wrote a Time message + uint64_t last_time_time_ms; + //! @brief Serialize the current time state and publish to the IO stream(s) + void write_time_topic(); + static void update_topic(builtin_interfaces_msg_Time& msg); +#endif // AP_DDS_TIME_PUB_ENABLED + +#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED geographic_msgs_msg_GeoPointStamped gps_global_origin_topic; + // The last ms timestamp AP_DDS wrote a gps global origin message + uint64_t last_gps_global_origin_time_ms; + //! @brief Serialize the current gps global origin and publish to the IO stream(s) + void write_gps_global_origin_topic(); + static void update_topic(geographic_msgs_msg_GeoPointStamped& msg); +# endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED + +#if AP_DDS_GEOPOSE_PUB_ENABLED geographic_msgs_msg_GeoPoseStamped geo_pose_topic; + // The last ms timestamp AP_DDS wrote a GeoPose message + uint64_t last_geo_pose_time_ms; + //! @brief Serialize the current geo_pose and publish to the IO stream(s) + void write_geo_pose_topic(); + static void update_topic(geographic_msgs_msg_GeoPoseStamped& msg); +#endif // AP_DDS_GEOPOSE_PUB_ENABLED + +#if AP_DDS_LOCAL_POSE_PUB_ENABLED geometry_msgs_msg_PoseStamped local_pose_topic; + // The last ms timestamp AP_DDS wrote a Local Pose message + uint64_t last_local_pose_time_ms; + //! @brief Serialize the current local_pose and publish to the IO stream(s) + void write_local_pose_topic(); + static void update_topic(geometry_msgs_msg_PoseStamped& msg); +#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED + +#if AP_DDS_LOCAL_VEL_PUB_ENABLED geometry_msgs_msg_TwistStamped tx_local_velocity_topic; + // The last ms timestamp AP_DDS wrote a Local Velocity message + uint64_t last_local_velocity_time_ms; + //! @brief Serialize the current local velocity and publish to the IO stream(s) + void write_tx_local_velocity_topic(); + static void update_topic(geometry_msgs_msg_TwistStamped& msg); +#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED + +#if AP_DDS_BATTERY_STATE_PUB_ENABLED sensor_msgs_msg_BatteryState battery_state_topic; + // The last ms timestamp AP_DDS wrote a BatteryState message + uint64_t last_battery_state_time_ms; + //! @brief Serialize the current nav_sat_fix state and publish it to the IO stream(s) + void write_battery_state_topic(); + static void update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance); +#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED + +#if AP_DDS_NAVSATFIX_PUB_ENABLED sensor_msgs_msg_NavSatFix nav_sat_fix_topic; + // The last ms timestamp AP_DDS wrote a NavSatFix message + uint64_t last_nav_sat_fix_time_ms; + //! @brief Serialize the current nav_sat_fix state and publish to the IO stream(s) + void write_nav_sat_fix_topic(); + bool update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) WARN_IF_UNUSED; +#endif // AP_DDS_NAVSATFIX_PUB_ENABLED + #if AP_DDS_IMU_PUB_ENABLED sensor_msgs_msg_Imu imu_topic; + // The last ms timestamp AP_DDS wrote an IMU message + uint64_t last_imu_time_ms; + static void update_topic(sensor_msgs_msg_Imu& msg); + //! @brief Serialize the current IMU data and publish to the IO stream(s) + void write_imu_topic(); #endif // AP_DDS_IMU_PUB_ENABLED + +#if AP_DDS_CLOCK_PUB_ENABLED rosgraph_msgs_msg_Clock clock_topic; - // incoming joystick data - static sensor_msgs_msg_Joy rx_joy_topic; - // incoming REP147 velocity control - static geometry_msgs_msg_TwistStamped rx_velocity_control_topic; - // incoming REP147 goal interface global position - static ardupilot_msgs_msg_GlobalPosition rx_global_position_control_topic; + // The last ms timestamp AP_DDS wrote a Clock message + uint64_t last_clock_time_ms; + //! @brief Serialize the current clock and publish to the IO stream(s) + void write_clock_topic(); + static void update_topic(rosgraph_msgs_msg_Clock& msg); +#endif // AP_DDS_CLOCK_PUB_ENABLED + +#if AP_DDS_STATIC_TF_PUB_ENABLED // outgoing transforms tf2_msgs_msg_TFMessage tx_static_transforms_topic; + //! @brief Serialize the static transforms and publish to the IO stream(s) + void write_static_transforms(); + static void populate_static_transforms(tf2_msgs_msg_TFMessage& msg); +#endif // AP_DDS_STATIC_TF_PUB_ENABLED + +#if AP_DDS_JOY_SUB_ENABLED + // incoming joystick data + static sensor_msgs_msg_Joy rx_joy_topic; +#endif // AP_DDS_JOY_SUB_ENABLED +#if AP_DDS_VEL_CTRL_ENABLED + // incoming REP147 velocity control + static geometry_msgs_msg_TwistStamped rx_velocity_control_topic; +#endif // AP_DDS_VEL_CTRL_ENABLED +#if AP_DDS_GLOBAL_POS_CTRL_ENABLED + // 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 // incoming transforms static tf2_msgs_msg_TFMessage rx_dynamic_transforms_topic; - +#endif // AP_DDS_DYNAMIC_TF_SUB HAL_Semaphore csem; // connection parametrics bool status_ok{false}; bool connected{false}; - static void update_topic(builtin_interfaces_msg_Time& msg); - bool update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) WARN_IF_UNUSED; - static void populate_static_transforms(tf2_msgs_msg_TFMessage& msg); - static void update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance); - static void update_topic(geometry_msgs_msg_PoseStamped& msg); - static void update_topic(geometry_msgs_msg_TwistStamped& msg); - static void update_topic(geographic_msgs_msg_GeoPoseStamped& msg); -#if AP_DDS_IMU_PUB_ENABLED - static void update_topic(sensor_msgs_msg_Imu& msg); -#endif // AP_DDS_IMU_PUB_ENABLED - static void update_topic(rosgraph_msgs_msg_Clock& msg); - static void update_topic(geographic_msgs_msg_GeoPointStamped& msg); + // 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); @@ -117,27 +211,6 @@ private: .min_pace_period = 0 }; - // The last ms timestamp AP_DDS wrote a Time message - uint64_t last_time_time_ms; - // The last ms timestamp AP_DDS wrote a NavSatFix message - uint64_t last_nav_sat_fix_time_ms; - // The last ms timestamp AP_DDS wrote a BatteryState message - uint64_t last_battery_state_time_ms; -#if AP_DDS_IMU_PUB_ENABLED - // The last ms timestamp AP_DDS wrote an IMU message - uint64_t last_imu_time_ms; -#endif // AP_DDS_IMU_PUB_ENABLED - // The last ms timestamp AP_DDS wrote a Local Pose message - uint64_t last_local_pose_time_ms; - // The last ms timestamp AP_DDS wrote a Local Velocity message - uint64_t last_local_velocity_time_ms; - // The last ms timestamp AP_DDS wrote a GeoPose message - uint64_t last_geo_pose_time_ms; - // The last ms timestamp AP_DDS wrote a Clock message - uint64_t last_clock_time_ms; - // The last ms timestamp AP_DDS wrote a gps global origin message - uint64_t last_gps_global_origin_time_ms; - // functions for serial transport bool ddsSerialInit(); static bool serial_transport_open(uxrCustomTransport* args); @@ -191,28 +264,6 @@ public: //! @return True on successful creation, false on failure bool create() WARN_IF_UNUSED; - //! @brief Serialize the current time state and publish to the IO stream(s) - void write_time_topic(); - //! @brief Serialize the current nav_sat_fix state and publish to the IO stream(s) - void write_nav_sat_fix_topic(); - //! @brief Serialize the static transforms and publish to the IO stream(s) - void write_static_transforms(); - //! @brief Serialize the current nav_sat_fix state and publish it to the IO stream(s) - void write_battery_state_topic(); - //! @brief Serialize the current local_pose and publish to the IO stream(s) - void write_local_pose_topic(); - //! @brief Serialize the current local velocity and publish to the IO stream(s) - void write_tx_local_velocity_topic(); - //! @brief Serialize the current geo_pose and publish to the IO stream(s) - void write_geo_pose_topic(); -#if AP_DDS_IMU_PUB_ENABLED - //! @brief Serialize the current IMU data and publish to the IO stream(s) - void write_imu_topic(); -#endif // AP_DDS_IMU_PUB_ENABLED - //! @brief Serialize the current clock and publish to the IO stream(s) - void write_clock_topic(); - //! @brief Serialize the current gps global origin and publish to the IO stream(s) - void write_gps_global_origin_topic(); //! @brief Update the internally stored DDS messages with latest data void update(); diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 2329dde701..4864a15b67 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -14,22 +14,48 @@ // Can use jinja to template (like Flask) enum class TopicIndex: uint8_t { +#if AP_DDS_TIME_PUB_ENABLED TIME_PUB, +#endif // AP_DDS_TIME_PUB_ENABLED +#if AP_DDS_NAVSATFIX_PUB_ENABLED NAV_SAT_FIX_PUB, +#endif // AP_DDS_NAVSATFIX_PUB_ENABLED +#if AP_DDS_STATIC_TF_PUB_ENABLED STATIC_TRANSFORMS_PUB, +#endif // AP_DDS_STATIC_TF_PUB_ENABLED +#if AP_DDS_BATTERY_STATE_PUB_ENABLED BATTERY_STATE_PUB, +#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED #if AP_DDS_IMU_PUB_ENABLED IMU_PUB, #endif //AP_DDS_IMU_PUB_ENABLED +#if AP_DDS_LOCAL_POSE_PUB_ENABLED LOCAL_POSE_PUB, +#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED +#if AP_DDS_LOCAL_VEL_PUB_ENABLED LOCAL_VELOCITY_PUB, +#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED +#if AP_DDS_GEOPOSE_PUB_ENABLED GEOPOSE_PUB, +#endif // AP_DDS_GEOPOSE_PUB_ENABLED +#if AP_DDS_CLOCK_PUB_ENABLED CLOCK_PUB, +#endif // AP_DDS_CLOCK_PUB_ENABLED +#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED GPS_GLOBAL_ORIGIN_PUB, +#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_JOY_SUB_ENABLED JOY_SUB, +#endif // AP_DDS_JOY_SUB_ENABLED +#if AP_DDS_DYNAMIC_TF_SUB DYNAMIC_TRANSFORMS_SUB, +#endif // AP_DDS_DYNAMIC_TF_SUB +#if AP_DDS_VEL_CTRL_ENABLED VELOCITY_CONTROL_SUB, +#endif // AP_DDS_VEL_CTRL_ENABLED +#if AP_DDS_GLOBAL_POS_CTRL_ENABLED GLOBAL_POSITION_SUB, +#endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED }; static inline constexpr uint8_t to_underlying(const TopicIndex index) @@ -40,6 +66,7 @@ static inline constexpr uint8_t to_underlying(const TopicIndex index) constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { +#if AP_DDS_TIME_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::TIME_PUB), .pub_id = to_underlying(TopicIndex::TIME_PUB), @@ -56,6 +83,8 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 20, }, }, +#endif // AP_DDS_TIME_PUB_ENABLED +#if AP_DDS_NAVSATFIX_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::NAV_SAT_FIX_PUB), .pub_id = to_underlying(TopicIndex::NAV_SAT_FIX_PUB), @@ -72,6 +101,8 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, +#endif // AP_DDS_NAVSATFIX_PUB_ENABLED +#if AP_DDS_STATIC_TF_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), .pub_id = to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), @@ -88,6 +119,8 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 1, }, }, +#endif // AP_DDS_STATIC_TF_PUB_ENABLED +#if AP_DDS_BATTERY_STATE_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::BATTERY_STATE_PUB), .pub_id = to_underlying(TopicIndex::BATTERY_STATE_PUB), @@ -104,6 +137,7 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, +#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED #if AP_DDS_IMU_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::IMU_PUB), @@ -122,6 +156,7 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { }, }, #endif //AP_DDS_IMU_PUB_ENABLED +#if AP_DDS_LOCAL_POSE_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::LOCAL_POSE_PUB), .pub_id = to_underlying(TopicIndex::LOCAL_POSE_PUB), @@ -138,6 +173,8 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, +#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED +#if AP_DDS_LOCAL_VEL_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), .pub_id = to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), @@ -154,6 +191,8 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, +#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED +#if AP_DDS_GEOPOSE_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::GEOPOSE_PUB), .pub_id = to_underlying(TopicIndex::GEOPOSE_PUB), @@ -170,6 +209,8 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, +#endif // AP_DDS_GEOPOSE_PUB_ENABLED +#if AP_DDS_CLOCK_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::CLOCK_PUB), .pub_id = to_underlying(TopicIndex::CLOCK_PUB), @@ -186,6 +227,8 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 20, }, }, +#endif // AP_DDS_CLOCK_PUB_ENABLED +#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB), .pub_id = to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB), @@ -202,6 +245,8 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, +#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_JOY_SUB_ENABLED { .topic_id = to_underlying(TopicIndex::JOY_SUB), .pub_id = to_underlying(TopicIndex::JOY_SUB), @@ -218,6 +263,8 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, +#endif // AP_DDS_JOY_SUB_ENABLED +#if AP_DDS_DYNAMIC_TF_SUB { .topic_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), .pub_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), @@ -234,6 +281,8 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, +#endif // AP_DDS_DYNAMIC_TF_SUB +#if AP_DDS_VEL_CTRL_ENABLED { .topic_id = to_underlying(TopicIndex::VELOCITY_CONTROL_SUB), .pub_id = to_underlying(TopicIndex::VELOCITY_CONTROL_SUB), @@ -250,6 +299,8 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, +#endif // AP_DDS_VEL_CTRL_ENABLED +#if AP_DDS_GLOBAL_POS_CTRL_ENABLED { .topic_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .pub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB), @@ -266,4 +317,5 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, +#endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED }; diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index 6d46c94694..01f168ea21 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -26,6 +26,64 @@ #define AP_DDS_IMU_PUB_ENABLED AP_DDS_EXPERIMENTAL_ENABLED #endif +#ifndef AP_DDS_TIME_PUB_ENABLED +#define AP_DDS_TIME_PUB_ENABLED 1 +#endif + +#ifndef AP_DDS_NAVSATFIX_PUB_ENABLED +#define AP_DDS_NAVSATFIX_PUB_ENABLED 1 +#endif + +#ifndef AP_DDS_STATIC_TF_PUB_ENABLED +#define AP_DDS_STATIC_TF_PUB_ENABLED 1 +#endif + +#ifndef AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#define AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED 1 +#endif + +#ifndef AP_DDS_GEOPOSE_PUB_ENABLED +#define AP_DDS_GEOPOSE_PUB_ENABLED 1 +#endif + +#ifndef AP_DDS_LOCAL_POSE_PUB_ENABLED +#define AP_DDS_LOCAL_POSE_PUB_ENABLED 1 +#endif + +#ifndef AP_DDS_LOCAL_VEL_PUB_ENABLED +#define AP_DDS_LOCAL_VEL_PUB_ENABLED 1 +#endif + +#ifndef AP_DDS_BATTERY_STATE_PUB_ENABLED +#define AP_DDS_BATTERY_STATE_PUB_ENABLED 1 +#endif + +#ifndef AP_DDS_CLOCK_PUB_ENABLED +#define AP_DDS_CLOCK_PUB_ENABLED 1 +#endif + +#ifndef AP_DDS_JOY_SUB_ENABLED +#define AP_DDS_JOY_SUB_ENABLED 1 +#endif + +#ifndef AP_DDS_VEL_CTRL_ENABLED +#define AP_DDS_VEL_CTRL_ENABLED 1 +#endif + +#ifndef AP_DDS_GLOBAL_POS_CTRL_ENABLED +#define AP_DDS_GLOBAL_POS_CTRL_ENABLED 1 +#endif + +#ifndef AP_DDS_DYNAMIC_TF_SUB +#define AP_DDS_DYNAMIC_TF_SUB 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 + #ifndef AP_DDS_DEFAULT_UDP_IP_ADDR #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #define AP_DDS_DEFAULT_UDP_IP_ADDR "192.168.13.2"