fix(behavior_path_planner): add guard to extend lane

Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Oct 25, 2023 · 6cae986 · 6cae986
1 parent 6e60536
commit 6cae986
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -3104,6 +3104,8 @@ lanelet::ConstLanelets getCurrentLanesFromPath(
 lanelet::ConstLanelets extendNextLane(
   const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes)
 {
+  if (lanes.empty()) return lanes;
+
   auto extended_lanes = lanes;
 
   // Add next lane
@@ -3125,6 +3127,8 @@ lanelet::ConstLanelets extendNextLane(
 lanelet::ConstLanelets extendPrevLane(
   const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes)
 {
+  if (lanes.empty()) return lanes;
+
   auto extended_lanes = lanes;
 
   // Add previous lane