diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index f6344b94a6bc..af4bebe183d1 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -1183,6 +1183,8 @@ std::vector getBoundWithHatchedRoadMarkings( } else { if (!polygon) { will_close_polygon = true; + } else if (polygon.value().id() != current_polygon.value().id()) { + will_close_polygon = true; } else { current_polygon_border_indices.push_back( get_corresponding_polygon_index(*current_polygon, bound_point.id())); @@ -1217,6 +1219,17 @@ std::vector getBoundWithHatchedRoadMarkings( (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)]); } } + + if (polygon.has_value() && current_polygon.has_value()) { + if (polygon.value().id() != current_polygon.value().id()) { + current_polygon = polygon; + current_polygon_border_indices.clear(); + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(current_polygon.value(), bound_point.id())); + continue; + } + } + current_polygon = std::nullopt; current_polygon_border_indices.clear(); }