perf(lanelet2_extension): more efficient getLinkedParkingLot

Signed-off-by: Maxime CLEMENT <[email protected]>
autowarefoundation · Jun 14, 2024 · 56898c1 · 56898c1
1 parent c3fa064
commit 56898c1
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 11 deletions.
diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp
@@ -189,7 +189,7 @@ bool getLinkedParkingLot(
   lanelet::ConstPolygon3d * linked_parking_lot);
 // get linked parking lot from current pose of ego car
 bool getLinkedParkingLot(
-  const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots,
+  const lanelet::BasicPoint2d & current_position, const lanelet::LaneletMapPtr & lanelet_map_ptr,
   lanelet::ConstPolygon3d * linked_parking_lot);
 // get linked parking lot from parking space
 bool getLinkedParkingLot(

diff --git a/tmp/lanelet2_extension/lib/query.cpp b/tmp/lanelet2_extension/lib/query.cpp
@@ -591,15 +591,20 @@ bool query::getLinkedParkingLot(
 
 // get overlapping parking lot
 bool query::getLinkedParkingLot(
-  const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots,
+  const lanelet::BasicPoint2d & current_position, const lanelet::LaneletMapPtr & lanelet_map_ptr,
   lanelet::ConstPolygon3d * linked_parking_lot)
 {
-  for (const auto & parking_lot : all_parking_lots) {
-    const double distance =
-      boost::geometry::distance(current_position, to2D(parking_lot).basicPolygon());
-    if (distance < std::numeric_limits<double>::epsilon()) {
-      *linked_parking_lot = parking_lot;
-      return true;
+  const auto candidates =
+    lanelet_map_ptr->polygonLayer.search(lanelet::geometry::boundingBox2d(current_position));
+  for (const auto & candidate : candidates) {
+    const std::string type = candidate.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type == "parking_lot") {
+      const double distance =
+        boost::geometry::distance(current_position, to2D(candidate).basicPolygon());
+      if (distance < std::numeric_limits<double>::epsilon()) {
+        *linked_parking_lot = candidate;
+        return true;
+      }
     }
   }
   return false;

diff --git a/tmp/lanelet2_extension_python/src/utility.cpp b/tmp/lanelet2_extension_python/src/utility.cpp
@@ -209,11 +209,11 @@ lanelet::Optional<lanelet::ConstPolygon3d> getLinkedParkingLot(
 }
 
 lanelet::Optional<lanelet::ConstPolygon3d> getLinkedParkingLot(
-  const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots)
+  const lanelet::BasicPoint2d & current_position, const lanelet::LaneletMapPtr lanelet_map_ptr)
 {
   lanelet::ConstPolygon3d linked_parking_lot;
   if (lanelet::utils::query::getLinkedParkingLot(
-        current_position, all_parking_lots, &linked_parking_lot)) {
+        current_position, lanelet_map_ptr, &linked_parking_lot)) {
     return linked_parking_lot;
   } else {
     return {};
@@ -506,7 +506,7 @@ BOOST_PYTHON_MODULE(_lanelet2_extension_python_boost_python_utility)
     const lanelet::ConstLanelet &, const lanelet::ConstPolygons3d &)>(
     "getLinkedParkingLot", ::getLinkedParkingLot);
   bp::def<lanelet::Optional<lanelet::ConstPolygon3d>(
-    const lanelet::BasicPoint2d &, const lanelet::ConstPolygons3d &)>(
+    const lanelet::BasicPoint2d &, const lanelet::LaneletMapPtr)>(
     "getLinkedParkingLot", ::getLinkedParkingLot);
   bp::def<lanelet::Optional<lanelet::ConstPolygon3d>(
     const lanelet::ConstLineString3d &, const lanelet::ConstPolygons3d &)>(