From 252aa1a3a8c85472f01ae4c82117bcb237209cf8 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Wed, 10 Jul 2024 13:23:12 +0900 Subject: [PATCH] perf(query): add faster getLinkedParkingLot function (#19) Signed-off-by: Maxime CLEMENT --- .../utility/query.hpp | 4 ++++ autoware_lanelet2_extension/lib/query.cpp | 16 ++++++++++++++++ 2 files changed, 20 insertions(+) diff --git a/autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/query.hpp b/autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/query.hpp index 9ed44e2..4af3bf1 100644 --- 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 index f07cf58..8b1ad6d 100644 --- 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(