From 24e97836d79b99876e3698ff9d58c73c5ad56a43 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Fri, 5 Jul 2024 13:14:39 +0900 Subject: [PATCH] perf(map_based_prediction): remove query on all fences linestrings (#7237) Signed-off-by: Maxime CLEMENT --- .../src/map_based_prediction_node.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 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 index 2b8358eb52e4e..4f223d5c99bd6 100644 --- 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; } }