From 5de95b01c51cabadc42cf11f83a534ff7cf4f11e Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 22 Oct 2024 15:16:06 +0900 Subject: [PATCH] feat: improve lanelet search logic in getPredictedReferencePath() --- .../src/map_based_prediction_node.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) 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