Merge pull request #19 from botsandus/AUTO-763_fix_accel_decel

AUTO-763 fix smoother accel decel

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);
 }