fix(start_planner): use non shifted path is shift line idx invalid (#…

…4227) Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
autowarefoundation · Jul 11, 2023 · 7bc034a · 7bc034a
1 parent b8374ec
commit 7bc034a
Showing 1 changed file with 7 additions and 0 deletions.
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
@@ -249,6 +249,13 @@ std::vector<PullOutPath> 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;