From 4c51a39093511f395393e402cab6a7afc1f7d773 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Fri, 7 Apr 2023 19:39:09 +0100 Subject: [PATCH 1/2] fix inverted accel / deccel --- nav2_velocity_smoother/src/velocity_smoother.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 1c42ff44fb..d838e8a88b 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -202,8 +202,18 @@ 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; + + 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_; + } + return v_curr + std::clamp(eta * dv, v_component_min, v_component_max); } From 6d448d11f801718d864c4ef1ffc38185e5afa814 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 11 Apr 2023 11:16:44 +0100 Subject: [PATCH 2/2] handle speed through 0.0 --- nav2_velocity_smoother/src/velocity_smoother.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index d838e8a88b..a44fd221a7 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -206,9 +206,17 @@ double VelocitySmoother::applyConstraints( double v_component_max; double v_component_min; - if (abs(v_cmd) >= abs(v_curr)) { - v_component_max = accel / smoothing_frequency_; - v_component_min = -accel / smoothing_frequency_; + // 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_;