AP_DDS: add parameters for topic publish rates

Adds DDS_RATE_* parameters so users can configure ROS 2 topic publication
rates without recompiling. Defaults are taken from the existing
AP_DDS_DELAY_*_TOPIC_MS defines so existing behaviour is preserved.

Setting a rate to 0 disables that topic.

Fixes ArduPilot#32960

Author: ja51d

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -55,70 +55,15 @@
 
 // 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 = AP_DDS_DELAY_TIME_TOPIC_MS;
-#endif // AP_DDS_TIME_PUB_ENABLED
-#if AP_DDS_BATTERY_STATE_PUB_ENABLED
-static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = AP_DDS_DELAY_BATTERY_STATE_TOPIC_MS;
-#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED
-#if AP_DDS_IMU_PUB_ENABLED
-static constexpr uint16_t DELAY_IMU_TOPIC_MS = AP_DDS_DELAY_IMU_TOPIC_MS;
-#endif // AP_DDS_IMU_PUB_ENABLED
-#if AP_DDS_LOCAL_POSE_PUB_ENABLED
-static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = AP_DDS_DELAY_LOCAL_POSE_TOPIC_MS;
-#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED
-#if AP_DDS_LOCAL_VEL_PUB_ENABLED
-static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = AP_DDS_DELAY_LOCAL_VELOCITY_TOPIC_MS;
-#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
-#if AP_DDS_AIRSPEED_PUB_ENABLED
-static constexpr uint16_t DELAY_AIRSPEED_TOPIC_MS = AP_DDS_DELAY_AIRSPEED_TOPIC_MS;
-#endif // AP_DDS_AIRSPEED_PUB_ENABLED
-#if AP_DDS_RC_PUB_ENABLED
-static constexpr uint16_t DELAY_RC_TOPIC_MS = AP_DDS_DELAY_RC_TOPIC_MS;
-#endif // AP_DDS_RC_PUB_ENABLED
-#if AP_DDS_GEOPOSE_PUB_ENABLED
-static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = AP_DDS_DELAY_GEO_POSE_TOPIC_MS;
-#endif // AP_DDS_GEOPOSE_PUB_ENABLED
-#if AP_DDS_GOAL_PUB_ENABLED
-static constexpr uint16_t DELAY_GOAL_TOPIC_MS = AP_DDS_DELAY_GOAL_TOPIC_MS ;
-#endif // AP_DDS_GOAL_PUB_ENABLED
-#if AP_DDS_CLOCK_PUB_ENABLED
-static constexpr uint16_t DELAY_CLOCK_TOPIC_MS =AP_DDS_DELAY_CLOCK_TOPIC_MS;
-#endif // AP_DDS_CLOCK_PUB_ENABLED
-#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
-static constexpr uint16_t DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS = AP_DDS_DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS;
-#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
-static constexpr uint16_t DELAY_PING_MS = 500;
-#if AP_DDS_STATUS_PUB_ENABLED
-static constexpr uint16_t DELAY_STATUS_TOPIC_MS = AP_DDS_DELAY_STATUS_TOPIC_MS;
-#endif // AP_DDS_STATUS_PUB_ENABLED
-
-// 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
-#if AP_DDS_DYNAMIC_TF_SUB_ENABLED
-tf2_msgs_msg_TFMessage AP_DDS_Client::rx_dynamic_transforms_topic {};
-#endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED
-#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
-
-// Define the parameter server data members, which are static class scope.
-// If these are created on the stack, then the AP_DDS_Client::on_request
-// frame size is exceeded.
-#if AP_DDS_PARAMETER_SERVER_ENABLED
-rcl_interfaces_srv_SetParameters_Request AP_DDS_Client::set_parameter_request {};
-rcl_interfaces_srv_SetParameters_Response AP_DDS_Client::set_parameter_response {};
-rcl_interfaces_srv_GetParameters_Request AP_DDS_Client::get_parameters_request {};
-rcl_interfaces_srv_GetParameters_Response AP_DDS_Client::get_parameters_response {};
-rcl_interfaces_msg_Parameter AP_DDS_Client::param {};
-#endif
+// returns publish period in ms for the given rate (Hz).
+// rate <= 0 returns UINT32_MAX so the publish-due check never triggers.
+static uint32_t rate_hz_to_period_ms(int16_t rate_hz)
+{
+    if (rate_hz <= 0) {
+        return UINT32_MAX;
+    }
+    return 1000U / uint16_t(rate_hz);
+}
 
 const AP_Param::GroupInfo AP_DDS_Client::var_info[] {
 
@@ -180,6 +125,126 @@ const AP_Param::GroupInfo AP_DDS_Client::var_info[] {
     // @User: Standard
     AP_GROUPINFO("_USE_NS", 7, AP_DDS_Client, use_ns, 0),
 
+#if AP_DDS_TIME_PUB_ENABLED
+    // @Param: _RATE_TIME
+    // @DisplayName: DDS time topic publish rate
+    // @Description: Publish rate for the time topic. 0 disables it.
+    // @Units: Hz
+    // @Range: 0 1000
+    // @User: Advanced
+    AP_GROUPINFO("_RATE_TIME", 8, AP_DDS_Client, rate_time_hz, 1000 / AP_DDS_DELAY_TIME_TOPIC_MS),
+#endif
+
+#if AP_DDS_BATTERY_STATE_PUB_ENABLED
+    // @Param: _RATE_BATT
+    // @DisplayName: DDS battery_state topic publish rate
+    // @Description: Publish rate for the battery_state topic. 0 disables it.
+    // @Units: Hz
+    // @Range: 0 1000
+    // @User: Advanced
+    AP_GROUPINFO("_RATE_BATT", 9, AP_DDS_Client, rate_battery_hz, 1000 / AP_DDS_DELAY_BATTERY_STATE_TOPIC_MS),
+#endif
+
+#if AP_DDS_LOCAL_POSE_PUB_ENABLED
+    // @Param: _RATE_LPOSE
+    // @DisplayName: DDS local pose topic publish rate
+    // @Description: Publish rate for the local pose topic. 0 disables it.
+    // @Units: Hz
+    // @Range: 0 1000
+    // @User: Advanced
+    AP_GROUPINFO("_RATE_LPOSE", 10, AP_DDS_Client, rate_local_pose_hz, 1000 / AP_DDS_DELAY_LOCAL_POSE_TOPIC_MS),
+#endif
+
+#if AP_DDS_LOCAL_VEL_PUB_ENABLED
+    // @Param: _RATE_LVEL
+    // @DisplayName: DDS local velocity topic publish rate
+    // @Description: Publish rate for the local velocity topic. 0 disables it.
+    // @Units: Hz
+    // @Range: 0 1000
+    // @User: Advanced
+    AP_GROUPINFO("_RATE_LVEL", 11, AP_DDS_Client, rate_local_vel_hz, 1000 / AP_DDS_DELAY_LOCAL_VELOCITY_TOPIC_MS),
+#endif
+
+#if AP_DDS_AIRSPEED_PUB_ENABLED
+    // @Param: _RATE_AIRSP
+    // @DisplayName: DDS airspeed topic publish rate
+    // @Description: Publish rate for the airspeed topic. 0 disables it.
+    // @Units: Hz
+    // @Range: 0 1000
+    // @User: Advanced
+    AP_GROUPINFO("_RATE_AIRSP", 12, AP_DDS_Client, rate_airspeed_hz, 1000 / AP_DDS_DELAY_AIRSPEED_TOPIC_MS),
+#endif
+
+#if AP_DDS_RC_PUB_ENABLED
+    // @Param: _RATE_RC
+    // @DisplayName: DDS rc topic publish rate
+    // @Description: Publish rate for the rc topic. 0 disables it.
+    // @Units: Hz
+    // @Range: 0 1000
+    // @User: Advanced
+    AP_GROUPINFO("_RATE_RC", 13, AP_DDS_Client, rate_rc_hz, 1000 / AP_DDS_DELAY_RC_TOPIC_MS),
+#endif
+
+#if AP_DDS_IMU_PUB_ENABLED
+    // @Param: _RATE_IMU
+    // @DisplayName: DDS imu topic publish rate
+    // @Description: Publish rate for the imu topic. 0 disables it.
+    // @Units: Hz
+    // @Range: 0 1000
+    // @User: Advanced
+    AP_GROUPINFO("_RATE_IMU", 14, AP_DDS_Client, rate_imu_hz, 1000 / AP_DDS_DELAY_IMU_TOPIC_MS),
+#endif
+
+#if AP_DDS_GEOPOSE_PUB_ENABLED
+    // @Param: _RATE_GPOSE
+    // @DisplayName: DDS geo_pose topic publish rate
+    // @Description: Publish rate for the geo_pose topic. 0 disables it.
+    // @Units: Hz
+    // @Range: 0 1000
+    // @User: Advanced
+    AP_GROUPINFO("_RATE_GPOSE", 15, AP_DDS_Client, rate_geo_pose_hz, 1000 / AP_DDS_DELAY_GEO_POSE_TOPIC_MS),
+#endif
+
+#if AP_DDS_CLOCK_PUB_ENABLED
+    // @Param: _RATE_CLOCK
+    // @DisplayName: DDS clock topic publish rate
+    // @Description: Publish rate for the clock topic. 0 disables it.
+    // @Units: Hz
+    // @Range: 0 1000
+    // @User: Advanced
+    AP_GROUPINFO("_RATE_CLOCK", 16, AP_DDS_Client, rate_clock_hz, 1000 / AP_DDS_DELAY_CLOCK_TOPIC_MS),
+#endif
+
+#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
+    // @Param: _RATE_GPSOR
+    // @DisplayName: DDS gps_global_origin topic publish rate
+    // @Description: Publish rate for the gps_global_origin topic. 0 disables it.
+    // @Units: Hz
+    // @Range: 0 1000
+    // @User: Advanced
+    AP_GROUPINFO("_RATE_GPSOR", 17, AP_DDS_Client, rate_gps_origin_hz, 1000 / AP_DDS_DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS),
+#endif
+
+#if AP_DDS_GOAL_PUB_ENABLED
+    // @Param: _RATE_GOAL
+    // @DisplayName: DDS goal topic publish rate
+    // @Description: Publish rate for the goal topic. 0 disables it.
+    // @Units: Hz
+    // @Range: 0 1000
+    // @User: Advanced
+    AP_GROUPINFO("_RATE_GOAL", 18, AP_DDS_Client, rate_goal_hz, 1000 / AP_DDS_DELAY_GOAL_TOPIC_MS),
+#endif
+
+#if AP_DDS_STATUS_PUB_ENABLED
+    // @Param: _RATE_STAT
+    // @DisplayName: DDS status topic publish rate
+    // @Description: Publish rate for the status topic. 0 disables it.
+    // @Units: Hz
+    // @Range: 0 1000
+    // @User: Advanced
+    AP_GROUPINFO("_RATE_STAT", 19, AP_DDS_Client, rate_status_hz, 1000 / AP_DDS_DELAY_STATUS_TOPIC_MS),
+#endif
+
     AP_GROUPEND
 };
 
@@ -763,7 +828,7 @@ bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Status& msg)
         last_status_publish_time_ms = timestamp;
         update_topic(msg.header.stamp);
         return true;
-    } else if (timestamp - last_status_publish_time_ms > DELAY_STATUS_TOPIC_MS * 5) {
+    } else if (timestamp - last_status_publish_time_ms > rate_hz_to_period_ms(rate_status_hz) * 5) {
         // Publish the status message at 2Hz even if no change is detected.
         last_status_publish_time_ms = timestamp;
         update_topic(msg.header.stamp);
@@ -1812,7 +1877,7 @@ void AP_DDS_Client::update()
     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) {
+    if (cur_time_ms - last_time_time_ms > rate_hz_to_period_ms(rate_time_hz)) {
         update_topic(time_topic);
         last_time_time_ms = cur_time_ms;
         write_time_topic();
@@ -1826,7 +1891,7 @@ void AP_DDS_Client::update()
     }
 #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) {
+    if (cur_time_ms - last_battery_state_time_ms > rate_hz_to_period_ms(rate_battery_hz)) {
         for (uint8_t battery_instance = 0; battery_instance < AP_BATT_MONITOR_MAX_INSTANCES; battery_instance++) {
             update_topic(battery_state_topic, battery_instance);
             if (battery_state_topic.present) {
@@ -1837,73 +1902,73 @@ void AP_DDS_Client::update()
     }
 #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) {
+    if (cur_time_ms - last_local_pose_time_ms > rate_hz_to_period_ms(rate_local_pose_hz)) {
         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) {
+    if (cur_time_ms - last_local_velocity_time_ms > rate_hz_to_period_ms(rate_local_vel_hz)) {
         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_AIRSPEED_PUB_ENABLED
-    if (cur_time_ms - last_airspeed_time_ms > DELAY_AIRSPEED_TOPIC_MS) {
+    if (cur_time_ms - last_airspeed_time_ms > rate_hz_to_period_ms(rate_airspeed_hz)) {
         last_airspeed_time_ms = cur_time_ms;
         if (update_topic(tx_local_airspeed_topic)) {
             write_tx_local_airspeed_topic();
         }
     }
 #endif // AP_DDS_AIRSPEED_PUB_ENABLED
 #if AP_DDS_RC_PUB_ENABLED
-    if (cur_time_ms - last_rc_time_ms > DELAY_RC_TOPIC_MS) {
+    if (cur_time_ms - last_rc_time_ms > rate_hz_to_period_ms(rate_rc_hz)) {
         last_rc_time_ms = cur_time_ms;
         if (update_topic(tx_local_rc_topic)) {
             write_tx_local_rc_topic();
         }
     }
 #endif // AP_DDS_RC_PUB_ENABLED
 #if AP_DDS_IMU_PUB_ENABLED
-    if (cur_time_ms - last_imu_time_ms > DELAY_IMU_TOPIC_MS) {
+    if (cur_time_ms - last_imu_time_ms > rate_hz_to_period_ms(rate_imu_hz)) {
         update_topic(imu_topic);
         last_imu_time_ms = cur_time_ms;
         write_imu_topic();
     }
 #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) {
+    if (cur_time_ms - last_geo_pose_time_ms > rate_hz_to_period_ms(rate_geo_pose_hz)) {
         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) {
+    if (cur_time_ms - last_clock_time_ms > rate_hz_to_period_ms(rate_clock_hz)) {
         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) {
+    if (cur_time_ms - last_gps_global_origin_time_ms > rate_hz_to_period_ms(rate_gps_origin_hz)) {
         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
 #if AP_DDS_GOAL_PUB_ENABLED
-    if (cur_time_ms - last_goal_time_ms > DELAY_GOAL_TOPIC_MS) {
+    if (cur_time_ms - last_goal_time_ms > rate_hz_to_period_ms(rate_goal_hz)) {
         if (update_topic_goal(goal_topic)) {
             write_goal_topic();
         }
         last_goal_time_ms = cur_time_ms;
     }
 #endif // AP_DDS_GOAL_PUB_ENABLED
 #if AP_DDS_STATUS_PUB_ENABLED
-    if (cur_time_ms - last_status_check_time_ms > DELAY_STATUS_TOPIC_MS) {
+    if (cur_time_ms - last_status_check_time_ms > rate_hz_to_period_ms(rate_status_hz)) {
         if (update_topic(status_topic)) {
             write_status_topic();
         }

libraries/AP_DDS/AP_DDS_Client.h

@@ -359,6 +359,44 @@ class AP_DDS_Client
     //! @brief Use namespace when enabled
     AP_Int8 use_ns;
 
+    //! @brief Publish rate (Hz) per topic. 0 disables a topic.
+#if AP_DDS_TIME_PUB_ENABLED
+    AP_Int16 rate_time_hz;
+#endif
+#if AP_DDS_BATTERY_STATE_PUB_ENABLED
+    AP_Int16 rate_battery_hz;
+#endif
+#if AP_DDS_LOCAL_POSE_PUB_ENABLED
+    AP_Int16 rate_local_pose_hz;
+#endif
+#if AP_DDS_LOCAL_VEL_PUB_ENABLED
+    AP_Int16 rate_local_vel_hz;
+#endif
+#if AP_DDS_AIRSPEED_PUB_ENABLED
+    AP_Int16 rate_airspeed_hz;
+#endif
+#if AP_DDS_RC_PUB_ENABLED
+    AP_Int16 rate_rc_hz;
+#endif
+#if AP_DDS_IMU_PUB_ENABLED
+    AP_Int16 rate_imu_hz;
+#endif
+#if AP_DDS_GEOPOSE_PUB_ENABLED
+    AP_Int16 rate_geo_pose_hz;
+#endif
+#if AP_DDS_CLOCK_PUB_ENABLED
+    AP_Int16 rate_clock_hz;
+#endif
+#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
+    AP_Int16 rate_gps_origin_hz;
+#endif
+#if AP_DDS_GOAL_PUB_ENABLED
+    AP_Int16 rate_goal_hz;
+#endif
+#if AP_DDS_STATUS_PUB_ENABLED
+    AP_Int16 rate_status_hz;
+#endif
+
     //! @brief Enum used to mark a topic as a data reader or writer
     enum class Topic_rw : uint8_t {
         DataReader = 0,