perf(query): add faster getLinkedParkingLot function

Signed-off-by: Maxime CLEMENT <[email protected]>
autowarefoundation · Jul 10, 2024 · aa3ce3c · aa3ce3c
1 parent 98b1edb
commit aa3ce3c
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 0 deletions.
diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/query.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/query.hpp
@@ -167,6 +167,10 @@ bool getLinkedParkingLot(
 bool getLinkedParkingLot(
   const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots,
   lanelet::ConstPolygon3d * linked_parking_lot);
+bool getLinkedParkingLot(
+  const lanelet::BasicPoint2d & current_position,
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr,
+  lanelet::ConstPolygon3d * linked_parking_lot);
 // get linked parking lot from parking space
 bool getLinkedParkingLot(
   const lanelet::ConstLineString3d & parking_space,

diff --git a/autoware_lanelet2_extension/lib/query.cpp b/autoware_lanelet2_extension/lib/query.cpp
@@ -549,6 +549,22 @@ bool getLinkedParkingLot(
   }
   return false;
 }
+bool getLinkedParkingLot(
+  const lanelet::BasicPoint2d & current_position,
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstPolygon3d * linked_parking_lot)
+{
+  auto candidates =
+    lanelet_map_ptr->polygonLayer.search(lanelet::geometry::boundingBox2d(current_position));
+  candidates.erase(
+    std::remove_if(
+      candidates.begin(), candidates.end(),
+      [](const auto & c) {
+        const std::string type = c.attributeOr(lanelet::AttributeName::Type, "none");
+        return type != "parking_lot";
+      }),
+    candidates.end());
+  return getLinkedParkingLot(current_position, candidates, linked_parking_lot);
+}
 
 // get overlapping parking lot
 bool getLinkedParkingLot(