From 6dddb2221027ca52bdf451feab32b380b2ea6988 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 13 Nov 2023 09:45:46 +0900 Subject: [PATCH] fix(avoidance): fix avoidance exit condition Signed-off-by: satoshi-ota --- .../src/scene_module/avoidance/avoidance_module.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) 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 2d85f8cbe3224..46dfd93d83432 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 @@ -222,6 +222,19 @@ bool AvoidanceModule::canTransitSuccessState() const bool has_base_offset = std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; + // If the vehicle is on the shift line, keep RUNNING. + { + const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); + const auto within = [](const auto & line, const size_t idx) { + return line.start_idx < idx && idx < line.end_idx; + }; + for (const auto & shift_line : path_shifter_.getShiftLines()) { + if (within(shift_line, idx)) { + return false; + } + } + } + // Nothing to do. -> EXIT. if (!has_avoidance_target) { if (!has_shift_point && !has_base_offset) {