added conditioning to have rolling tags compilable in older versions (…

…#1071)
ros-controls · Mar 4, 2024 · 0b43291 · 0b43291
1 parent 61617b3
commit 0b43291
Show file tree

Hide file tree

Showing 10 changed files with 63 additions and 7 deletions.
diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp
@@ -25,7 +25,12 @@
 #include <cmath>
 
 #include "rclcpp/time.hpp"
+// \note The versions conditioning is added here to support the source-compatibility with Humble
+#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6
 #include "rcpputils/rolling_mean_accumulator.hpp"
+#else
+#include "rcppmath/rolling_mean_accumulator.hpp"
+#endif
 
 namespace diff_drive_controller
 {
@@ -50,7 +55,12 @@ class Odometry
   void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
 
 private:
+// \note The versions conditioning is added here to support the source-compatibility with Humble
+#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6
   using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator<double>;
+#else
+  using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
+#endif
 
   void integrateRungeKutta2(double linear, double angular);
   void integrateExact(double linear, double angular);

diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp
@@ -24,7 +24,6 @@
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/clock.hpp"
-#include "rclcpp/event_handler.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"

diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -29,7 +29,6 @@
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "joint_trajectory_controller/trajectory.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
-#include "rclcpp/event_handler.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/qos.hpp"
 #include "rclcpp/time.hpp"

diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -32,7 +32,6 @@
 #include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/clock.hpp"
 #include "rclcpp/duration.hpp"
-#include "rclcpp/event_handler.hpp"
 #include "rclcpp/executors/multi_threaded_executor.hpp"
 #include "rclcpp/executors/single_threaded_executor.hpp"
 #include "rclcpp/node.hpp"

diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp
@@ -30,10 +30,24 @@ namespace
 {  // utility
 
 // Changed services history QoS to keep all so we don't lose any client service calls
+// \note The versions conditioning is added here to support the source-compatibility with Humble
+#if RCLCPP_VERSION_MAJOR >= 17
 rclcpp::QoS qos_services =
   rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1))
     .reliable()
     .durability_volatile();
+#else
+static const rmw_qos_profile_t qos_services = {
+  RMW_QOS_POLICY_HISTORY_KEEP_ALL,
+  1,  // message queue depth
+  RMW_QOS_POLICY_RELIABILITY_RELIABLE,
+  RMW_QOS_POLICY_DURABILITY_VOLATILE,
+  RMW_QOS_DEADLINE_DEFAULT,
+  RMW_QOS_LIFESPAN_DEFAULT,
+  RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
+  RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
+  false};
+#endif
 
 using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg;
 

diff --git a/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp b/range_sensor_broadcaster/src/range_sensor_broadcaster.cpp
@@ -79,7 +79,10 @@ controller_interface::CallbackReturn RangeSensorBroadcaster::on_configure(
   realtime_publisher_->msg_.field_of_view = params_.field_of_view;
   realtime_publisher_->msg_.min_range = params_.min_range;
   realtime_publisher_->msg_.max_range = params_.max_range;
+// \note The versions conditioning is added here to support the source-compatibility with Humble
+#if SENSOR_MSGS_VERSION_MAJOR >= 5
   realtime_publisher_->msg_.variance = params_.variance;
+#endif
   realtime_publisher_->unlock();
 
   RCLCPP_DEBUG(get_node()->get_logger(), "configure successful");

diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp
@@ -203,7 +203,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_RangeBroadcaster_Success)
   EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_));
   EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_));
   EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_));
+#if SENSOR_MSGS_VERSION_MAJOR >= 5
   EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_));
+#endif
 }
 
 TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success)
@@ -224,7 +226,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success)
   EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_));
   EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_));
   EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_));
+#if SENSOR_MSGS_VERSION_MAJOR >= 5
   EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_));
+#endif
 
   sensor_range_ = 4.0;
   subscribe_and_get_message(range_msg);
@@ -235,7 +239,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success)
   EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_));
   EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_));
   EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_));
+#if SENSOR_MSGS_VERSION_MAJOR >= 5
   EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_));
+#endif
 }
 
 TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Success)
@@ -257,7 +263,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe
   EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_));
   EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_));
   EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_));
+#if SENSOR_MSGS_VERSION_MAJOR >= 5
   EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_));
+#endif
 
   sensor_range_ = 6.0;
   subscribe_and_get_message(range_msg);
@@ -269,7 +277,9 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe
   EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_));
   EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_));
   EXPECT_THAT(range_msg.max_range, ::testing::FloatEq(max_range_));
+#if SENSOR_MSGS_VERSION_MAJOR >= 5
   EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_));
+#endif
 }
 
 int main(int argc, char ** argv)

diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp
@@ -24,7 +24,12 @@
 #include "realtime_tools/realtime_buffer.h"
 #include "realtime_tools/realtime_publisher.h"
 
+// \note The versions conditioning is added here to support the source-compatibility with Humble
+#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6
 #include "rcpputils/rolling_mean_accumulator.hpp"
+#else
+#include "rcppmath/rolling_mean_accumulator.hpp"
+#endif
 
 namespace steering_odometry
 {
@@ -229,6 +234,13 @@ class SteeringOdometry
    */
   void reset_accumulators();
 
+// \note The versions conditioning is added here to support the source-compatibility with Humble
+#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6
+  using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator<double>;
+#else
+  using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
+#endif
+
   /// Current timestamp:
   rclcpp::Time timestamp_;
 
@@ -256,8 +268,8 @@ class SteeringOdometry
   double traction_left_wheel_old_pos_;
   /// Rolling mean accumulators for the linear and angular velocities:
   size_t velocity_rolling_window_size_;
-  rcpputils::RollingMeanAccumulator<double> linear_acc_;
-  rcpputils::RollingMeanAccumulator<double> angular_acc_;
+  RollingMeanAccumulator linear_acc_;
+  RollingMeanAccumulator angular_acc_;
 };
 }  // namespace steering_odometry
 

diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp
@@ -324,8 +324,8 @@ void SteeringOdometry::integrate_exact(double linear, double angular)
 
 void SteeringOdometry::reset_accumulators()
 {
-  linear_acc_ = rcpputils::RollingMeanAccumulator<double>(velocity_rolling_window_size_);
-  angular_acc_ = rcpputils::RollingMeanAccumulator<double>(velocity_rolling_window_size_);
+  linear_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_);
+  angular_acc_ = RollingMeanAccumulator(velocity_rolling_window_size_);
 }
 
 }  // namespace steering_odometry
diff --git a/tricycle_controller/include/tricycle_controller/odometry.hpp b/tricycle_controller/include/tricycle_controller/odometry.hpp
@@ -22,7 +22,12 @@
 #include <cmath>
 
 #include "rclcpp/time.hpp"
+// \note The versions conditioning is added here to support the source-compatibility with Humble
+#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6
 #include "rcpputils/rolling_mean_accumulator.hpp"
+#else
+#include "rcppmath/rolling_mean_accumulator.hpp"
+#endif
 
 namespace tricycle_controller
 {
@@ -45,7 +50,12 @@ class Odometry
   void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
 
 private:
+// \note The versions conditioning is added here to support the source-compatibility with Humble
+#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6
   using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator<double>;
+#else
+  using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
+#endif
 
   void integrateRungeKutta2(double linear, double angular);
   void integrateExact(double linear, double angular);