diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.h b/diff_drive_controller/include/diff_drive_controller/speed_limiter.h index 9be008007..67624bbea 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.h +++ b/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 --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index ebb8a090f..7d42e5ce0 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -247,6 +247,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 ); @@ -257,6 +259,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 --git a/diff_drive_controller/src/speed_limiter.cpp b/diff_drive_controller/src/speed_limiter.cpp index a7e84c621..794d1e519 100644 --- a/diff_drive_controller/src/speed_limiter.cpp +++ b/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; }