Unify parameters with diff_drive

mecanum_drive_controller/CMakeLists.txt

@@ -11,10 +11,11 @@ if(NOT CMAKE_CXX_STANDARD)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra)
+  add_compile_options(-Wall -Wextra -Wpedantic -Wshadow -Wold-style-cast)
 endif()
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+    control_toolbox
     controller_interface
     generate_parameter_library
     geometry_msgs
@@ -33,24 +34,25 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
-generate_parameter_library(mecanum_drive_controller_parameters
-                           src/mecanum_drive_controller_parameter.yaml)
+# 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_parameter.yaml
+  ${TB_INCLUDE_DIRS}/control_toolbox/custom_validators.hpp)
 
-add_library(
-  mecanum_drive_controller SHARED src/mecanum_drive_controller.cpp
-                                  src/odometry.cpp src/speed_limiter.cpp)
+add_library(mecanum_drive_controller SHARED src/mecanum_drive_controller.cpp
+                                            src/odometry.cpp)
 target_include_directories(
   mecanum_drive_controller
-  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
          $<INSTALL_INTERFACE:include/mecanum_drive_controller>)
-target_link_libraries(mecanum_drive_controller
-                      PUBLIC mecanum_drive_controller_parameters)
+target_link_libraries(
+  mecanum_drive_controller PUBLIC mecanum_drive_controller_parameters
+                                  control_toolbox::rate_limiter_parameters)
 ament_target_dependencies(mecanum_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.
-target_compile_definitions(mecanum_drive_controller
-                           PRIVATE "MECANUM_DRIVE_CONTROLLER_BUILDING_DLL")
 pluginlib_export_plugin_description_file(controller_interface
                                          mecanum_drive_plugin.xml)
 

mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp

@@ -47,7 +47,6 @@
 #include "mecanum_drive_controller/mecanum_drive_controller_parameters.hpp"
 #include "mecanum_drive_controller/odometry.hpp"
 #include "mecanum_drive_controller/speed_limiter.hpp"
-#include "mecanum_drive_controller/visibility_control.h"
 #include "odometry.hpp"
 
 namespace mecanum_drive_controller
@@ -57,46 +56,32 @@ class MecanumDriveController : public controller_interface::ControllerInterface
   using TwistStamped = geometry_msgs::msg::TwistStamped;
 
 public:
-  MECANUM_DRIVE_CONTROLLER_PUBLIC
   MecanumDriveController();
 
-  MECANUM_DRIVE_CONTROLLER_PUBLIC
   controller_interface::InterfaceConfiguration command_interface_configuration() const override;
 
-  MECANUM_DRIVE_CONTROLLER_PUBLIC
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
-  MECANUM_DRIVE_CONTROLLER_PUBLIC
   controller_interface::return_type update(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
-  MECANUM_DRIVE_CONTROLLER_PUBLIC
   controller_interface::CallbackReturn on_init() override;
 
-  MECANUM_DRIVE_CONTROLLER_PUBLIC
   controller_interface::CallbackReturn on_configure(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  MECANUM_DRIVE_CONTROLLER_PUBLIC
   controller_interface::CallbackReturn on_activate(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  MECANUM_DRIVE_CONTROLLER_PUBLIC
   controller_interface::CallbackReturn on_deactivate(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  MECANUM_DRIVE_CONTROLLER_PUBLIC
   controller_interface::CallbackReturn on_cleanup(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  MECANUM_DRIVE_CONTROLLER_PUBLIC
   controller_interface::CallbackReturn on_error(
     const rclcpp_lifecycle::State & previous_state) override;
 
-  MECANUM_DRIVE_CONTROLLER_PUBLIC
-  controller_interface::CallbackReturn on_shutdown(
-    const rclcpp_lifecycle::State & previous_state) override;
-
 protected:
   struct WheelHandle
   {
@@ -124,6 +109,9 @@ class MecanumDriveController : public controller_interface::ControllerInterface
 
   Odometry odometry_;
 
+  // Timeout to consider cmd_vel commands old
+  rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5);
+
   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ = nullptr;
   std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
     realtime_odometry_publisher_ = nullptr;
@@ -132,10 +120,6 @@ class MecanumDriveController : public controller_interface::ControllerInterface
     nullptr;
   std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
     realtime_odometry_transform_publisher_ = nullptr;
-
-  // Timeout to consider cmd_vel commands old
-  std::chrono::milliseconds cmd_vel_timeout_{500};
-
   bool subscriber_is_active_ = false;
   rclcpp::Subscription<TwistStamped>::SharedPtr velocity_command_subscriber_ = nullptr;
 
@@ -144,9 +128,9 @@ class MecanumDriveController : public controller_interface::ControllerInterface
   std::queue<TwistStamped> previous_commands_;  // last two commands
 
   // speed limiters
-  SpeedLimiter limiter_linear_x_;
-  SpeedLimiter limiter_linear_y_;
-  SpeedLimiter limiter_angular_;
+  std::unique_ptr<SpeedLimiter> limiter_linear_x_;
+  std::unique_ptr<SpeedLimiter> limiter_linear_y_;
+  std::unique_ptr<SpeedLimiter> limiter_angular_;
 
   bool publish_limited_velocity_ = false;
   std::shared_ptr<rclcpp::Publisher<TwistStamped>> limited_velocity_publisher_ = nullptr;

mecanum_drive_controller/include/mecanum_drive_controller/speed_limiter.hpp

@@ -23,7 +23,9 @@
 #ifndef MECANUM_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_
 #define MECANUM_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_
 
-#include <cmath>
+#include <limits>
+
+#include "control_toolbox/rate_limiter.hpp"
 
 namespace mecanum_drive_controller
 {
@@ -37,16 +39,62 @@ 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
    */
-  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);
+  [[deprecated]] explicit 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] 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
+   *
+   * \note
+   * If max_* values are NAN, the respective limit is deactivated
+   * If min_* values are NAN (unspecified), defaults to -max
+   * If min_first_derivative_pos/max_first_derivative_neg values are NAN, symmetric limits are used
+   */
+  explicit SpeedLimiter(
+    double min_velocity, double max_velocity, double max_acceleration_reverse,
+    double max_acceleration, double max_deceleration, double max_deceleration_reverse,
+    double min_jerk, double max_jerk)
+  {
+    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
@@ -56,14 +104,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
@@ -72,7 +123,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
@@ -83,25 +137,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 mecanum_drive_controller

mecanum_drive_controller/package.xml

@@ -3,32 +3,38 @@
 <package format="3">
   <name>mecanum_drive_controller</name>
   <version>1.0.0</version>
-
   <description>Controller for a mecanum drive mobile base.</description>
+  <maintainer email="support@husarion.com">Husarion</maintainer>
   <license>Apache License 2.0</license>
 
+  <url type="website">https://husarion.com/</url>
+  <url type="repository">https://github.com/husarion/husarion_controllers</url>
+  <url type="bugtracker">https://github.com/husarion/husarion_controllers/issues</url>
+
   <author email="jakub.delicat@husarion.com">Jakub Delicat</author>
+  <author email="rafal.gorecki@husarion.com">Rafał Górecki</author>
+  <author email="dawid.kmak@husarion.com">Dawid Kmak</author>
   <author email="maciej.stepien@husarion.com">Maciej Stępień</author>
   <author email="bence.magyar.robotics@gmail.com">Bence Magyar</author>
   <author email="jordan.palacios@pal-robotics.com">Jordan Palacios</author>
 
-  <maintainer email="contact@husarion.com">Husarion</maintainer>
-
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <build_depend>generate_parameter_library</build_depend>
+
+  <depend>control_toolbox</depend>
   <depend>controller_interface</depend>
   <depend>geometry_msgs</depend>
   <depend>hardware_interface</depend>
   <depend>nav_msgs</depend>
+  <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>rcpputils</depend>
   <depend>realtime_tools</depend>
   <depend>tf2</depend>
   <depend>tf2_msgs</depend>
 
-  <build_depend>pluginlib</build_depend>
-
   <export>
     <build_type>ament_cmake</build_type>
   </export>