fix(lane_change): add guard to neigibor lanelets (autowarefoundation#…

…5245) Signed-off-by: kosuke55 <[email protected]>
tier4 · Oct 13, 2023 · cc5d206 · cc5d206
1 parent 7ac2439
commit cc5d206
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 0 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -942,6 +942,9 @@ bool NormalLaneChange::getLaneChangePaths(
 
   const auto target_neighbor_preferred_lane_poly_2d =
     utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_);
+  if (target_neighbor_preferred_lane_poly_2d.empty()) {
+    return false;
+  }
 
   const auto target_objects = getTargetObjects(current_lanes, target_lanes);
   debug_filtered_objects_ = target_objects;

diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp
@@ -242,6 +242,9 @@ lanelet::BasicPolygon2d getTargetNeighborLanesPolygon(
 {
   const auto target_neighbor_lanelets =
     utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type);
+  if (target_neighbor_lanelets.empty()) {
+    return {};
+  }
   const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength(
     target_neighbor_lanelets, 0, std::numeric_limits<double>::max());