fix(behavior_path_planner, behavior_velocity_planner): fix to not rea…

…d invalid ID (autowarefoundation#9103)

* fix(behavior_path_planner, behavior_velocity_planner): fix to not read invalid ID

Signed-off-by: T-Kimura-MM <tkc1110bs@gmail.com>

* style(pre-commit): autofix

* fix typo

Signed-off-by: T-Kimura-MM <tkc1110bs@gmail.com>

* fix(behavior_path_planner, behavior_velocity_planner): fix typo and indentation

Signed-off-by: T-Kimura-MM <tkc1110bs@gmail.com>

---------

Signed-off-by: T-Kimura-MM <tkc1110bs@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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())));

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

@@ -1193,11 +1193,19 @@ std::vector<lanelet::ConstPoint3d> getBoundWithIntersectionAreas(
       continue;
     }
 
+    if (!std::atoi(id.c_str())) {
+      continue;
+    }
+
     // Step1. extract intersection partial bound.
     std::vector<lanelet::ConstPoint3d> 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()));

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()))

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/util.cpp

@@ -342,8 +342,12 @@ std::optional<autoware::universe_utils::Polygon2d> 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());