feat: improve lanelet search logic in getPredictedReferencePath()

technolojin · Oct 22, 2024 · 5de95b0 · 5de95b0
1 parent b864937
commit 5de95b0
Showing 1 changed file with 9 additions and 0 deletions.
diff --git 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<PredictedRefPath> 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