From 97280f361f67b4ba627ea34e95741d4408bd3c4f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Sun, 17 Dec 2023 16:18:11 +0900 Subject: [PATCH] fix(static_drivable_area_expansion): fix bug in expansion logic for hatched road marking (#5842) fix(utils): fix drivable area expansion logic for zebra zone Signed-off-by: satoshi-ota --- planning/behavior_path_planner/src/utils/utils.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 39d31491b961d..54666576caa96 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1586,6 +1586,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())); @@ -1620,6 +1622,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(); }