diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 1c42ff44fb2..a44fd221a7d 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -202,8 +202,26 @@ double VelocitySmoother::applyConstraints( const double accel, const double decel, const double eta) { double dv = v_cmd - v_curr; - const double v_component_max = accel / smoothing_frequency_; - const double v_component_min = decel / smoothing_frequency_; + + double v_component_max; + double v_component_min; + + // Accelerating if magnitude of v_cmd is above magnitude of v_curr + // and if v_cmd and v_curr have the same sign (i.e. speed is NOT passing through 0.0) + // Deccelerating otherwise + if (v_curr * v_cmd >= 0.0) { + if (abs(v_cmd) >= abs(v_curr)) { + v_component_max = accel / smoothing_frequency_; + v_component_min = -accel / smoothing_frequency_; + } else { + v_component_max = -decel / smoothing_frequency_; + v_component_min = decel / smoothing_frequency_; + } + } else { + v_component_max = -decel / smoothing_frequency_; + v_component_min = decel / smoothing_frequency_; + } + return v_curr + std::clamp(eta * dv, v_component_min, v_component_max); }