From dcb6b1042cdcfe17bf53dccf9e0ad24a986f7c84 Mon Sep 17 00:00:00 2001
From: Yuki Takagi <yuki.takagi@tier4.jp>
Date: Tue, 14 May 2024 19:35:55 +0900
Subject: [PATCH] fix out of range access

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 planning/obstacle_cruise_planner/src/node.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
index 37c567a9637b8..c4383341314a7 100644
--- a/planning/obstacle_cruise_planner/src/node.cpp
+++ b/planning/obstacle_cruise_planner/src/node.cpp
@@ -1181,6 +1181,14 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
       if (id == 0) {
         continue;
       }
+
+      if (!route_handler_->getLaneletMapPtr()->polygonLayer.exists(id)) {
+        RCLCPP_DEBUG(
+          rclcpp::get_logger("ObstacleCruisePlanner"),
+          "Specified Lanelet polygon id [%ld] is not exsit in the map", id);
+        continue;
+      }
+
       const auto intersection_poly =
         lanelet::utils::to2D(route_handler_->getLaneletMapPtr()->polygonLayer.get(id))
           .basicPolygon();