From 1fe0d21527ef78345414642fc86c0fd67a211217 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 7 Mar 2025 13:28:12 +0900 Subject: [PATCH] fix(static_obstacle_avoidance): ego doesn't keep stopping in unsafe condition (#10242) Signed-off-by: satoshi-ota --- .../src/scene.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 948d0cdd58b4b..1a4829a559470 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -1679,7 +1679,7 @@ void StaticObstacleAvoidanceModule::insertReturnDeadLine( const auto min_return_distance = helper_->getMinAvoidanceDistance(shift_length) + helper_->getNominalPrepareDistance(0.0); const auto to_stop_line = data.to_return_point - min_return_distance - buffer; - if (to_stop_line < 0.0) { + if (to_stop_line < -1.0 * parameters_->stop_buffer) { RCLCPP_WARN(getLogger(), "ego overran return shift dead line. do nothing."); return; } @@ -1752,7 +1752,7 @@ void StaticObstacleAvoidanceModule::insertWaitPoint( return; } - if (data.to_stop_line < 0.0) { + if (data.to_stop_line < -1.0 * parameters_->stop_buffer) { RCLCPP_WARN(getLogger(), "ego overran avoidance dead line. do nothing."); return; }