Merge pull request #17 from clearpathrobotics/deceleration_limits

Separate acceleration and deceleration limits.

Author: p6chen

diff_drive_controller/include/diff_drive_controller/speed_limiter.h

@@ -56,6 +56,8 @@ namespace diff_drive_controller
      * \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
+     * \param [in] min_deceleration Minimum deceleration [m/s^2], usually <= 0
+     * \param [in] max_deceleration Maximum deceleration [m/s^2], usually >= 0
      */
     SpeedLimiter(
       bool has_velocity_limits = false,
@@ -66,7 +68,9 @@ namespace diff_drive_controller
       double min_acceleration = 0.0,
       double max_acceleration = 0.0,
       double min_jerk = 0.0,
-      double max_jerk = 0.0);
+      double max_jerk = 0.0,
+      double min_deceleration = 0.0,
+      double max_deceleration = 0.0);
 
     /**
      * \brief Limit the velocity and acceleration
@@ -122,6 +126,10 @@ namespace diff_drive_controller
     // Jerk limits:
     double min_jerk;
     double max_jerk;
+
+    // Deceleration limits
+    double min_deceleration;  // Minimum acceleration used to slow down vehicle to zero velocity.
+    double max_deceleration;  // Maximum acceleration used to slow down vehicle to zero velocity.
   };
 
 }  // namespace diff_drive_controller

diff_drive_controller/src/diff_drive_controller.cpp

@@ -261,6 +261,8 @@ namespace diff_drive_controller
     controller_nh.param("linear/x/min_velocity"           , limiter_lin_.min_velocity           , -limiter_lin_.max_velocity          );
     controller_nh.param("linear/x/max_acceleration"       , limiter_lin_.max_acceleration       ,  limiter_lin_.max_acceleration      );
     controller_nh.param("linear/x/min_acceleration"       , limiter_lin_.min_acceleration       , -limiter_lin_.max_acceleration      );
+    controller_nh.param("linear/x/max_deceleration"       , limiter_lin_.max_deceleration       ,  limiter_lin_.max_acceleration      );
+    controller_nh.param("linear/x/min_deceleration"       , limiter_lin_.min_deceleration       ,  limiter_lin_.min_acceleration      );
     controller_nh.param("linear/x/max_jerk"               , limiter_lin_.max_jerk               ,  limiter_lin_.max_jerk              );
     controller_nh.param("linear/x/min_jerk"               , limiter_lin_.min_jerk               , -limiter_lin_.max_jerk              );
 
@@ -271,6 +273,8 @@ namespace diff_drive_controller
     controller_nh.param("angular/z/min_velocity"           , limiter_ang_.min_velocity           , -limiter_ang_.max_velocity          );
     controller_nh.param("angular/z/max_acceleration"       , limiter_ang_.max_acceleration       ,  limiter_ang_.max_acceleration      );
     controller_nh.param("angular/z/min_acceleration"       , limiter_ang_.min_acceleration       , -limiter_ang_.max_acceleration      );
+    controller_nh.param("angular/z/max_deceleration"       , limiter_ang_.max_deceleration       ,  limiter_ang_.max_acceleration      );
+    controller_nh.param("angular/z/min_deceleration"       , limiter_ang_.min_deceleration       ,  limiter_ang_.min_acceleration      );
     controller_nh.param("angular/z/max_jerk"               , limiter_ang_.max_jerk               ,  limiter_ang_.max_jerk              );
     controller_nh.param("angular/z/min_jerk"               , limiter_ang_.min_jerk               , -limiter_ang_.max_jerk              );
 

diff_drive_controller/src/speed_limiter.cpp

@@ -58,7 +58,9 @@ namespace diff_drive_controller
     double min_acceleration,
     double max_acceleration,
     double min_jerk,
-    double max_jerk
+    double max_jerk,
+    double min_deceleration,
+    double max_deceleration
   )
   : has_velocity_limits(has_velocity_limits)
   , has_acceleration_limits(has_acceleration_limits)
@@ -69,6 +71,8 @@ namespace diff_drive_controller
   , max_acceleration(max_acceleration)
   , min_jerk(min_jerk)
   , max_jerk(max_jerk)
+  , min_deceleration(min_deceleration)
+  , max_deceleration(max_deceleration)
   {
   }
 
@@ -101,11 +105,45 @@ namespace diff_drive_controller
 
     if (has_acceleration_limits)
     {
-      const double dv_min = min_acceleration * dt;
-      const double dv_max = max_acceleration * dt;
-
-      const double dv = clamp(v - v0, dv_min, dv_max);
-
+      double dv_min, dv_max, dv;
+
+      if (v0 >= 0.0 && v >= 0.0)
+      {
+        // Moving in positive direction
+        dv_min = min_deceleration * dt;
+        dv_max = max_acceleration * dt;
+      }
+      else if (v0 <= 0.0 && v <= 0.0)
+      {
+        // Moving in the negative direction
+        dv_min = min_acceleration * dt;
+        dv_max = max_deceleration * dt;
+      }
+      else
+      {
+        // Transitioning from positive to negative velocity
+        if (v0 > 0)
+        {
+          dv_max = v0;
+          dv_min = min_deceleration * dt;
+          if (v0 + dv < 0)
+          {
+            dv_min = min_acceleration * (dt - v0 / min_deceleration);
+          }
+        }
+        // Transitioning from negative to positive velocity
+        else if (v0 < 0)
+        {
+          dv_min = v0;
+          dv_max = max_deceleration * dt;
+          if (v0 + dv > 0)
+          {
+            dv_max = max_acceleration * (dt - v0 / max_deceleration);
+          }
+        }
+      }
+
+      dv = clamp(v - v0, dv_min, dv_max);
       v = v0 + dv;
     }