From 568765e45c75857c9daf23042ce34f6993373d53 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 20 Nov 2024 19:57:14 +0000 Subject: [PATCH] Use new rate_limiter --- .../diff_drive_controller.hpp | 6 +-- .../diff_drive_controller/speed_limiter.hpp | 36 ++++++++++----- .../src/diff_drive_controller.cpp | 45 ++++++++++++++++++- .../src/diff_drive_controller_parameter.yaml | 9 ++-- 4 files changed, 77 insertions(+), 19 deletions(-) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp index dfb0663fd6..ac34b81eca 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp @@ -26,9 +26,9 @@ #include #include -#include "control_toolbox/speed_limiter.hpp" #include "controller_interface/controller_interface.hpp" #include "diff_drive_controller/odometry.hpp" +#include "diff_drive_controller/speed_limiter.hpp" #include "diff_drive_controller/visibility_control.h" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -135,8 +135,8 @@ class DiffDriveController : public controller_interface::ControllerInterface std::queue previous_commands_; // last two commands // speed limiters - control_toolbox::SpeedLimiter limiter_linear_; - control_toolbox::SpeedLimiter limiter_angular_; + SpeedLimiter limiter_linear_; + SpeedLimiter limiter_angular_; bool publish_limited_velocity_ = false; std::shared_ptr> limited_velocity_publisher_ = nullptr; diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp index 24a9c66647..9c33d2bdb1 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -21,11 +21,11 @@ // TODO(christophfroehlich) remove this file after the next release -#include "control_toolbox/speed_limiter.hpp" +#include "control_toolbox/rate_limiter.hpp" namespace diff_drive_controller { -class [[deprecated("Use control_toolbox/speed_limiter instead")]] SpeedLimiter +class SpeedLimiter { public: /** @@ -41,14 +41,28 @@ class [[deprecated("Use control_toolbox/speed_limiter instead")]] SpeedLimiter * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 */ SpeedLimiter( - bool has_velocity_limits = false, bool has_acceleration_limits = false, - bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN, + bool has_velocity_limits = true, bool has_acceleration_limits = true, + bool has_jerk_limits = true, double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN, double max_jerk = NAN) - : speed_limiter_( - has_velocity_limits, has_acceleration_limits, has_jerk_limits, min_velocity, max_velocity, - min_acceleration, max_acceleration, min_jerk, max_jerk) { + // START DEPRECATED + if (!has_velocity_limits) + { + min_velocity = max_velocity = NAN; + } + if (!has_acceleration_limits) + { + min_acceleration = max_acceleration = NAN; + } + if (!has_jerk_limits) + { + min_jerk = max_jerk = NAN; + } + // END DEPRECATED + speed_limiter_ = control_toolbox::RateLimiter( + min_velocity, max_velocity, min_acceleration, max_acceleration, min_acceleration, + max_acceleration, min_jerk, max_jerk); } /** @@ -69,7 +83,7 @@ class [[deprecated("Use control_toolbox/speed_limiter instead")]] SpeedLimiter * \param [in, out] v Velocity [m/s] * \return Limiting factor (1.0 if none) */ - double limit_velocity(double & v) { return speed_limiter_.limit_velocity(v); } + double limit_velocity(double & v) { return speed_limiter_.limit_value(v); } /** * \brief Limit the acceleration @@ -80,7 +94,7 @@ class [[deprecated("Use control_toolbox/speed_limiter instead")]] SpeedLimiter */ double limit_acceleration(double & v, double v0, double dt) { - return speed_limiter_.limit_acceleration(v, v0, dt); + return speed_limiter_.limit_first_derivative(v, v0, dt); } /** @@ -94,11 +108,11 @@ class [[deprecated("Use control_toolbox/speed_limiter instead")]] SpeedLimiter */ double limit_jerk(double & v, double v0, double v1, double dt) { - return speed_limiter_.limit_jerk(v, v0, v1, dt); + return speed_limiter_.limit_second_derivative(v, v0, v1, dt); } private: - control_toolbox::SpeedLimiter speed_limiter_; // Instance of the new SpeedLimiter + control_toolbox::RateLimiter speed_limiter_; // Instance of the new RateLimiter }; } // namespace diff_drive_controller diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index a1145be47d..58d989ccca 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -296,13 +296,54 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( cmd_vel_timeout_ = std::chrono::milliseconds{static_cast(params_.cmd_vel_timeout * 1000.0)}; publish_limited_velocity_ = params_.publish_limited_velocity; - limiter_linear_ = control_toolbox::SpeedLimiter( + // TODO(christophfroehlich) remove deprecated parameters + // START DEPRECATED + if (!params_.linear.x.has_velocity_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_velocity_limits parameter is deprecated. Set the respective limits to NAN"); + } + if (!params_.linear.x.has_acceleration_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_acceleration_limits parameter is deprecated. Set the respective limits to " + "NAN"); + } + if (!params_.linear.x.has_jerk_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_jerk_limits parameter is deprecated. Set the respective limits to NAN"); + } + if (!params_.angular.z.has_velocity_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_velocity_limits parameter is deprecated. Set the respective limits to NAN"); + } + if (!params_.angular.z.has_acceleration_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_acceleration_limits parameter is deprecated. Set the respective limits to " + "NAN"); + } + if (!params_.angular.z.has_jerk_limits) + { + RCLCPP_WARN( + logger, + "[deprecated] has_jerk_limits parameter is deprecated. Set the respective limits to NAN"); + } + // END DEPRECATED + limiter_linear_ = SpeedLimiter( params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits, params_.linear.x.has_jerk_limits, params_.linear.x.min_velocity, params_.linear.x.max_velocity, params_.linear.x.min_acceleration, params_.linear.x.max_acceleration, params_.linear.x.min_jerk, params_.linear.x.max_jerk); - limiter_angular_ = control_toolbox::SpeedLimiter( + limiter_angular_ = SpeedLimiter( params_.angular.z.has_velocity_limits, params_.angular.z.has_acceleration_limits, params_.angular.z.has_jerk_limits, params_.angular.z.min_velocity, params_.angular.z.max_velocity, params_.angular.z.min_acceleration, diff --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index 755259cc2a..eb3663820a 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/diff_drive_controller/src/diff_drive_controller_parameter.yaml @@ -115,15 +115,18 @@ diff_drive_controller: x: has_velocity_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to NAN for deactivation" } has_acceleration_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to NAN for deactivation" } has_jerk_limits: { type: bool, - default_value: false, + default_value: true, + description: "deprecated, set the respective limits to NAN for deactivation" } max_velocity: { type: double,