From dcfbf0ab690646039c76d4979ab7a2c47465b325 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 13 Sep 2024 17:02:38 -0600 Subject: [PATCH] AP_DDS: Add defines for experimental topics such as IMU * Experimental topics, such as IMU, should have an easy way to be disabled at compile time * This demonstrates a pattern to add in new topics at compile time Signed-off-by: Ryan Friedman --- libraries/AP_DDS/AP_DDS_Client.cpp | 9 ++++++++- libraries/AP_DDS/AP_DDS_Client.h | 10 ++++++++++ libraries/AP_DDS/AP_DDS_Topic_Table.h | 6 ++++++ libraries/AP_DDS/AP_DDS_config.h | 9 +++++++++ 4 files changed, 33 insertions(+), 1 deletion(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 41d71b35a2..77f052de62 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -32,7 +32,9 @@ static constexpr uint8_t ENABLED_BY_DEFAULT = 1; static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10; static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000; +#if AP_DDS_IMU_PUB_ENABLED static constexpr uint16_t DELAY_IMU_TOPIC_MS = 5; +#endif // AP_DDS_IMU_PUB_ENABLED static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33; static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = 33; static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33; @@ -440,6 +442,7 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) } } +#if AP_DDS_IMU_PUB_ENABLED void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg) { update_topic(msg.header.stamp); @@ -477,6 +480,7 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg) msg.angular_velocity_covariance[0] = -1; msg.linear_acceleration_covariance[0] = -1; } +#endif // AP_DDS_IMU_PUB_ENABLED void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg) { @@ -1039,6 +1043,7 @@ void AP_DDS_Client::write_tx_local_velocity_topic() } } +#if AP_DDS_IMU_PUB_ENABLED void AP_DDS_Client::write_imu_topic() { WITH_SEMAPHORE(csem); @@ -1053,6 +1058,7 @@ void AP_DDS_Client::write_imu_topic() } } } +#endif // AP_DDS_IMU_PUB_ENABLED void AP_DDS_Client::write_geo_pose_topic() { @@ -1132,12 +1138,13 @@ void AP_DDS_Client::update() last_local_velocity_time_ms = cur_time_ms; write_tx_local_velocity_topic(); } - +#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 if (cur_time_ms - last_geo_pose_time_ms > DELAY_GEO_POSE_TOPIC_MS) { update_topic(geo_pose_topic); diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index b06a61a6b9..daddfff968 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -13,7 +13,9 @@ #include "sensor_msgs/msg/NavSatFix.h" #include "tf2_msgs/msg/TFMessage.h" #include "sensor_msgs/msg/BatteryState.h" +#if AP_DDS_IMU_PUB_ENABLED #include "sensor_msgs/msg/Imu.h" +#endif // AP_DDS_IMU_PUB_ENABLED #include "sensor_msgs/msg/Joy.h" #include "geometry_msgs/msg/PoseStamped.h" #include "geometry_msgs/msg/TwistStamped.h" @@ -65,7 +67,9 @@ private: geometry_msgs_msg_TwistStamped tx_local_velocity_topic; sensor_msgs_msg_BatteryState battery_state_topic; sensor_msgs_msg_NavSatFix nav_sat_fix_topic; +#if AP_DDS_IMU_PUB_ENABLED sensor_msgs_msg_Imu imu_topic; +#endif // AP_DDS_IMU_PUB_ENABLED rosgraph_msgs_msg_Clock clock_topic; // incoming joystick data static sensor_msgs_msg_Joy rx_joy_topic; @@ -91,7 +95,9 @@ private: 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); @@ -117,8 +123,10 @@ private: 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 @@ -197,8 +205,10 @@ public: 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) diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index ba08453af3..2329dde701 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -3,7 +3,9 @@ #include "tf2_msgs/msg/TFMessage.h" #include "sensor_msgs/msg/BatteryState.h" #include "geographic_msgs/msg/GeoPoseStamped.h" +#if AP_DDS_IMU_PUB_ENABLED #include "sensor_msgs/msg/Imu.h" +#endif //AP_DDS_IMU_PUB_ENABLED #include "uxr/client/client.h" @@ -16,7 +18,9 @@ enum class TopicIndex: uint8_t { NAV_SAT_FIX_PUB, STATIC_TRANSFORMS_PUB, BATTERY_STATE_PUB, +#if AP_DDS_IMU_PUB_ENABLED IMU_PUB, +#endif //AP_DDS_IMU_PUB_ENABLED LOCAL_POSE_PUB, LOCAL_VELOCITY_PUB, GEOPOSE_PUB, @@ -100,6 +104,7 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, +#if AP_DDS_IMU_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::IMU_PUB), .pub_id = to_underlying(TopicIndex::IMU_PUB), @@ -116,6 +121,7 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, +#endif //AP_DDS_IMU_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::LOCAL_POSE_PUB), .pub_id = to_underlying(TopicIndex::LOCAL_POSE_PUB), diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index f1c71bbc87..6d46c94694 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -17,6 +17,15 @@ #define AP_DDS_VISUALODOM_ENABLED HAL_VISUALODOM_ENABLED && AP_DDS_ENABLED #endif +// Whether experimental interfaces are enabled. +#ifndef AP_DDS_EXPERIMENTAL_ENABLED +#define AP_DDS_EXPERIMENTAL_ENABLED 1 +#endif + +#ifndef AP_DDS_IMU_PUB_ENABLED +#define AP_DDS_IMU_PUB_ENABLED AP_DDS_EXPERIMENTAL_ENABLED +#endif + #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"