diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp index 8200db93ae2fb..5cece81f91a44 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/costmap_generator/nodes/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/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index 7cc251a03296c..04c63299bcdc9 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -41,11 +41,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;