From 5f174127d893d0a2e3e1036289425bd0ea9982dc Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Wed, 20 Dec 2023 14:01:47 +0900 Subject: [PATCH] fix(run_out): do not insert stop point when stopped in Object method (#5911) Signed-off-by: Tomohito Ando --- .../src/scene.cpp | 21 +++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index 92516e7b4424b..b14152863d8d0 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -615,16 +615,25 @@ std::optional RunOutModule::calcStopPoint( // vehicle have to decelerate if there is not enough distance with deceleration_jerk const bool deceleration_needed = *stop_dist > dist_to_collision - planner_param_.run_out.stop_margin; - // avoid acceleration when ego is decelerating - // TODO(Tomohito Ando): replace with more appropriate method - constexpr float epsilon = 1.0e-2; - constexpr float stopping_vel_mps = 2.5 / 3.6; - const bool is_stopping = current_vel < stopping_vel_mps && current_acc < epsilon; - if (!deceleration_needed && !is_stopping) { + const auto & detection_method = planner_param_.run_out.detection_method; + + if (!deceleration_needed && detection_method == "Object") { debug_ptr_->setAccelReason(RunOutDebug::AccelReason::LOW_JERK); return {}; } + // If the detection method assumes running out, avoid acceleration when the ego is decelerating. + // TODO(Tomohito Ando): replace with more appropriate way + if (!deceleration_needed && detection_method != "Object") { + constexpr float epsilon = 1.0e-2; + constexpr float stopping_vel_mps = 2.5 / 3.6; + const bool is_stopping = current_vel < stopping_vel_mps && current_acc < epsilon; + if (!is_stopping) { + debug_ptr_->setAccelReason(RunOutDebug::AccelReason::LOW_JERK); + return {}; + } + } + // calculate the stop point for base link const float base_to_collision_point = planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front;