From 7bc034ad176c6784b3a8751ad421d2ba03c1c4dc Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 11 Jul 2023 11:14:51 +0900 Subject: [PATCH] fix(start_planner): use non shifted path is shift line idx invalid (#4227) Signed-off-by: Zulfaqar Azmi --- .../src/utils/start_planner/shift_pull_out.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 9d21dbae99171..1612280c14872 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -249,6 +249,13 @@ std::vector ShiftPullOut::calcPullOutPaths( path_shifter.setLongitudinalAcceleration(longitudinal_acc); path_shifter.setLateralAccelerationLimit(lateral_acc); + const auto shift_line_idx = path_shifter.getShiftLines().front(); + if (!has_non_shifted_path && (shift_line_idx.end_idx - shift_line_idx.start_idx <= 1)) { + candidate_paths.push_back(non_shifted_path); + has_non_shifted_path = true; + continue; + } + // offset front side ShiftedPath shifted_path; const bool offset_back = false;