perf(map_based_prediction): remove query on all fences linestrings (a…

…utowarefoundation#7237) Signed-off-by: Maxime CLEMENT <[email protected]> Signed-off-by: palas21 <[email protected]>
palas21 · Jul 12, 2024 · 2f65be9 · 2f65be9
1 parent 1a4a192
commit 2f65be9
Showing 1 changed file with 8 additions and 4 deletions.
diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
@@ -1309,10 +1309,14 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(
 
 bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path)
 {
-  const lanelet::ConstLineStrings3d & all_fences =
-    lanelet::utils::query::getAllFences(lanelet_map_ptr_);
-  for (const auto & fence_line : all_fences) {
-    if (doesPathCrossFence(predicted_path, fence_line)) {
+  lanelet::BasicLineString2d predicted_path_ls;
+  for (const auto & p : predicted_path.path)
+    predicted_path_ls.emplace_back(p.position.x, p.position.y);
+  const auto candidates =
+    lanelet_map_ptr_->lineStringLayer.search(lanelet::geometry::boundingBox2d(predicted_path_ls));
+  for (const auto & candidate : candidates) {
+    const std::string type = candidate.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type == "fence" && doesPathCrossFence(predicted_path, candidate)) {
       return true;
     }
   }