From 967449ada56c9c02f60da315d494a6c80a9d1fed Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 18 May 2023 01:03:03 +0900 Subject: [PATCH 1/2] fix(behavior_path_planner): extract obstacles from drivable area without intersection Signed-off-by: Takayuki Murooka --- .../behavior_path_planner/src/utils/utils.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 4babccc812dbe..405b281637d31 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -224,6 +224,7 @@ std::vector generatePolygonInsideBounds( full_polygon.push_back(polygon_point); } + // 1. check the case where the polygon intersects the bound std::vector inside_poly; bool has_intersection = false; // NOTE: between obstacle polygon and bound for (int i = 0; i < static_cast(full_polygon.size()); ++i) { @@ -262,10 +263,25 @@ std::vector generatePolygonInsideBounds( inside_poly.push_back(intersect_point); continue; } - if (has_intersection) { return inside_poly; } + + // 2. check the case where the polygon does not intersect the bound + const bool is_polygon_fully_inside_bounds = [&]() { + for (int i = 0; i < static_cast(full_polygon.size()); ++i) { + const auto & curr_poly = full_polygon.at(i); + const bool is_curr_outside = curr_poly.is_outside_bounds(is_object_right); + if (is_curr_outside) { + return false; + } + } + return true; + }(); + if (is_polygon_fully_inside_bounds) { + return full_polygon; + } + return std::vector{}; } From 95faf3a39ef3eafb0714673b46fe5831af8178e2 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 18 May 2023 01:42:43 +0900 Subject: [PATCH 2/2] update Signed-off-by: Takayuki Murooka --- planning/behavior_path_planner/src/utils/utils.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 405b281637d31..554a0fe081b5c 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -269,8 +269,7 @@ std::vector generatePolygonInsideBounds( // 2. check the case where the polygon does not intersect the bound const bool is_polygon_fully_inside_bounds = [&]() { - for (int i = 0; i < static_cast(full_polygon.size()); ++i) { - const auto & curr_poly = full_polygon.at(i); + for (const auto & curr_poly : full_polygon) { const bool is_curr_outside = curr_poly.is_outside_bounds(is_object_right); if (is_curr_outside) { return false;