fix(motion_utils): properly use negative jerk in calculate_stop_distance (#870)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp

@@ -34,8 +34,8 @@ namespace autoware::motion_utils
  * * If the vehicle reaches a full stop during phase 1 or phase 2, the calculation
  * terminates early and returns the exact distance covered up to the point where v = 0.
  *
- * @param current_vel        Current longitudinal speed v₀ [m/s].
- * @param current_acc        Current longitudinal acceleration a₀ [m/s²].
+ * @param v0 Current longitudinal speed v₀ [m/s].
+ * @param a0 Current longitudinal acceleration a₀ [m/s²].
  * @param decel_limit        Maximum braking deceleration [m/s²]
  * @param jerk_limit         Maximum braking jerk [m/s³]
  * @param initial_time_delay Latency before any braking jerk is applied t₁ [s]. Defaults to 0.0.
@@ -45,8 +45,8 @@ namespace autoware::motion_utils
  * Returns std::nullopt if the kinematic limits are invalid (e.g., limits are 0).
  */
 [[nodiscard]] std::optional<double> calculate_stop_distance(
-  const double current_vel, const double current_acc, const double acc_limit,
-  const double jerk_limit, const double initial_time_delay = 0.0);
+  const double v0, const double a0, const double decel_limit, const double jerk_limit,
+  const double initial_time_delay = 0.0);
 
 }  // namespace autoware::motion_utils
 

common/autoware_motion_utils/src/distance/distance.cpp

@@ -305,22 +305,22 @@ std::optional<double> calcDecelDistWithJerkAndAccConstraints(
   if (v2 <= 0.0) {
     // The vehicle reaches v = 0 before hitting the maximum deceleration limit.
     // Solve for t where 0 = v1 + a1*t + 0.5*j*t^2
-    const double discriminant = a1 * a1 - 2.0 * jerk_limit * v1;
+    const double discriminant = a1 * a1 - 2.0 * negative_jerk_limit * v1;
 
     // should be impossible
     if (discriminant < 0.0) {
       return std::nullopt;
     }
 
-    const double t2 = -(a1 + std::sqrt(discriminant)) / jerk_limit;
-    const auto [x_stop, v_stop, a_stop] = update(x1, v1, a1, jerk_limit, t2);
+    const double t2 = std::max(0.0, -(a1 + std::sqrt(discriminant)) / negative_jerk_limit);
+    const auto [x_stop, v_stop, a_stop] = update(x1, v1, a1, negative_jerk_limit, t2);
 
     return std::max(0.0, x_stop);
   }
 
   // The vehicle successfully reaches the maximum deceleration limit.
-  const double t2 = (negative_decel_limit - a1) / jerk_limit;
-  const auto [x2, v2_final, a2_final] = update(x1, v1, a1, jerk_limit, t2);
+  const double t2 = std::max(0.0, (negative_decel_limit - a1) / negative_jerk_limit);
+  const auto [x2, v2_final, a2_final] = update(x1, v1, a1, negative_jerk_limit, t2);
 
   // Phase 3: Constant maximum deceleration
   // Decelerate at acc_limit from v2_final down to 0.