diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index dba9ecf8eaef8..9ecdca2d9f12e 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -603,6 +603,10 @@ boost::optional getNearestLaneId( nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex( path.points, current_pose, std::numeric_limits::max(), M_PI_2); + if (!nearest_segment_idx) { + return boost::none; + } + lanelet::ConstLanelets current_lanes; if ( lanelet::utils::query::getCurrentLanelets(