From 3e0d3c74e1021f9331a1a5b6545a4939d15faf40 Mon Sep 17 00:00:00 2001 From: T-Kimura-MM <119379767+T-Kimura-MM@users.noreply.github.com> Date: Thu, 24 Oct 2024 18:50:44 +0900 Subject: [PATCH] fix(behavior_path_planner, behavior_velocity_planner): fix to not read invalid ID (#9103) * fix(behavior_path_planner, behavior_velocity_planner): fix to not read invalid ID Signed-off-by: T-Kimura-MM * style(pre-commit): autofix * fix typo Signed-off-by: T-Kimura-MM * fix(behavior_path_planner, behavior_velocity_planner): fix typo and indentation Signed-off-by: T-Kimura-MM --------- Signed-off-by: T-Kimura-MM Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/utils/utils.cpp | 12 ++++++++++-- .../drivable_area_expansion/static_drivable_area.cpp | 12 ++++++++++-- .../src/utils.cpp | 12 ++++++++++-- .../src/util.cpp | 4 ++++ 4 files changed, 34 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index c900b7fe54302..455792733ac97 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -1001,8 +1001,16 @@ bool isWithinIntersection( return false; } - const auto lanelet_polygon = - route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + if (!std::atoi(id.c_str())) { + return false; + } + + const auto lanelet_polygon_opt = + route_handler->getLaneletMapPtr()->polygonLayer.find(std::atoi(id.c_str())); + if (lanelet_polygon_opt == route_handler->getLaneletMapPtr()->polygonLayer.end()) { + return false; + } + const auto & lanelet_polygon = *lanelet_polygon_opt; return boost::geometry::within( polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet_polygon.basicPolygon()))); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 3d5908bc4e02f..9c35032e51063 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -1193,11 +1193,19 @@ std::vector getBoundWithIntersectionAreas( continue; } + if (!std::atoi(id.c_str())) { + continue; + } + // Step1. extract intersection partial bound. std::vector intersection_bound{}; { - const auto polygon = - route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + const auto polygon_opt = + route_handler->getLaneletMapPtr()->polygonLayer.find(std::atoi(id.c_str())); + if (polygon_opt == route_handler->getLaneletMapPtr()->polygonLayer.end()) { + continue; + } + const auto & polygon = *polygon_opt; const auto is_clockwise_polygon = boost::geometry::is_valid(lanelet::utils::to2D(polygon.basicPolygon())); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 091a15bea158b..72b75586d42c5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -282,13 +282,21 @@ bool isWithinIntersection( return false; } + if (!std::atoi(area_id.c_str())) { + return false; + } + const std::string location = object.overhang_lanelet.attributeOr("location", "else"); if (location == "private") { return false; } - const auto polygon = - route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str())); + const auto polygon_opt = + route_handler->getLaneletMapPtr()->polygonLayer.find(std::atoi(area_id.c_str())); + if (polygon_opt == route_handler->getLaneletMapPtr()->polygonLayer.end()) { + return false; + } + const auto & polygon = *polygon_opt; return boost::geometry::within( lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp index a446493793c53..6474435afa26e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp @@ -342,8 +342,12 @@ std::optional getIntersectionArea( { const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); if (area_id_str == "else") return std::nullopt; + if (!std::atoi(area_id_str.c_str())) return std::nullopt; const lanelet::Id area_id = std::atoi(area_id_str.c_str()); + const auto polygon_opt = lanelet_map_ptr->polygonLayer.find(area_id); + if (polygon_opt == lanelet_map_ptr->polygonLayer.end()) return std::nullopt; + const auto poly_3d = lanelet_map_ptr->polygonLayer.get(area_id); Polygon2d poly{}; for (const auto & p : poly_3d) poly.outer().emplace_back(p.x(), p.y());