Use the renamed and merged RateLimiter

diff_drive_controller/CMakeLists.txt

@@ -33,6 +33,7 @@ add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR})
 
 generate_parameter_library(diff_drive_controller_parameters
   src/diff_drive_controller_parameter.yaml
+  include/diff_drive_controller/custom_validators.hpp
 )
 
 add_library(diff_drive_controller SHARED
@@ -44,7 +45,9 @@ target_include_directories(diff_drive_controller PUBLIC
   $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/diff_drive_controller>
 )
-target_link_libraries(diff_drive_controller PUBLIC diff_drive_controller_parameters)
+target_link_libraries(diff_drive_controller
+  PUBLIC
+    diff_drive_controller_parameters)
 ament_target_dependencies(diff_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.

diff_drive_controller/include/diff_drive_controller/custom_validators.hpp

@@ -0,0 +1,64 @@
+// Copyright 2024 AIT - Austrian Institute of Technology GmbH
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// TODO(christophfroehlich) remove this file and use it from control_toolbox once
+// https://github.com/PickNikRobotics/generate_parameter_library/pull/213 is merged and released
+
+#ifndef DIFF_DRIVE_CONTROLLER__CUSTOM_VALIDATORS_HPP_
+#define DIFF_DRIVE_CONTROLLER__CUSTOM_VALIDATORS_HPP_
+
+#include <fmt/core.h>
+
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rsl/parameter_validators.hpp>
+#include <tl_expected/expected.hpp>
+
+namespace diff_drive_controller
+{
+
+/**
+ * @brief gt_eq, but check only if the value is not NaN
+ */
+template <typename T>
+tl::expected<void, std::string> gt_eq_or_nan(rclcpp::Parameter const & parameter, T expected_value)
+{
+  auto param_value = parameter.as_double();
+  if (!std::isnan(param_value))
+  {
+    // check only if the value is not NaN
+    return rsl::gt_eq<T>(parameter, expected_value);
+  }
+  return {};
+}
+
+/**
+ * @brief lt_eq, but check only if the value is not NaN
+ */
+template <typename T>
+tl::expected<void, std::string> lt_eq_or_nan(rclcpp::Parameter const & parameter, T expected_value)
+{
+  auto param_value = parameter.as_double();
+  if (!std::isnan(param_value))
+  {
+    // check only if the value is not NaN
+    return rsl::lt_eq<T>(parameter, expected_value);
+  }
+  return {};
+}
+
+}  // namespace diff_drive_controller
+
+#endif  // DIFF_DRIVE_CONTROLLER__CUSTOM_VALIDATORS_HPP_

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,