diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 552e51ccc1b5b..499115f07f4fc 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -1550,6 +1550,15 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( if (!unconnected_lanelets.empty()) { return unconnected_lanelets.front(); } + // search side of the next lanelet + const lanelet::ConstLanelets next_lanelet = routing_graph_ptr_->following(lanelet); + if (!next_lanelet.empty()) { + const auto next = get_left ? routing_graph_ptr_->left(next_lanelet.front()) + : routing_graph_ptr_->right(next_lanelet.front()); + if (!!next) { + return *next; + } + } } // if no candidate lanelet found, return empty