fix(lane_change): empty check

Signed-off-by: Fumiya Watanabe <[email protected]>
autowarefoundation · May 29, 2024 · 8ce22af · 8ce22af
1 parent a831064
commit 8ce22af
Showing 1 changed file with 8 additions and 2 deletions.
diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp
@@ -646,8 +646,14 @@ std::vector<DrivableLanes> generateDrivableLanes(
 
 double getLateralShift(const LaneChangePath & path)
 {
-  const auto start_idx = path.info.shift_line.start_idx;
-  const auto end_idx = path.info.shift_line.end_idx;
+  if (path.shifted_path.shift_length.empty()) {
+    return 0.0;
+  }
+
+  const auto start_idx =
+    std::min(path.info.shift_line.start_idx, path.shifted_path.shift_length.size() - 1);
+  const auto end_idx =
+    std::min(path.info.shift_line.end_idx, path.shifted_path.shift_length.size() - 1);
 
   return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx);
 }