fix(trajectory_follower): fixed stop distance calculation in transiti…

…oning to emergency_stop (autowarefoundation#1915) (#157) Signed-off-by: Mamoru Sobue <[email protected]> Signed-off-by: Mamoru Sobue <[email protected]> Co-authored-by: Mamoru Sobue <[email protected]>
tier4 · Oct 19, 2022 · ec173ab · ec173ab
1 parent 46989f0
commit ec173ab
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 350 deletions.
diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp
@@ -498,8 +498,13 @@ PidLongitudinalController::ControlState PidLongitudinalController::updateControl
       ? (node_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time
       : false;
 
-  const bool8_t emergency_condition =
-    m_enable_overshoot_emergency && stop_dist < -p.emergency_state_overshoot_stop_dist;
+  static constexpr double vel_epsilon =
+    1e-3;  // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity
+  const float64_t current_vel_cmd =
+    std::fabs(m_trajectory_ptr->points.at(control_data.nearest_idx).longitudinal_velocity_mps);
+  const bool8_t emergency_condition = m_enable_overshoot_emergency &&
+                                      stop_dist < -p.emergency_state_overshoot_stop_dist &&
+                                      current_vel_cmd < vel_epsilon;
 
   // transit state
   if (current_control_state == ControlState::DRIVE) {