From 6e22d0232c385dc60783428803130e51ccaa3b34 Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Thu, 16 Apr 2026 13:16:56 -0400 Subject: [PATCH 1/6] Added velocity limiting to the mecanum controller. --- mecanum_drive_controller/CMakeLists.txt | 6 + .../mecanum_drive_controller.hpp | 8 + mecanum_drive_controller/package.xml | 1 + .../src/mecanum_drive_controller.cpp | 76 +++- .../src/mecanum_drive_controller.yaml | 186 +++++++++ .../test/mecanum_drive_controller_params.yaml | 38 ++ .../test/test_mecanum_drive_controller.cpp | 354 ++++++++++++++++++ .../test/test_mecanum_drive_controller.hpp | 5 + 8 files changed, 670 insertions(+), 4 deletions(-) diff --git a/mecanum_drive_controller/CMakeLists.txt b/mecanum_drive_controller/CMakeLists.txt index 2824878632..766cdc2ff0 100644 --- a/mecanum_drive_controller/CMakeLists.txt +++ b/mecanum_drive_controller/CMakeLists.txt @@ -8,6 +8,7 @@ export_windows_symbols() # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs + control_toolbox controller_interface hardware_interface generate_parameter_library @@ -27,8 +28,11 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +# get include dirs from control_toolbox for the custom validators +get_target_property(TB_INCLUDE_DIRS control_toolbox::rate_limiter_parameters INTERFACE_INCLUDE_DIRECTORIES) generate_parameter_library(mecanum_drive_controller_parameters src/mecanum_drive_controller.yaml + ${TB_INCLUDE_DIRS}/control_toolbox/custom_validators.hpp ) add_library( @@ -43,6 +47,8 @@ target_include_directories(mecanum_drive_controller PUBLIC "$") target_link_libraries(mecanum_drive_controller PUBLIC mecanum_drive_controller_parameters + control_toolbox::rate_limiter_parameters + control_toolbox::control_toolbox controller_interface::controller_interface hardware_interface::hardware_interface pluginlib::pluginlib diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 3a4c148bb5..fd586adf43 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -26,6 +26,7 @@ #include "control_msgs/msg/mecanum_drive_controller_state.hpp" #include "control_msgs/srv/set_odometry.hpp" +#include "control_toolbox/rate_limiter.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -158,6 +159,13 @@ class MecanumDriveController : public controller_interface::ChainableControllerI double velocity_in_center_frame_linear_x_; // [m/s] double velocity_in_center_frame_linear_y_; // [m/s] double velocity_in_center_frame_angular_z_; // [rad/s] + + // Speed limiters + std::unique_ptr> limiter_linear_x_; + std::unique_ptr> limiter_linear_y_; + std::unique_ptr> limiter_angular_z_; + // Previous two commands for jerk limiting: queue of [linear_x, linear_y, angular_z] + std::queue> previous_two_commands_; }; } // namespace mecanum_drive_controller diff --git a/mecanum_drive_controller/package.xml b/mecanum_drive_controller/package.xml index cd76c305d0..5f421c7931 100644 --- a/mecanum_drive_controller/package.xml +++ b/mecanum_drive_controller/package.xml @@ -27,6 +27,7 @@ backward_ros control_msgs + control_toolbox controller_interface geometry_msgs hardware_interface diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 2023e3b99a..2b22829b8c 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -267,6 +267,52 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully"); + // Configure speed limiters + try + { + limiter_linear_x_ = std::make_unique>( + params_.linear.x.min_velocity, params_.linear.x.max_velocity, + params_.linear.x.max_acceleration_reverse, params_.linear.x.max_acceleration, + params_.linear.x.max_deceleration, params_.linear.x.max_deceleration_reverse, + params_.linear.x.min_jerk, params_.linear.x.max_jerk); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to configure linear x speed limiter: %s", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + try + { + limiter_linear_y_ = std::make_unique>( + params_.linear.y.min_velocity, params_.linear.y.max_velocity, + params_.linear.y.max_acceleration_reverse, params_.linear.y.max_acceleration, + params_.linear.y.max_deceleration, params_.linear.y.max_deceleration_reverse, + params_.linear.y.min_jerk, params_.linear.y.max_jerk); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to configure linear y speed limiter: %s", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + try + { + limiter_angular_z_ = std::make_unique>( + params_.angular.z.min_velocity, params_.angular.z.max_velocity, + params_.angular.z.max_acceleration_reverse, params_.angular.z.max_acceleration, + params_.angular.z.max_deceleration, params_.angular.z.max_deceleration_reverse, + params_.angular.z.min_jerk, params_.angular.z.max_jerk); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to configure angular z speed limiter: %s", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // Initialize previous commands queue for jerk limiting + previous_two_commands_ = std::queue>(); + previous_two_commands_.push({{0.0, 0.0, 0.0}}); + previous_two_commands_.push({{0.0, 0.0, 0.0}}); + return controller_interface::CallbackReturn::SUCCESS; } @@ -498,6 +544,28 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma !std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]) && !std::isnan(reference_interfaces_[2])) { + // Apply speed limits before inverse kinematics + double linear_x_command = reference_interfaces_[0]; + double linear_y_command = reference_interfaces_[1]; + double angular_z_command = reference_interfaces_[2]; + + double & last_linear_x = previous_two_commands_.back()[0]; + double & second_to_last_linear_x = previous_two_commands_.front()[0]; + double & last_linear_y = previous_two_commands_.back()[1]; + double & second_to_last_linear_y = previous_two_commands_.front()[1]; + double & last_angular_z = previous_two_commands_.back()[2]; + double & second_to_last_angular_z = previous_two_commands_.front()[2]; + + limiter_linear_x_->limit( + linear_x_command, last_linear_x, second_to_last_linear_x, period.seconds()); + limiter_linear_y_->limit( + linear_y_command, last_linear_y, second_to_last_linear_y, period.seconds()); + limiter_angular_z_->limit( + angular_z_command, last_angular_z, second_to_last_angular_z, period.seconds()); + + previous_two_commands_.pop(); + previous_two_commands_.push({{linear_x_command, linear_y_command, angular_z_command}}); + tf2::Quaternion quaternion; quaternion.setRPY(0.0, 0.0, params_.kinematics.base_frame_offset.theta); /// \note The variables meaning: @@ -509,17 +577,17 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma tf2::Matrix3x3 rotation_from_base_to_center = tf2::Matrix3x3((quaternion)); tf2::Vector3 velocity_in_base_frame_w_r_t_center_frame_ = rotation_from_base_to_center * - tf2::Vector3(reference_interfaces_[0], reference_interfaces_[1], 0.0); + tf2::Vector3(linear_x_command, linear_y_command, 0.0); tf2::Vector3 linear_trans_from_base_to_center = tf2::Vector3( params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, 0.0); velocity_in_center_frame_linear_x_ = velocity_in_base_frame_w_r_t_center_frame_.x() + - linear_trans_from_base_to_center.y() * reference_interfaces_[2]; + linear_trans_from_base_to_center.y() * angular_z_command; velocity_in_center_frame_linear_y_ = velocity_in_base_frame_w_r_t_center_frame_.y() - - linear_trans_from_base_to_center.x() * reference_interfaces_[2]; - velocity_in_center_frame_angular_z_ = reference_interfaces_[2]; + linear_trans_from_base_to_center.x() * angular_z_command; + velocity_in_center_frame_angular_z_ = angular_z_command; const double wheel_front_left_vel = 1.0 / params_.kinematics.wheels_radius * diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index f789cdb19e..f9cc431e99 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -155,3 +155,189 @@ mecanum_drive_controller: description: "Diagonal values of pose covariance matrix.", read_only: false, } + + linear: + x: + max_velocity: { + type: double, + default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + min_velocity: { + type: double, + default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + max_acceleration: { + type: double, + default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + max_deceleration: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + max_acceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum acceleration in reverse direction. If not set, -max_acceleration will be used.", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + max_deceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in reverse direction. If not set, -max_deceleration will be used.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + max_jerk: { + type: double, + default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + min_jerk: { + type: double, + default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + y: + max_velocity: { + type: double, + default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + min_velocity: { + type: double, + default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + max_acceleration: { + type: double, + default_value: .NAN, + description: "Maximum acceleration in forward direction.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + max_deceleration: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in forward direction.", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + max_acceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum acceleration in reverse direction. If not set, -max_acceleration will be used.", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + max_deceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in reverse direction. If not set, -max_deceleration will be used.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + max_jerk: { + type: double, + default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + min_jerk: { + type: double, + default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + angular: + z: + max_velocity: { + type: double, + default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + min_velocity: { + type: double, + default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + max_acceleration: { + type: double, + default_value: .NAN, + description: "Maximum acceleration in positive direction.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + max_deceleration: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in positive direction.", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + max_acceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum acceleration in negative direction. If not set, -max_acceleration will be used.", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } + max_deceleration_reverse: { + type: double, + default_value: .NAN, + description: "Maximum deceleration in negative direction. If not set, -max_deceleration will be used.", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + max_jerk: { + type: double, + default_value: .NAN, + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + } + } + min_jerk: { + type: double, + default_value: .NAN, + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + } + } diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index dfa3e5de94..d86f6bd211 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -39,3 +39,41 @@ enable_odom_tf: true pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + + test_mecanum_drive_controller_with_limits: + ros__parameters: + reference_timeout: 0.9 + + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" + + kinematics: + base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } + wheels_radius: 0.5 + sum_of_robot_center_projection_on_X_Y_axis: 1.0 + + base_frame_id: "base_link" + odom_frame_id: "odom" + enable_odom_tf: true + twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] + pose_covariance_diagonal: [0.0, 6.0, 12.0, 18.0, 24.0, 30.0] + + linear: + x: + max_acceleration: 2.0 + max_deceleration: -4.0 + max_acceleration_reverse: -8.0 + max_deceleration_reverse: 10.0 + y: + max_acceleration: 2.0 + max_deceleration: -4.0 + max_acceleration_reverse: -8.0 + max_deceleration_reverse: 10.0 + angular: + z: + max_acceleration: 2.0 + max_deceleration: -4.0 + max_acceleration_reverse: -8.0 + max_deceleration_reverse: 10.0 diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 55b8386e5d..fd756cd552 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -875,6 +875,360 @@ TEST_F(MecanumDriveControllerTest, odometry_set_service) EXPECT_GT(controller_->odometry_.getY(), start_y); } +// Test that when no velocity limits are configured (all NaN defaults), +// commands pass through immediately without rate limiting. +TEST_F(MecanumDriveControllerTest, test_no_speed_limiter_when_not_configured) +{ + // Use the default config which has no velocity limits set + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // Fill the queue with zero velocity + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + } + + // Send a large step command - without limits it should be applied immediately + controller_->reference_interfaces_[0] = 10.0; + controller_->reference_interfaces_[1] = 5.0; + controller_->reference_interfaces_[2] = 3.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.001)), + controller_interface::return_type::OK); + + // With base_frame_offset = {0,0,0}: + // wheel_fl = 1/r * (vx - vy - k*wz) = 1/0.5 * (10.0 - 5.0 - 1.0*3.0) = 4.0 + // wheel_fr = 1/r * (vx + vy + k*wz) = 1/0.5 * (10.0 + 5.0 + 1.0*3.0) = 36.0 + // wheel_rr = 1/r * (vx - vy + k*wz) = 1/0.5 * (10.0 - 5.0 + 1.0*3.0) = 16.0 + // wheel_rl = 1/r * (vx + vy - k*wz) = 1/0.5 * (10.0 + 5.0 - 1.0*3.0) = 24.0 + const size_t fl = controller_->get_front_left_wheel_index(); + const size_t fr = controller_->get_front_right_wheel_index(); + const size_t rr = controller_->get_rear_right_wheel_index(); + const size_t rl = controller_->get_rear_left_wheel_index(); + + EXPECT_NEAR(4.0, joint_command_values_[fl], 1e-3); + EXPECT_NEAR(36.0, joint_command_values_[fr], 1e-3); + EXPECT_NEAR(16.0, joint_command_values_[rr], 1e-3); + EXPECT_NEAR(24.0, joint_command_values_[rl], 1e-3); +} + +// Test that velocity limits are applied to linear x commands. +// With base_frame_offset = {0,0,0}, wheels_radius = 0.5, and +// sum_of_robot_center_projection_on_X_Y_axis = 1.0: +// For pure linear x velocity v, all 4 wheel velocities = v / wheels_radius = 2*v +TEST_F(MecanumDriveControllerTest, test_speed_limiter_linear_x) +{ + SetUpController("test_mecanum_drive_controller_with_limits"); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // Fill the speed limiter queue with zero velocity + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(0.0, joint_command_values_[0], 1e-3); + } + + const double dt = 0.001; + const double wheels_radius = 0.5; + + // Phase 1: Forward acceleration (0 -> 1.0 m/s), max_acceleration = 2.0 m/s² + { + const double linear = 1.0; + const double max_acceleration = 2.0; + const double time_acc = linear / max_acceleration; // 0.5s + + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update( + rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_GT(linear / wheels_radius, joint_command_values_[0]) + << "at t: " << i * dt + << "s, but this wheel velocity should only be achieved at t: " << time_acc; + } + + // After acceleration time, should reach target + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + + // Fill queue at steady state + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update( + rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + } + } + + // Phase 2: Forward deceleration (1.0 -> 0.0 m/s), max_deceleration = -4.0 m/s² + { + const double linear = 0.0; + const double max_deceleration = -4.0; + const double time_acc = -1.0 / max_deceleration; // 0.25s + + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update( + rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_LT(linear / wheels_radius, joint_command_values_[0]) + << "at t: " << i * dt + << "s, but this wheel velocity should only be achieved at t: " << time_acc; + } + + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update( + rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + } + } + + // Phase 3: Reverse acceleration (0 -> -1.0 m/s), max_acceleration_reverse = -8.0 m/s² + { + const double linear = -1.0; + const double max_acceleration_reverse = -8.0; + const double time_acc = -1.0 / max_acceleration_reverse; // 0.125s + + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update( + rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_LT(linear / wheels_radius, joint_command_values_[0]) + << "at t: " << i * dt + << "s, but this wheel velocity should only be achieved at t: " << time_acc; + } + + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update( + rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + } + } + + // Phase 4: Reverse deceleration (-1.0 -> 0.0 m/s), max_deceleration_reverse = 10.0 m/s² + { + const double linear = 0.0; + const double max_deceleration_reverse = 10.0; + const double time_acc = 1.0 / max_deceleration_reverse; // 0.1s + + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update( + rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_GT(linear / wheels_radius, joint_command_values_[0]) + << "at t: " << i * dt + << "s, but this wheel velocity should only be achieved at t: " << time_acc; + } + + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update( + rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + } + } +} + +// Test that velocity limits are applied to linear y commands. +// For pure linear y velocity v with zero base_frame_offset: +// wheel_fl = -v/r, wheel_fr = v/r, wheel_rr = -v/r, wheel_rl = v/r +TEST_F(MecanumDriveControllerTest, test_speed_limiter_linear_y) +{ + SetUpController("test_mecanum_drive_controller_with_limits"); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // Fill the speed limiter queue with zero velocity + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + } + + const double dt = 0.001; + const double wheels_radius = 0.5; + const double linear_y = 1.0; + const double max_acceleration = 2.0; + const double time_acc = linear_y / max_acceleration; // 0.5s + + // Accelerate in linear y from 0 to 1.0 m/s + // wheel_fr = linear_y / wheels_radius (positive) + const size_t fr = controller_->get_front_right_wheel_index(); + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = linear_y; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_GT(linear_y / wheels_radius, joint_command_values_[fr]) + << "at t: " << i * dt + << "s, but this wheel velocity should only be achieved at t: " << time_acc; + } + + // After acceleration time, should reach target + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = linear_y; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear_y / wheels_radius, joint_command_values_[fr], 1e-3); +} + +// Test that velocity limits are applied to angular z commands. +// For pure angular z velocity w with zero base_frame_offset: +// wheel_fr = k*w/r (positive), wheel_fl = -k*w/r (negative) +// where k = sum_of_robot_center_projection_on_X_Y_axis = 1.0 +TEST_F(MecanumDriveControllerTest, test_speed_limiter_angular_z) +{ + SetUpController("test_mecanum_drive_controller_with_limits"); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // Fill the speed limiter queue with zero velocity + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + } + + const double dt = 0.001; + const double wheels_radius = 0.5; + const double k = 1.0; // sum_of_robot_center_projection_on_X_Y_axis + const double angular_z = 1.0; + const double max_acceleration = 2.0; + const double time_acc = angular_z / max_acceleration; // 0.5s + + // Accelerate in angular z from 0 to 1.0 rad/s + // wheel_fr = k * angular_z / wheels_radius (positive) + const size_t fr = controller_->get_front_right_wheel_index(); + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = angular_z; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_GT(k * angular_z / wheels_radius, joint_command_values_[fr]) + << "at t: " << i * dt + << "s, but this wheel velocity should only be achieved at t: " << time_acc; + } + + // After acceleration time, should reach target + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = angular_z; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(k * angular_z / wheels_radius, joint_command_values_[fr], 1e-3); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index d625c302b4..93821169ff 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -93,6 +93,11 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_prefix_set_namespace); FRIEND_TEST(MecanumDriveControllerTest, configure_succeeds_tf_tilde_prefix_set_namespace); + FRIEND_TEST(MecanumDriveControllerTest, test_no_speed_limiter_when_not_configured); + FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_linear_x); + FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_linear_y); + FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_angular_z); + public: controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override From 86a621c79f8d16099e9748d92940f04bca0a0987 Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Thu, 16 Apr 2026 13:24:31 -0400 Subject: [PATCH 2/6] Linting. --- .../src/mecanum_drive_controller.cpp | 22 ++++++++--------- .../test/test_mecanum_drive_controller.cpp | 24 +++++++------------ 2 files changed, 19 insertions(+), 27 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 2b22829b8c..31a7d270fc 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -278,7 +278,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( } catch (const std::invalid_argument & e) { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to configure linear x speed limiter: %s", e.what()); + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to configure linear x speed limiter: %s", e.what()); return controller_interface::CallbackReturn::ERROR; } try @@ -291,7 +292,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( } catch (const std::invalid_argument & e) { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to configure linear y speed limiter: %s", e.what()); + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to configure linear y speed limiter: %s", e.what()); return controller_interface::CallbackReturn::ERROR; } try @@ -304,7 +306,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( } catch (const std::invalid_argument & e) { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to configure angular z speed limiter: %s", e.what()); + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to configure angular z speed limiter: %s", e.what()); return controller_interface::CallbackReturn::ERROR; } @@ -576,17 +579,14 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma tf2::Matrix3x3 rotation_from_base_to_center = tf2::Matrix3x3((quaternion)); tf2::Vector3 velocity_in_base_frame_w_r_t_center_frame_ = - rotation_from_base_to_center * - tf2::Vector3(linear_x_command, linear_y_command, 0.0); + rotation_from_base_to_center * tf2::Vector3(linear_x_command, linear_y_command, 0.0); tf2::Vector3 linear_trans_from_base_to_center = tf2::Vector3( params_.kinematics.base_frame_offset.x, params_.kinematics.base_frame_offset.y, 0.0); - velocity_in_center_frame_linear_x_ = - velocity_in_base_frame_w_r_t_center_frame_.x() + - linear_trans_from_base_to_center.y() * angular_z_command; - velocity_in_center_frame_linear_y_ = - velocity_in_base_frame_w_r_t_center_frame_.y() - - linear_trans_from_base_to_center.x() * angular_z_command; + velocity_in_center_frame_linear_x_ = velocity_in_base_frame_w_r_t_center_frame_.x() + + linear_trans_from_base_to_center.y() * angular_z_command; + velocity_in_center_frame_linear_y_ = velocity_in_base_frame_w_r_t_center_frame_.y() - + linear_trans_from_base_to_center.x() * angular_z_command; velocity_in_center_frame_angular_z_ = angular_z_command; const double wheel_front_left_vel = diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index fd756cd552..d836d49ee2 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -962,8 +962,7 @@ TEST_F(MecanumDriveControllerTest, test_speed_limiter_linear_x) controller_->reference_interfaces_[1] = 0.0; controller_->reference_interfaces_[2] = 0.0; ASSERT_EQ( - controller_->update( - rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); EXPECT_GT(linear / wheels_radius, joint_command_values_[0]) << "at t: " << i * dt @@ -986,8 +985,7 @@ TEST_F(MecanumDriveControllerTest, test_speed_limiter_linear_x) controller_->reference_interfaces_[1] = 0.0; controller_->reference_interfaces_[2] = 0.0; ASSERT_EQ( - controller_->update( - rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); } @@ -1005,8 +1003,7 @@ TEST_F(MecanumDriveControllerTest, test_speed_limiter_linear_x) controller_->reference_interfaces_[1] = 0.0; controller_->reference_interfaces_[2] = 0.0; ASSERT_EQ( - controller_->update( - rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); EXPECT_LT(linear / wheels_radius, joint_command_values_[0]) << "at t: " << i * dt @@ -1027,8 +1024,7 @@ TEST_F(MecanumDriveControllerTest, test_speed_limiter_linear_x) controller_->reference_interfaces_[1] = 0.0; controller_->reference_interfaces_[2] = 0.0; ASSERT_EQ( - controller_->update( - rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); } @@ -1046,8 +1042,7 @@ TEST_F(MecanumDriveControllerTest, test_speed_limiter_linear_x) controller_->reference_interfaces_[1] = 0.0; controller_->reference_interfaces_[2] = 0.0; ASSERT_EQ( - controller_->update( - rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); EXPECT_LT(linear / wheels_radius, joint_command_values_[0]) << "at t: " << i * dt @@ -1068,8 +1063,7 @@ TEST_F(MecanumDriveControllerTest, test_speed_limiter_linear_x) controller_->reference_interfaces_[1] = 0.0; controller_->reference_interfaces_[2] = 0.0; ASSERT_EQ( - controller_->update( - rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); } @@ -1087,8 +1081,7 @@ TEST_F(MecanumDriveControllerTest, test_speed_limiter_linear_x) controller_->reference_interfaces_[1] = 0.0; controller_->reference_interfaces_[2] = 0.0; ASSERT_EQ( - controller_->update( - rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), controller_interface::return_type::OK); EXPECT_GT(linear / wheels_radius, joint_command_values_[0]) << "at t: " << i * dt @@ -1109,8 +1102,7 @@ TEST_F(MecanumDriveControllerTest, test_speed_limiter_linear_x) controller_->reference_interfaces_[1] = 0.0; controller_->reference_interfaces_[2] = 0.0; ASSERT_EQ( - controller_->update( - rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); } From 88a304180883f74d26c561dadf1855fe701c6b54 Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Mon, 20 Apr 2026 18:38:41 -0400 Subject: [PATCH 3/6] Update mecanum_drive_controller/src/mecanum_drive_controller.cpp Co-authored-by: Bence Magyar --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 31a7d270fc..c3262dfd92 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -312,9 +312,7 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( } // Initialize previous commands queue for jerk limiting - previous_two_commands_ = std::queue>(); - previous_two_commands_.push({{0.0, 0.0, 0.0}}); - previous_two_commands_.push({{0.0, 0.0, 0.0}}); + previous_two_commands_ = std::queue>({{{0.0, 0.0, 0.0}}, {{0.0, 0.0, 0.0}}}); return controller_interface::CallbackReturn::SUCCESS; } From 6d485d5d2c6146fad3857c4ca460bbabab4914fc Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Mon, 20 Apr 2026 18:40:51 -0400 Subject: [PATCH 4/6] Moved down logging statement. --- mecanum_drive_controller/src/mecanum_drive_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index c3262dfd92..a27692d59c 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -265,8 +265,6 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully"); - // Configure speed limiters try { @@ -314,6 +312,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( // Initialize previous commands queue for jerk limiting previous_two_commands_ = std::queue>({{{0.0, 0.0, 0.0}}, {{0.0, 0.0, 0.0}}}); + RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully"); + return controller_interface::CallbackReturn::SUCCESS; } From 15551f690646047f3fd3b670b29d9c6c96ca0319 Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Mon, 20 Apr 2026 19:22:26 -0400 Subject: [PATCH 5/6] Added reset for previous values on lifecycle changes and tests for it. --- .../mecanum_drive_controller.hpp | 4 + .../src/mecanum_drive_controller.cpp | 32 +++++-- .../test/test_mecanum_drive_controller.cpp | 96 +++++++++++++++++++ .../test/test_mecanum_drive_controller.hpp | 2 + 4 files changed, 127 insertions(+), 7 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index fd586adf43..a638f04fda 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -69,6 +69,8 @@ class MecanumDriveController : public controller_interface::ChainableControllerI controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; + void reset_buffers(); + controller_interface::return_type update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -164,6 +166,8 @@ class MecanumDriveController : public controller_interface::ChainableControllerI std::unique_ptr> limiter_linear_x_; std::unique_ptr> limiter_linear_y_; std::unique_ptr> limiter_angular_z_; + +protected: // Previous two commands for jerk limiting: queue of [linear_x, linear_y, angular_z] std::queue> previous_two_commands_; }; diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index a27692d59c..fe5c5ce033 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -14,6 +14,7 @@ #include "mecanum_drive_controller/mecanum_drive_controller.hpp" +#include #include #include #include @@ -309,8 +310,9 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( return controller_interface::CallbackReturn::ERROR; } - // Initialize previous commands queue for jerk limiting - previous_two_commands_ = std::queue>({{{0.0, 0.0, 0.0}}, {{0.0, 0.0, 0.0}}}); + // Allocate reference interfaces and reset their values to NaN to catch uninitialized usage. + reference_interfaces_.resize(NR_REF_ITFS, std::numeric_limits::quiet_NaN()); + reset_buffers(); RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully"); @@ -407,11 +409,10 @@ bool MecanumDriveController::on_set_chained_mode(bool /*chained_mode*/) { return controller_interface::CallbackReturn MecanumDriveController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - // Try to set default value in command. - // If this fails, then another command will be received soon anyways. - ControllerReferenceMsg emtpy_msg; - reset_controller_reference_msg(emtpy_msg, get_node()); - input_ref_.try_set(emtpy_msg); + // Reset limiter history and reference buffers so a previous activation cannot + // influence the behavior of the controller after a deactivate->activate cycle. + reset_buffers(); + return controller_interface::CallbackReturn::SUCCESS; } @@ -433,9 +434,26 @@ controller_interface::CallbackReturn MecanumDriveController::on_deactivate( return controller_interface::CallbackReturn::FAILURE; } + reset_buffers(); + return controller_interface::CallbackReturn::SUCCESS; } +void MecanumDriveController::reset_buffers() +{ + std::fill( + reference_interfaces_.begin(), reference_interfaces_.end(), + std::numeric_limits::quiet_NaN()); + + // Replace the queue with one initialized to two zero entries. + previous_two_commands_ = std::queue>( + std::deque>{{{0.0, 0.0, 0.0}}, {{0.0, 0.0, 0.0}}}); + + // Reset the latest received reference back to NaN so no stale command is applied. + reset_controller_reference_msg(current_ref_, get_node()); + input_ref_.set(current_ref_); +} + controller_interface::return_type MecanumDriveController::update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index d836d49ee2..6387910921 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -1221,6 +1222,101 @@ TEST_F(MecanumDriveControllerTest, test_speed_limiter_angular_z) EXPECT_NEAR(k * angular_z / wheels_radius, joint_command_values_[fr], 1e-3); } +// Test that reset_buffers() clears the jerk-limiter history, the reference interfaces, +// and the latest received reference back to NaN/zero. +TEST_F(MecanumDriveControllerTest, test_reset_buffers_clears_limiter_state) +{ + SetUpController("test_mecanum_drive_controller_with_limits"); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // Dirty all buffers that reset_buffers() is responsible for clearing. + controller_->reference_interfaces_[0] = 1.0; + controller_->reference_interfaces_[1] = 2.0; + controller_->reference_interfaces_[2] = 3.0; + + std::queue> dirty; + dirty.push({{4.0, 5.0, 6.0}}); + dirty.push({{7.0, 8.0, 9.0}}); + std::swap(controller_->previous_two_commands_, dirty); + + ControllerReferenceMsg dirty_ref; + dirty_ref.header.stamp = controller_->get_node()->now(); + dirty_ref.twist.linear.x = 1.0; + dirty_ref.twist.linear.y = 2.0; + dirty_ref.twist.angular.z = 3.0; + controller_->input_ref_.set(dirty_ref); + + controller_->reset_buffers(); + + for (const auto & itf : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(itf)); + } + ASSERT_EQ(controller_->previous_two_commands_.size(), 2u); + EXPECT_EQ(controller_->previous_two_commands_.front(), (std::array{{0.0, 0.0, 0.0}})); + EXPECT_EQ(controller_->previous_two_commands_.back(), (std::array{{0.0, 0.0, 0.0}})); + + auto reset_ref = controller_->input_ref_.get(); + EXPECT_TRUE(std::isnan(reset_ref.twist.linear.x)); + EXPECT_TRUE(std::isnan(reset_ref.twist.linear.y)); + EXPECT_TRUE(std::isnan(reset_ref.twist.angular.z)); +} + +// Test that lifecycle transitions reset the limiter history so that re-activating +// the controller re-limits commands from zero. +TEST_F(MecanumDriveControllerTest, test_lifecycle_transitions_reset_limiter_buffers) +{ + SetUpController("test_mecanum_drive_controller_with_limits"); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const double dt = 0.001; + const double wheels_radius = 0.5; + const double linear = 1.0; + const double max_acceleration = 2.0; // m/s^2 (from test_mecanum_drive_controller_params.yaml) + const double time_acc = linear / max_acceleration; + + // Ramp up linear x to the steady-state target so the limiter buffer holds + // non-zero history. + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) + 5; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + } + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + EXPECT_NEAR(linear, controller_->previous_two_commands_.back()[0], 1e-3); + + // Deactivate then re-activate: limiter history must be reset to zero. + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + EXPECT_EQ(controller_->previous_two_commands_.front(), (std::array{{0.0, 0.0, 0.0}})); + EXPECT_EQ(controller_->previous_two_commands_.back(), (std::array{{0.0, 0.0, 0.0}})); + + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + EXPECT_EQ(controller_->previous_two_commands_.front(), (std::array{{0.0, 0.0, 0.0}})); + EXPECT_EQ(controller_->previous_two_commands_.back(), (std::array{{0.0, 0.0, 0.0}})); + + // After reactivation, requesting the same target should once again be limited + // by max_acceleration starting from zero, not pass through immediately. + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_LT(joint_command_values_[0], linear / wheels_radius) + << "Limiter history was not reset across lifecycle transitions; the wheel command " + "should be ramping up from zero again."; +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index 93821169ff..cbb3ea21bb 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -97,6 +97,8 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_linear_x); FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_linear_y); FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_angular_z); + FRIEND_TEST(MecanumDriveControllerTest, test_reset_buffers_clears_limiter_state); + FRIEND_TEST(MecanumDriveControllerTest, test_lifecycle_transitions_reset_limiter_buffers); public: controller_interface::CallbackReturn on_configure( From 9dbe9b4af6dbabdccb5347f2bbd33984024d1dee Mon Sep 17 00:00:00 2001 From: Tony Baltovski Date: Thu, 7 May 2026 09:13:04 -0400 Subject: [PATCH 6/6] Updated limiter parameters as read_only. --- .../src/mecanum_drive_controller.cpp | 28 ++++ .../src/mecanum_drive_controller.yaml | 14 +- .../test/test_mecanum_drive_controller.cpp | 124 ++++++++++++++++++ .../test/test_mecanum_drive_controller.hpp | 1 + 4 files changed, 161 insertions(+), 6 deletions(-) diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index fe5c5ce033..027bc6f1ff 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -510,6 +510,34 @@ controller_interface::return_type MecanumDriveController::update_reference_from_ controller_interface::return_type MecanumDriveController::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) { + if (param_listener_->try_update_params(params_)) + { + ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); + try + { + limiter_linear_x_->set_params( + params_.linear.x.min_velocity, params_.linear.x.max_velocity, + params_.linear.x.max_acceleration_reverse, params_.linear.x.max_acceleration, + params_.linear.x.max_deceleration, params_.linear.x.max_deceleration_reverse, + params_.linear.x.min_jerk, params_.linear.x.max_jerk); + limiter_linear_y_->set_params( + params_.linear.y.min_velocity, params_.linear.y.max_velocity, + params_.linear.y.max_acceleration_reverse, params_.linear.y.max_acceleration, + params_.linear.y.max_deceleration, params_.linear.y.max_deceleration_reverse, + params_.linear.y.min_jerk, params_.linear.y.max_jerk); + limiter_angular_z_->set_params( + params_.angular.z.min_velocity, params_.angular.z.max_velocity, + params_.angular.z.max_acceleration_reverse, params_.angular.z.max_acceleration, + params_.angular.z.max_deceleration, params_.angular.z.max_deceleration_reverse, + params_.angular.z.min_jerk, params_.angular.z.max_jerk); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to update speed limiter parameters: %s", e.what()); + } + } + // FORWARD KINEMATICS (odometry). const auto wheel_front_left_state_vel_op = state_interfaces_[FRONT_LEFT].get_optional(); const auto wheel_front_right_state_vel_op = state_interfaces_[FRONT_RIGHT].get_optional(); diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index f9cc431e99..38a2ceac2b 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -100,7 +100,7 @@ mecanum_drive_controller: type: double, default_value: 0.0, description: "Wheel's radius.", - read_only: false, + read_only: true, validation: { gt<>: [0.0] } @@ -110,30 +110,32 @@ mecanum_drive_controller: type: double, default_value: 0.0, description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis = lx+ly", - read_only: false, + read_only: true, } tf_frame_prefix_enable: { type: bool, default_value: true, description: "Deprecated: this parameter will be removed in a future release. Use 'tf_frame_prefix' instead.", + read_only: true, } tf_frame_prefix: { type: string, default_value: "", description: "(optional) Prefix to be appended to the tf frames, will be added to odom_frame_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.", + read_only: true, } base_frame_id: { type: string, default_value: "base_link", description: "Base frame_id set to value of base_frame_id.", - read_only: false, + read_only: true, } odom_frame_id: { type: string, default_value: "odom", description: "Odometry frame_id set to value of odom_frame_id.", - read_only: false, + read_only: true, } enable_odom_tf: { type: bool, @@ -146,14 +148,14 @@ mecanum_drive_controller: type: double_array, default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], description: "Diagonal values of twist covariance matrix.", - read_only: false, + read_only: true, } pose_covariance_diagonal: { type: double_array, default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], description: "Diagonal values of pose covariance matrix.", - read_only: false, + read_only: true, } linear: diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 6387910921..0c9af66869 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -1317,6 +1317,130 @@ TEST_F(MecanumDriveControllerTest, test_lifecycle_transitions_reset_limiter_buff "should be ramping up from zero again."; } +// This test verifies that parameters can be updated at runtime. +TEST_F(MecanumDriveControllerTest, test_speed_limiter_runtime_update) +{ + // If you set a linear velocity reference without acceleration limits, + // then the wheel velocity command (rotations/s) will be: + // ideal_wheel_velocity_command (rotations/s) = linear_velocity_command (m/s) / wheel_radius (m). + // (The velocity command looks like a step function). + // However, if there are acceleration limits, then the actual wheel velocity command + // should always be less than the ideal velocity, and should only become + // equal at time = linear_velocity_command (m/s) / acceleration_limit (m/s^2). + const double max_acceleration_1 = 2.0; + const double max_acceleration_2 = 5.0; + const double max_deceleration = -4.0; + + SetUpController("test_mecanum_drive_controller_with_limits"); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + const double dt = 0.001; + const double wheels_radius = 0.5; + + auto wait_for_limiter = [&](double linear_ref, double expected_vel) + { + for (int i = 0; i < 3; ++i) + { + controller_->reference_interfaces_[0] = linear_ref; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR(expected_vel, joint_command_values_[0], 1e-3); + } + }; + + // wait for the speed limiter to fill the queue + controller_->reference_interfaces_[0] = 0.0; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + wait_for_limiter(0.0, 0.0); + + // Phase 1: accelerate with max_acceleration = 2.0 + { + const double linear = 1.0; + const double time_acc = linear / max_acceleration_1; + for (int i = 0; i < static_cast(std::floor(time_acc / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + } + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + // wait for the speed limiter to fill the queue + wait_for_limiter(linear, linear / wheels_radius); + } + // Stop the robot + { + const double linear = 0.0; + const double time_dec = 1.0 / std::abs(max_deceleration); + for (int i = 0; i < static_cast(std::floor(time_dec / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + } + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + // wait for the speed limiter to fill the queue + wait_for_limiter(linear, 0.0); + } + // Phase 2: update parameter at runtime to max_acceleration = 5.0 + { + auto result = controller_->get_node()->set_parameter( + rclcpp::Parameter("linear.x.max_acceleration", rclcpp::ParameterValue(max_acceleration_2))); + ASSERT_TRUE(result.successful); + } + // Phase 3: accelerate with max_acceleration = 5.0 + { + const double linear = 1.0; + const double time_acc_1 = linear / max_acceleration_1; + const double time_acc_2 = linear / max_acceleration_2; + // With higher acceleration, should reach target faster + ASSERT_LT(time_acc_2, time_acc_1); + for (int i = 0; i < static_cast(std::floor(time_acc_2 / dt)) - 1; ++i) + { + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + } + controller_->reference_interfaces_[0] = linear; + controller_->reference_interfaces_[1] = 0.0; + controller_->reference_interfaces_[2] = 0.0; + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)), + controller_interface::return_type::OK); + EXPECT_NEAR(linear / wheels_radius, joint_command_values_[0], 1e-3); + // wait for the speed limiter to fill the queue + wait_for_limiter(linear, linear / wheels_radius); + } +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index cbb3ea21bb..7a9396b31e 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -97,6 +97,7 @@ class TestableMecanumDriveController : public mecanum_drive_controller::MecanumD FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_linear_x); FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_linear_y); FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_angular_z); + FRIEND_TEST(MecanumDriveControllerTest, test_speed_limiter_runtime_update); FRIEND_TEST(MecanumDriveControllerTest, test_reset_buffers_clears_limiter_state); FRIEND_TEST(MecanumDriveControllerTest, test_lifecycle_transitions_reset_limiter_buffers);