perf(map_based_prediction): create a fence LineString layer and use r…

…tree query (autowarefoundation#8406) use fence layer Signed-off-by: Mamoru Sobue <[email protected]>
technolojin · Aug 15, 2024 · bb2f1a2 · bb2f1a2
1 parent e71f3aa
commit bb2f1a2
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 4 deletions.
diff --git a/.../autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/.../autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -39,6 +39,7 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <lanelet2_core/Forward.h>
+#include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/Forward.h>
 #include <lanelet2_traffic_rules/TrafficRules.h>
 
@@ -178,6 +179,9 @@ class MapBasedPredictionNode : public rclcpp::Node
   // Crosswalk Entry Points
   lanelet::ConstLanelets crosswalks_;
 
+  // Fences
+  lanelet::LaneletMapUPtr fence_layer_{nullptr};
+
   // Parameters
   bool enable_delay_compensation_;
   PredictionTimeHorizon prediction_time_horizon_;

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
@@ -929,6 +929,16 @@ void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg
   const auto walkways = lanelet::utils::query::walkwayLanelets(all_lanelets);
   crosswalks_.insert(crosswalks_.end(), crosswalks.begin(), crosswalks.end());
   crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end());
+
+  lanelet::LineStrings3d fences;
+  for (const auto & linestring : lanelet_map_ptr_->lineStringLayer) {
+    if (const std::string type = linestring.attributeOr(lanelet::AttributeName::Type, "none");
+        type == "fence") {
+      fences.push_back(lanelet::LineString3d(
+        std::const_pointer_cast<lanelet::LineStringData>(linestring.constData())));
+    }
+  }
+  fence_layer_ = lanelet::utils::createMap(fences);
 }
 
 void MapBasedPredictionNode::trafficSignalsCallback(
@@ -1314,10 +1324,14 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(
 bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path)
 {
   autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
-  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 =
+    fence_layer_->lineStringLayer.search(lanelet::geometry::boundingBox2d(predicted_path_ls));
+  for (const auto & candidate : candidates) {
+    if (doesPathCrossFence(predicted_path, candidate)) {
       return true;
     }
   }