From 00c5db654d6e850e9c7e31a08bdbcfa50bc70987 Mon Sep 17 00:00:00 2001 From: mohammad alqudah Date: Fri, 20 Dec 2024 08:30:25 +0900 Subject: [PATCH] add comment to implementation Signed-off-by: mohammad alqudah --- .../src/mpc_lateral_controller.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index 977ec6242104a..f8673c3c79c73 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -419,6 +419,9 @@ bool MpcLateralController::isStoppedState() const m_current_trajectory.points, m_current_kinematic_state.pose.pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); + // It is possible that stop is executed earlier than stop point, and velocity controller + // will not start when the distance from ego to stop point is less than 0.5 meter. + // So we use a distance margin to ensure we can detect stopped state. static constexpr double distance_margin = 1.0; const double target_vel = std::invoke([&]() -> double { auto min_vel = m_current_trajectory.points.at(nearest).longitudinal_velocity_mps;