From d9b3b20ccad096c2aac7fc5dd88cdf13b91e5a0e Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 23 Aug 2023 18:09:34 +0900 Subject: [PATCH] fix(utils): drivable area generation supports anti-clockwise polygon (#4678) Signed-off-by: satoshi-ota --- planning/behavior_path_planner/src/utils/utils.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 506b3ff2d634b..e64fe41463e48 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1537,6 +1537,10 @@ std::vector calcBound( const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + const auto is_clockwise_polygon = + boost::geometry::is_valid(lanelet::utils::to2D(polygon.basicPolygon())); + const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; + const auto start_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { return p.id() == bound.front().id(); }); @@ -1552,7 +1556,8 @@ std::vector calcBound( // extract line strings between start_idx and end_idx. const size_t start_idx = std::distance(polygon.begin(), start_itr); const size_t end_idx = std::distance(polygon.begin(), end_itr); - for (const auto & point : extract_bound_from_polygon(polygon, start_idx, end_idx, is_left)) { + for (const auto & point : + extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration)) { output_points.push_back(point); }