From 9a6da84951cc2668ef7565b259db24a6be715c34 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Sat, 9 Dec 2023 00:37:53 +0900 Subject: [PATCH] fix(avoidance): fix avoidance return dead point calculation logic for high curverture path (#5489) (#1063) Signed-off-by: satoshi-ota --- .../src/scene_module/avoidance/avoidance_module.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 4342629ca57f6..9e8d33924ac5e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2956,11 +2956,19 @@ void AvoidanceModule::insertReturnDeadLine( return; } + // Consider the difference in path length between the shifted path and original path (the path + // that is shifted inward has a shorter distance to the end of the path than the other one.) + const auto & to_reference_path_end = data.arclength_from_ego.back(); + const auto to_shifted_path_end = calcSignedArcLength( + shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); + const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); + const auto min_return_distance = helper_.getMinAvoidanceDistance(shift_length); const auto to_goal = calcSignedArcLength( shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); - const auto to_stop_line = to_goal - min_return_distance - parameters_->remain_buffer_distance; + const auto to_stop_line = + to_goal - min_return_distance - parameters_->remain_buffer_distance - buffer; // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately