diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 364896347bf2a..33f76eee6b003 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1707,11 +1707,13 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c extended_lanelets.push_back(lanelet_at_left.get()); lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); } - auto lanelet_at_right = - planner_data_->route_handler->getRightLanelet(lanelet_at_left.get()); - while (lanelet_at_right) { - extended_lanelets.push_back(lanelet_at_right.get()); - lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); + if (lanelet_at_left) { + auto lanelet_at_right = + planner_data_->route_handler->getRightLanelet(lanelet_at_left.get()); + while (lanelet_at_right) { + extended_lanelets.push_back(lanelet_at_right.get()); + lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); + } } } else { auto lanelet_at_right = route_handler->getRightLanelet(object_lanelet); @@ -1719,10 +1721,12 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c extended_lanelets.push_back(lanelet_at_right.get()); lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); } - auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get()); - while (lanelet_at_left) { - extended_lanelets.push_back(lanelet_at_left.get()); - lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); + if (lanelet_at_right) { + auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get()); + while (lanelet_at_left) { + extended_lanelets.push_back(lanelet_at_left.get()); + lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); + } } } }