From 83d316c6201141da1cce70aa3fc94a828faa157e Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 25 Dec 2023 02:41:58 +0900 Subject: [PATCH] fix(behavior_path_planner_common): added a guard of obstacle polygon points (#5945) * fix(behavior_path_planner_common): added a guard of obstacle polygon points Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../utils/drivable_area_expansion/static_drivable_area.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) 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 ef676c8aaeb8b..1d4af1c7a65d2 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 @@ -1085,13 +1085,17 @@ void extractObstaclesFromDrivableArea( std::vector> right_polygons; std::vector> left_polygons; for (const auto & obstacle : obstacles) { + if (obstacle.poly.outer().empty()) { + continue; + } + const auto & obj_pos = obstacle.pose.position; // get edge points of the object const size_t nearest_path_idx = motion_utils::findNearestIndex(path.points, obj_pos); // to get z for object polygon std::vector edge_points; - for (size_t i = 0; i < obstacle.poly.outer().size() - 1; + for (int i = 0; i < static_cast(obstacle.poly.outer().size()) - 1; ++i) { // NOTE: There is a duplicated points edge_points.push_back(tier4_autoware_utils::createPoint( obstacle.poly.outer().at(i).x(), obstacle.poly.outer().at(i).y(),