From ed049ba895baafecf5ca689a7f006da11375785f Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 22 Jul 2024 14:55:36 +0900 Subject: [PATCH] perf(costmap_generator, scenario_selector): faster getLinkedParkingLot (#7930) Signed-off-by: Maxime CLEMENT --- .../autoware_costmap_generator/costmap_generator_node.cpp | 4 +--- planning/autoware_scenario_selector/src/node.cpp | 4 +--- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp index 3cccd9fab0b48..b77aa31947980 100644 --- a/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp @@ -95,11 +95,9 @@ std::shared_ptr findNearestParkinglot( const std::shared_ptr & lanelet_map_ptr, const lanelet::BasicPoint2d & current_position) { - const auto all_parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr); - const auto linked_parking_lot = std::make_shared(); const auto result = lanelet::utils::query::getLinkedParkingLot( - current_position, all_parking_lots, linked_parking_lot.get()); + current_position, lanelet_map_ptr, linked_parking_lot.get()); if (result) { return linked_parking_lot; diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index 0e204ae82146c..54496a66910b0 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -37,11 +37,9 @@ std::shared_ptr findNearestParkinglot( const std::shared_ptr & lanelet_map_ptr, const lanelet::BasicPoint2d & current_position) { - const auto all_parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr); - const auto linked_parking_lot = std::make_shared(); const auto result = lanelet::utils::query::getLinkedParkingLot( - current_position, all_parking_lots, linked_parking_lot.get()); + current_position, lanelet_map_ptr, linked_parking_lot.get()); if (result) { return linked_parking_lot;