Update command limiter of diff_drive_controller

diff_drive_controller/CMakeLists.txt

@@ -8,6 +8,7 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
 endif()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+  control_toolbox
   controller_interface
   generate_parameter_library
   geometry_msgs
@@ -32,19 +33,21 @@ 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
   src/diff_drive_controller.cpp
   src/odometry.cpp
-  src/speed_limiter.cpp
 )
 target_compile_features(diff_drive_controller PUBLIC cxx_std_17)
 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.
@@ -78,6 +81,11 @@ if(BUILD_TESTING)
     controller_manager
     ros2_control_test_assets
   )
+  ament_add_gmock(test_configure_diff_drive_controller test/test_configure_diff_drive_controller.cpp)
+  ament_target_dependencies(test_configure_diff_drive_controller
+    controller_manager
+    ros2_control_test_assets
+  )
 endif()
 
 install(

diff_drive_controller/doc/parameters_context.yaml

@@ -1,9 +1,5 @@
 linear.x: |
   Joint limits structure for the linear ``x``-axis.
-  The limiter ignores position limits.
-  For details see ``joint_limits`` package from ros2_control repository.
 
 angular.z: |
   Joint limits structure for the rotation about ``z``-axis.
-  The limiter ignores position limits.
-  For details see ``joint_limits`` package from ros2_control repository.

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

@@ -20,7 +20,6 @@
 #define DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
 
 #include <chrono>
-#include <cmath>
 #include <memory>
 #include <queue>
 #include <string>

diff_drive_controller/include/diff_drive_controller/odometry.hpp

@@ -22,8 +22,6 @@
 #ifndef DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_
 #define DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_
 
-#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

diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp

@@ -19,7 +19,9 @@
 #ifndef DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_
 #define DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_
 
-#include <cmath>
+#include <limits>
+
+#include "control_toolbox/rate_limiter.hpp"
 
 namespace diff_drive_controller
 {
@@ -33,16 +35,68 @@ class SpeedLimiter
    * \param [in] has_jerk_limits         if true, applies jerk limits
    * \param [in] min_velocity Minimum velocity [m/s], usually <= 0
    * \param [in] max_velocity Maximum velocity [m/s], usually >= 0
-   * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0
+   * \param [in] max_deceleration Maximum deceleration [m/s^2], usually <= 0
+   * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
+   * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
+   * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
+   */
+  [[deprecated]] SpeedLimiter(
+    bool has_velocity_limits = true, bool has_acceleration_limits = true,
+    bool has_jerk_limits = true, double min_velocity = std::numeric_limits<double>::quiet_NaN(),
+    double max_velocity = std::numeric_limits<double>::quiet_NaN(),
+    double max_deceleration = std::numeric_limits<double>::quiet_NaN(),
+    double max_acceleration = std::numeric_limits<double>::quiet_NaN(),
+    double min_jerk = std::numeric_limits<double>::quiet_NaN(),
+    double max_jerk = std::numeric_limits<double>::quiet_NaN())
+  {
+    if (!has_velocity_limits)
+    {
+      min_velocity = max_velocity = std::numeric_limits<double>::quiet_NaN();
+    }
+    if (!has_acceleration_limits)
+    {
+      max_deceleration = max_acceleration = std::numeric_limits<double>::quiet_NaN();
+    }
+    if (!has_jerk_limits)
+    {
+      min_jerk = max_jerk = std::numeric_limits<double>::quiet_NaN();
+    }
+    speed_limiter_ = control_toolbox::RateLimiter<double>(
+      min_velocity, max_velocity, max_deceleration, max_acceleration,
+      std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(), min_jerk,
+      max_jerk);
+  }
+
+  /**
+   * \brief Constructor
+   * \param [in] has_velocity_limits     if true, applies velocity limits
+   * \param [in] has_acceleration_limits if true, applies acceleration limits
+   * \param [in] has_jerk_limits         if true, applies jerk limits
+   * \param [in] min_velocity Minimum velocity [m/s], usually <= 0
+   * \param [in] max_velocity Maximum velocity [m/s], usually >= 0
+   * \param [in] max_acceleration_reverse Maximum acceleration in reverse direction [m/s^2], usually
+   * <= 0
    * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
+   * \param [in] max_deceleration Maximum deceleration [m/s^2], usually <= 0
+   * \param [in] max_deceleration_reverse Maximum deceleration in reverse direction [m/s^2], usually
+   * >= 0
    * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
    * \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,
-    double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN,
-    double max_jerk = NAN);
+    double min_velocity = std::numeric_limits<double>::quiet_NaN(),
+    double max_velocity = std::numeric_limits<double>::quiet_NaN(),
+    double max_acceleration_reverse = std::numeric_limits<double>::quiet_NaN(),
+    double max_acceleration = std::numeric_limits<double>::quiet_NaN(),
+    double max_deceleration = std::numeric_limits<double>::quiet_NaN(),
+    double max_deceleration_reverse = std::numeric_limits<double>::quiet_NaN(),
+    double min_jerk = std::numeric_limits<double>::quiet_NaN(),
+    double max_jerk = std::numeric_limits<double>::quiet_NaN())
+  {
+    speed_limiter_ = control_toolbox::RateLimiter<double>(
+      min_velocity, max_velocity, max_acceleration_reverse, max_acceleration, max_deceleration,
+      max_deceleration_reverse, min_jerk, max_jerk);
+  }
 
   /**
    * \brief Limit the velocity and acceleration
@@ -52,14 +106,17 @@ class SpeedLimiter
    * \param [in]      dt Time step [s]
    * \return Limiting factor (1.0 if none)
    */
-  double limit(double & v, double v0, double v1, double dt);
+  double limit(double & v, double v0, double v1, double dt)
+  {
+    return speed_limiter_.limit(v, v0, v1, dt);
+  }
 
   /**
    * \brief Limit the velocity
    * \param [in, out] v Velocity [m/s]
    * \return Limiting factor (1.0 if none)
    */
-  double limit_velocity(double & v);
+  double limit_velocity(double & v) { return speed_limiter_.limit_value(v); }
 
   /**
    * \brief Limit the acceleration
@@ -68,7 +125,10 @@ class SpeedLimiter
    * \param [in]      dt Time step [s]
    * \return Limiting factor (1.0 if none)
    */
-  double limit_acceleration(double & v, double v0, double dt);
+  double limit_acceleration(double & v, double v0, double dt)
+  {
+    return speed_limiter_.limit_first_derivative(v, v0, dt);
+  }
 
   /**
    * \brief Limit the jerk
@@ -79,25 +139,13 @@ class SpeedLimiter
    * \return Limiting factor (1.0 if none)
    * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control
    */
-  double limit_jerk(double & v, double v0, double v1, double dt);
+  double limit_jerk(double & v, double v0, double v1, double dt)
+  {
+    return speed_limiter_.limit_second_derivative(v, v0, v1, dt);
+  }
 
 private:
-  // Enable/Disable velocity/acceleration/jerk limits:
-  bool has_velocity_limits_;
-  bool has_acceleration_limits_;
-  bool has_jerk_limits_;
-
-  // Velocity limits:
-  double min_velocity_;
-  double max_velocity_;
-
-  // Acceleration limits:
-  double min_acceleration_;
-  double max_acceleration_;
-
-  // Jerk limits:
-  double min_jerk_;
-  double max_jerk_;
+  control_toolbox::RateLimiter<double> speed_limiter_;  // Instance of the new RateLimiter
 };
 
 }  // namespace diff_drive_controller

diff_drive_controller/package.xml

@@ -24,6 +24,7 @@
   <build_depend>generate_parameter_library</build_depend>
 
   <depend>backward_ros</depend>
+  <depend>control_toolbox</depend>
   <depend>controller_interface</depend>
   <depend>geometry_msgs</depend>
   <depend>hardware_interface</depend>

diff_drive_controller/src/diff_drive_controller.cpp

@@ -45,7 +45,14 @@ using hardware_interface::HW_IF_POSITION;
 using hardware_interface::HW_IF_VELOCITY;
 using lifecycle_msgs::msg::State;
 
-DiffDriveController::DiffDriveController() : controller_interface::ControllerInterface() {}
+DiffDriveController::DiffDriveController()
+: controller_interface::ControllerInterface(),
+  // dummy limiter, will be created in on_configure
+  // could be done with shared_ptr instead -> but will break ABI
+  limiter_angular_(std::numeric_limits<double>::quiet_NaN()),
+  limiter_linear_(std::numeric_limits<double>::quiet_NaN())
+{
+}
 
 const char * DiffDriveController::feedback_type() const
 {
@@ -296,17 +303,72 @@ 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;
 
+  // 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");
+    params_.linear.x.min_velocity = params_.linear.x.max_velocity =
+      std::numeric_limits<double>::quiet_NaN();
+  }
+  if (!params_.linear.x.has_acceleration_limits)
+  {
+    RCLCPP_WARN(
+      logger,
+      "[deprecated] has_acceleration_limits parameter is deprecated. Set the respective limits to "
+      "NAN");
+    params_.linear.x.max_deceleration = params_.linear.x.max_acceleration =
+      params_.linear.x.max_deceleration_reverse = params_.linear.x.max_acceleration_reverse =
+        std::numeric_limits<double>::quiet_NaN();
+  }
+  if (!params_.linear.x.has_jerk_limits)
+  {
+    RCLCPP_WARN(
+      logger,
+      "[deprecated] has_jerk_limits parameter is deprecated. Set the respective limits to NAN");
+    params_.linear.x.min_jerk = params_.linear.x.max_jerk =
+      std::numeric_limits<double>::quiet_NaN();
+  }
+  if (!params_.angular.z.has_velocity_limits)
+  {
+    RCLCPP_WARN(
+      logger,
+      "[deprecated] has_velocity_limits parameter is deprecated. Set the respective limits to NAN");
+    params_.angular.z.min_velocity = params_.angular.z.max_velocity =
+      std::numeric_limits<double>::quiet_NaN();
+  }
+  if (!params_.angular.z.has_acceleration_limits)
+  {
+    RCLCPP_WARN(
+      logger,
+      "[deprecated] has_acceleration_limits parameter is deprecated. Set the respective limits to "
+      "NAN");
+    params_.angular.z.max_deceleration = params_.angular.z.max_acceleration =
+      params_.angular.z.max_deceleration_reverse = params_.angular.z.max_acceleration_reverse =
+        std::numeric_limits<double>::quiet_NaN();
+  }
+  if (!params_.angular.z.has_jerk_limits)
+  {
+    RCLCPP_WARN(
+      logger,
+      "[deprecated] has_jerk_limits parameter is deprecated. Set the respective limits to NAN");
+    params_.angular.z.min_jerk = params_.angular.z.max_jerk =
+      std::numeric_limits<double>::quiet_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);
+    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_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,
-    params_.angular.z.max_acceleration, params_.angular.z.min_jerk, params_.angular.z.max_jerk);
+    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);
 
   if (!reset())
   {