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