Fix SpeedLimiter Constructor regression (#1478)

ros-controls · Jan 13, 2025 · e0b929b · e0b929b
1 parent ba7c1dd
commit e0b929b
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 24 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
@@ -120,8 +120,8 @@ class DiffDriveController : public controller_interface::ControllerInterface
   std::queue<TwistStamped> previous_commands_;  // last two commands
 
   // speed limiters
-  SpeedLimiter limiter_linear_;
-  SpeedLimiter limiter_angular_;
+  std::unique_ptr<SpeedLimiter> limiter_linear_;
+  std::unique_ptr<SpeedLimiter> limiter_angular_;
 
   bool publish_limited_velocity_ = false;
   std::shared_ptr<rclcpp::Publisher<TwistStamped>> 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
@@ -40,7 +40,7 @@ class SpeedLimiter
    * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
    * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
    */
-  [[deprecated]] SpeedLimiter(
+  [[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(),
@@ -79,16 +79,16 @@ class SpeedLimiter
    * >= 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
    */
-  SpeedLimiter(
-    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())
+  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,

diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp
@@ -45,14 +45,7 @@ using hardware_interface::HW_IF_POSITION;
 using hardware_interface::HW_IF_VELOCITY;
 using lifecycle_msgs::msg::State;
 
-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())
-{
-}
+DiffDriveController::DiffDriveController() : controller_interface::ControllerInterface() {}
 
 const char * DiffDriveController::feedback_type() const
 {
@@ -241,9 +234,9 @@ controller_interface::return_type DiffDriveController::update(
 
   auto & last_command = previous_commands_.back().twist;
   auto & second_to_last_command = previous_commands_.front().twist;
-  limiter_linear_.limit(
+  limiter_linear_->limit(
     linear_command, last_command.linear.x, second_to_last_command.linear.x, period.seconds());
-  limiter_angular_.limit(
+  limiter_angular_->limit(
     angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds());
 
   previous_commands_.pop();
@@ -365,13 +358,13 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
       std::numeric_limits<double>::quiet_NaN();
   }
   // END DEPRECATED
-  limiter_linear_ = SpeedLimiter(
+  limiter_linear_ = std::make_unique<SpeedLimiter>(
     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(
+  limiter_angular_ = std::make_unique<SpeedLimiter>(
     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,