Use new rate_limiter

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -26,9 +26,9 @@
 #include <string>
 #include <vector>
 
-#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<Twist> 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<rclcpp::Publisher<Twist>> limited_velocity_publisher_ = nullptr;

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<double>(
+      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<double> speed_limiter_;  // Instance of the new RateLimiter
 };
 
 }  // namespace diff_drive_controller

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<int>(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_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,