Merge branch 'beta/v0.19.1' into publish_control_mode_before_ego_pos

tier4 · May 16, 2024 · fde35b7 · fde35b7
2 parents 44f32fe + 61ab202
commit fde35b7
Showing 1 changed file with 8 additions and 0 deletions.
diff --git 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();