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 <ryanfriedman5410+github@gmail.com>

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);

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)

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),

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"