Add new deceleration parameters

ros-controls · Dec 1, 2024 · ffea38a · ffea38a
1 parent c77f873
commit ffea38a
Show file tree

Hide file tree

Showing 3 changed files with 84 additions and 34 deletions.
diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp
@@ -35,37 +35,67 @@ 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(
+  [[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 min_acceleration = 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())
   {
-    // START DEPRECATED
     if (!has_velocity_limits)
     {
       min_velocity = max_velocity = std::numeric_limits<double>::quiet_NaN();
     }
     if (!has_acceleration_limits)
     {
-      min_acceleration = max_acceleration = std::numeric_limits<double>::quiet_NaN();
+      max_deceleration = max_acceleration = std::numeric_limits<double>::quiet_NaN();
     }
     if (!has_jerk_limits)
     {
       min_jerk = max_jerk = std::numeric_limits<double>::quiet_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);
+      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(
+    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);
   }
 
   /**

diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/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
 {
@@ -303,51 +310,65 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
     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())
   {

diff --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml
@@ -23,21 +23,20 @@ test_diff_drive_controller:
     publish_limited_velocity: true
     velocity_rolling_window_size: 10
 
-    linear.x.has_velocity_limits: false
-    linear.x.has_acceleration_limits: false
-    linear.x.has_jerk_limits: false
-    linear.x.max_velocity: 0.0
-    linear.x.min_velocity: 0.0
-    linear.x.max_acceleration: 0.0
-    linear.x.max_jerk: 0.0
-    linear.x.min_jerk: 0.0
+    linear.x.max_velocity: .NAN
+    linear.x.min_velocity: .NAN
+    linear.x.max_deceleration: .NAN
+    linear.x.max_acceleration: .NAN
+    linear.x.max_deceleration_reverse: .NAN
+    linear.x.max_acceleration_reverse: .NAN
+    linear.x.max_jerk: .NAN
+    linear.x.min_jerk: .NAN
 
-    angular.z.has_velocity_limits: false
-    angular.z.has_acceleration_limits: false
-    angular.z.has_jerk_limits: false
-    angular.z.max_velocity: 0.0
-    angular.z.min_velocity: 0.0
-    angular.z.max_acceleration: 0.0
-    angular.z.min_acceleration: 0.0
-    angular.z.max_jerk: 0.0
-    angular.z.min_jerk: 0.0
+    angular.z.max_velocity: .NAN
+    angular.z.min_velocity: .NAN
+    angular.z.max_deceleration: .NAN
+    angular.z.max_acceleration: .NAN
+    angular.z.max_deceleration_reverse: .NAN
+    angular.z.max_acceleration_reverse: .NAN
+    angular.z.max_jerk: .NAN
+    angular.z.min_jerk: .NAN