Skip to content

Commit

Permalink
added conditioning to have rolling tags compilable in older versions (
Browse files Browse the repository at this point in the history
  • Loading branch information
saikishor authored Mar 4, 2024
1 parent 61617b3 commit 0b43291
Show file tree
Hide file tree
Showing 10 changed files with 63 additions and 7 deletions.
10 changes: 10 additions & 0 deletions diff_drive_controller/include/diff_drive_controller/odometry.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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);
Expand Down
1 change: 0 additions & 1 deletion joint_state_broadcaster/src/joint_state_broadcaster.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
14 changes: 14 additions & 0 deletions pid_controller/src/pid_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
3 changes: 3 additions & 0 deletions range_sensor_broadcaster/src/range_sensor_broadcaster.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down
10 changes: 10 additions & 0 deletions range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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);
Expand All @@ -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)
Expand All @@ -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);
Expand All @@ -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)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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_;

Expand Down Expand Up @@ -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

Expand Down
4 changes: 2 additions & 2 deletions steering_controllers_library/src/steering_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
10 changes: 10 additions & 0 deletions tricycle_controller/include/tricycle_controller/odometry.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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);
Expand Down

0 comments on commit 0b43291

Please sign in to comment.