fix(static_drivable_area_expansion): fix bug in expansion logic for h…

…atched road marking (#5842) fix(utils): fix drivable area expansion logic for zebra zone Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · Dec 17, 2023 · 2175b57 · 2175b57
1 parent 90d5b94
commit 2175b57
Showing 1 changed file with 13 additions and 0 deletions.
diff --git a/...g/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/...g/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
@@ -1183,6 +1183,8 @@ std::vector<lanelet::ConstPoint3d> 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<lanelet::ConstPoint3d> 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();
     }