diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
index a890cfc21ed13..d423b8c922ee7 100644
--- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
+++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -447,12 +447,10 @@ bool GoalPlannerModule::isExecutionRequested() const
 
   // check that goal is in current neighbor shoulder lane
   const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() {
-    lanelet::ConstLanelet neighbor_shoulder_lane{};
     for (const auto & lane : current_lanes) {
-      const bool has_shoulder_lane =
-        left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane, &neighbor_shoulder_lane)
-                           : route_handler->getRightShoulderLanelet(lane, &neighbor_shoulder_lane);
-      if (has_shoulder_lane && lanelet::utils::isInLanelet(goal_pose, neighbor_shoulder_lane)) {
+      const auto shoulder_lane = left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane)
+                                                    : route_handler->getRightShoulderLanelet(lane);
+      if (shoulder_lane && lanelet::utils::isInLanelet(goal_pose, *shoulder_lane)) {
         return true;
       }
     }
@@ -1897,20 +1895,11 @@ bool GoalPlannerModule::isCrossingPossible(
   // Lambda function to get the neighboring lanelet based on left_side_parking_
   auto getNeighboringLane =
     [&](const lanelet::ConstLanelet & lane) -> std::optional<lanelet::ConstLanelet> {
-    lanelet::ConstLanelet neighboring_lane{};
-    if (left_side_parking_) {
-      if (route_handler->getLeftShoulderLanelet(lane, &neighboring_lane)) {
-        return neighboring_lane;
-      } else {
-        return route_handler->getLeftLanelet(lane);
-      }
-    } else {
-      if (route_handler->getRightShoulderLanelet(lane, &neighboring_lane)) {
-        return neighboring_lane;
-      } else {
-        return route_handler->getRightLanelet(lane);
-      }
-    }
+    const auto neighboring_lane = left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane)
+                                                     : route_handler->getRightShoulderLanelet(lane);
+    if (neighboring_lane) return neighboring_lane;
+    return left_side_parking_ ? route_handler->getLeftLanelet(lane)
+                              : route_handler->getRightLanelet(lane);
   };
 
   // Iterate through start_lane_sequence to find a path to end_lane_sequence
diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp
index dab0867a7e29b..72ae0143783ea 100644
--- a/planning/behavior_path_goal_planner_module/src/util.cpp
+++ b/planning/behavior_path_goal_planner_module/src/util.cpp
@@ -55,12 +55,11 @@ lanelet::ConstLanelets getPullOverLanes(
   // todo(kosuek55): automatically calculates this distance.
   const double backward_distance_with_buffer = backward_distance + 100;
 
-  lanelet::ConstLanelet target_shoulder_lane{};
-  if (route_handler::RouteHandler::getPullOverTarget(
-        route_handler.getShoulderLanelets(), goal_pose, &target_shoulder_lane)) {
+  const auto target_shoulder_lane = route_handler.getPullOverTarget(goal_pose);
+  if (target_shoulder_lane) {
     // pull over on shoulder lane
     return route_handler.getShoulderLaneletSequence(
-      target_shoulder_lane, goal_pose, backward_distance_with_buffer, forward_distance);
+      *target_shoulder_lane, goal_pose, backward_distance_with_buffer, forward_distance);
   }
 
   lanelet::ConstLanelet closest_lane{};
diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp
index 83b1affa763a4..cdb4b778d3ab3 100644
--- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp
+++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp
@@ -354,11 +354,9 @@ TargetObjectsOnLane createTargetObjectsOnLane(
   };
 
   const auto update_left_shoulder_lanelet = [&](const lanelet::ConstLanelet & target_lane) {
-    lanelet::ConstLanelet neighbor_shoulder_lane{};
-    const bool shoulder_lane_is_found =
-      route_handler->getLeftShoulderLanelet(target_lane, &neighbor_shoulder_lane);
-    if (shoulder_lane_is_found) {
-      all_left_shoulder_lanelets.insert(all_left_shoulder_lanelets.end(), neighbor_shoulder_lane);
+    const auto neighbor_shoulder_lane = route_handler->getLeftShoulderLanelet(target_lane);
+    if (neighbor_shoulder_lane) {
+      all_left_shoulder_lanelets.insert(all_left_shoulder_lanelets.end(), *neighbor_shoulder_lane);
     }
   };
 
diff --git a/planning/behavior_path_planner_common/src/utils/path_utils.cpp b/planning/behavior_path_planner_common/src/utils/path_utils.cpp
index b064e0a172824..d11bc5a68e9b5 100644
--- a/planning/behavior_path_planner_common/src/utils/path_utils.cpp
+++ b/planning/behavior_path_planner_common/src/utils/path_utils.cpp
@@ -645,15 +645,12 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr<const PlannerDat
   const auto & modified_goal = planner_data->prev_modified_goal;
 
   const Pose goal_pose = modified_goal ? modified_goal->pose : route_handler->getGoalPose();
-  const auto shoulder_lanes = route_handler->getShoulderLanelets();
 
   lanelet::ConstLanelet goal_lane;
-  const bool is_failed_getting_lanelet = std::invoke([&]() {
-    if (isInLanelets(goal_pose, shoulder_lanes)) {
-      return !lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane);
-    }
-    return !route_handler->getGoalLanelet(&goal_lane);
-  });
+  const auto shoulder_goal_lanes = route_handler->getShoulderLaneletsAtPose(goal_pose);
+  if (!shoulder_goal_lanes.empty()) goal_lane = shoulder_goal_lanes.front();
+  const auto is_failed_getting_lanelet =
+    shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane);
   if (is_failed_getting_lanelet) {
     return output;
   }
diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp
index 5282509b973db..d674f7770ac67 100644
--- a/planning/behavior_path_planner_common/src/utils/utils.cpp
+++ b/planning/behavior_path_planner_common/src/utils/utils.cpp
@@ -496,15 +496,12 @@ bool isEgoOutOfRoute(
   const Pose & goal_pose = (modified_goal && modified_goal->uuid == route_handler->getRouteUuid())
                              ? modified_goal->pose
                              : route_handler->getGoalPose();
-  const auto shoulder_lanes = route_handler->getShoulderLanelets();
 
   lanelet::ConstLanelet goal_lane;
-  const bool is_failed_getting_lanelet = std::invoke([&]() {
-    if (utils::isInLanelets(goal_pose, shoulder_lanes)) {
-      return !lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane);
-    }
-    return !route_handler->getGoalLanelet(&goal_lane);
-  });
+  const auto shoulder_goal_lanes = route_handler->getShoulderLaneletsAtPose(goal_pose);
+  if (!shoulder_goal_lanes.empty()) goal_lane = shoulder_goal_lanes.front();
+  const auto is_failed_getting_lanelet =
+    shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane);
   if (is_failed_getting_lanelet) {
     RCLCPP_WARN_STREAM(
       rclcpp::get_logger("behavior_path_planner").get_child("util"), "cannot find goal lanelet");
@@ -527,14 +524,7 @@ bool isEgoOutOfRoute(
 
   // If ego vehicle is out of the closest lanelet, return true
   // Check if ego vehicle is in shoulder lane
-  const bool is_in_shoulder_lane = std::invoke([&]() {
-    lanelet::Lanelet closest_shoulder_lanelet;
-    if (!lanelet::utils::query::getClosestLanelet(
-          shoulder_lanes, self_pose, &closest_shoulder_lanelet)) {
-      return false;
-    }
-    return lanelet::utils::isInLanelet(self_pose, closest_shoulder_lanelet);
-  });
+  const bool is_in_shoulder_lane = !route_handler->getShoulderLaneletsAtPose(self_pose).empty();
   // Check if ego vehicle is in road lane
   const bool is_in_road_lane = std::invoke([&]() {
     lanelet::ConstLanelet closest_road_lane;
@@ -1662,13 +1652,6 @@ bool isAllowedGoalModification(const std::shared_ptr<RouteHandler> & route_handl
 bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & route_handler)
 {
   const Pose & goal_pose = route_handler->getOriginalGoalPose();
-  const auto shoulder_lanes = route_handler->getShoulderLanelets();
-
-  lanelet::ConstLanelet closest_shoulder_lane{};
-  if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) {
-    return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1);
-  }
-
-  return false;
+  return !route_handler->getShoulderLaneletsAtPose(goal_pose).empty();
 }
 }  // namespace behavior_path_planner::utils
diff --git a/planning/behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_start_planner_module/src/util.cpp
index 82e9d1a08e816..4ed499826dbd1 100644
--- a/planning/behavior_path_start_planner_module/src/util.cpp
+++ b/planning/behavior_path_start_planner_module/src/util.cpp
@@ -90,11 +90,10 @@ lanelet::ConstLanelets getPullOutLanes(
   const auto & route_handler = planner_data->route_handler;
   const auto start_pose = planner_data->route_handler->getOriginalStartPose();
 
-  lanelet::ConstLanelet current_shoulder_lane;
-  if (route_handler->getPullOutStartLane(
-        route_handler->getShoulderLanelets(), start_pose, vehicle_width, &current_shoulder_lane)) {
+  const auto current_shoulder_lane = route_handler->getPullOutStartLane(start_pose, vehicle_width);
+  if (current_shoulder_lane) {
     // pull out from shoulder lane
-    return route_handler->getShoulderLaneletSequence(current_shoulder_lane, start_pose);
+    return route_handler->getShoulderLaneletSequence(*current_shoulder_lane, start_pose);
   }
 
   // pull out from road lane
diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp
index ac8e472b2f943..d13c0536cf3c1 100644
--- a/planning/route_handler/include/route_handler/route_handler.hpp
+++ b/planning/route_handler/include/route_handler/route_handler.hpp
@@ -27,6 +27,7 @@
 #include <lanelet2_core/Forward.h>
 #include <lanelet2_core/primitives/Lanelet.h>
 #include <lanelet2_routing/Forward.h>
+#include <lanelet2_routing/RoutingCost.h>
 #include <lanelet2_traffic_rules/TrafficRules.h>
 
 #include <limits>
@@ -77,6 +78,7 @@ class RouteHandler
   lanelet::traffic_rules::TrafficRulesPtr getTrafficRulesPtr() const;
   std::shared_ptr<const lanelet::routing::RoutingGraphContainer> getOverallGraphPtr() const;
   lanelet::LaneletMapPtr getLaneletMapPtr() const;
+  static bool isNoDrivableLane(const lanelet::ConstLanelet & llt);
 
   // for routing
   bool planPathLaneletsBetweenCheckpoints(
@@ -324,9 +326,9 @@ class RouteHandler
     const double check_length) const;
   lanelet::routing::RelationType getRelation(
     const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const;
-  lanelet::ConstLanelets getShoulderLanelets() const;
   bool isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const;
   bool isRouteLanelet(const lanelet::ConstLanelet & lanelet) const;
+  bool isRoadLanelet(const lanelet::ConstLanelet & lanelet) const;
   lanelet::ConstLanelets getPreferredLanelets() const;
 
   // for path
@@ -341,20 +343,18 @@ class RouteHandler
     const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const;
   bool getLeftLaneChangeTargetExceptPreferredLane(
     const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const;
-  static bool getPullOverTarget(
-    const lanelet::ConstLanelets & lanelets, const Pose & goal_pose,
-    lanelet::ConstLanelet * target_lanelet);
-  static bool getPullOutStartLane(
-    const lanelet::ConstLanelets & lanelets, const Pose & pose, const double vehicle_width,
-    lanelet::ConstLanelet * target_lanelet);
+  std::optional<lanelet::ConstLanelet> getPullOverTarget(const Pose & goal_pose) const;
+  std::optional<lanelet::ConstLanelet> getPullOutStartLane(
+    const Pose & pose, const double vehicle_width) const;
   double getLaneChangeableDistance(const Pose & current_pose, const Direction & direction) const;
-  bool getLeftShoulderLanelet(
-    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet) const;
-  bool getRightShoulderLanelet(
-    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const;
+  lanelet::ConstLanelets getRoadLaneletsAtPose(const Pose & pose) const;
+  std::optional<lanelet::ConstLanelet> getLeftShoulderLanelet(
+    const lanelet::ConstLanelet & lanelet) const;
+  std::optional<lanelet::ConstLanelet> getRightShoulderLanelet(
+    const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const;
   lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const;
   bool isPreferredLane(const lanelet::ConstLanelet & lanelet) const;
-  lanelet::ConstLanelets getClosestLanelets(const geometry_msgs::msg::Pose & target_pose) const;
 
 private:
   // MUST
@@ -362,12 +362,10 @@ class RouteHandler
   lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_;
   std::shared_ptr<const lanelet::routing::RoutingGraphContainer> overall_graphs_ptr_;
   lanelet::LaneletMapPtr lanelet_map_ptr_;
-  lanelet::ConstLanelets road_lanelets_;
   lanelet::ConstLanelets route_lanelets_;
   lanelet::ConstLanelets preferred_lanelets_;
   lanelet::ConstLanelets start_lanelets_;
   lanelet::ConstLanelets goal_lanelets_;
-  lanelet::ConstLanelets shoulder_lanelets_;
   std::shared_ptr<LaneletRoute> route_ptr_{nullptr};
 
   rclcpp::Logger logger_{rclcpp::get_logger("route_handler")};
@@ -409,19 +407,18 @@ class RouteHandler
     const lanelet::ConstLanelet & lanelet,
     const double min_length = std::numeric_limits<double>::max(),
     const bool only_route_lanes = true) const;
-  bool getFollowingShoulderLanelet(
-    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * following_lanelet) const;
+  std::optional<lanelet::ConstLanelet> getFollowingShoulderLanelet(
+    const lanelet::ConstLanelet & lanelet) const;
   lanelet::ConstLanelets getShoulderLaneletSequenceAfter(
     const lanelet::ConstLanelet & lanelet,
     const double min_length = std::numeric_limits<double>::max()) const;
-  bool getPreviousShoulderLanelet(
-    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const;
+  std::optional<lanelet::ConstLanelet> getPreviousShoulderLanelet(
+    const lanelet::ConstLanelet & lanelet) const;
   lanelet::ConstLanelets getShoulderLaneletSequenceUpTo(
     const lanelet::ConstLanelet & lanelet,
     const double min_length = std::numeric_limits<double>::max()) const;
   lanelet::ConstLanelets getPreviousLaneletSequence(
     const lanelet::ConstLanelets & lanelet_sequence) const;
-  lanelet::ConstLanelets getClosestLaneletSequence(const Pose & pose) const;
   lanelet::ConstLanelets getLaneChangeTargetLanes(const Pose & pose) const;
   lanelet::ConstLanelets getLaneSequenceUpTo(const lanelet::ConstLanelet & lanelet) const;
   lanelet::ConstLanelets getLaneSequenceAfter(const lanelet::ConstLanelet & lanelet) const;
@@ -441,17 +438,41 @@ class RouteHandler
    */
   bool hasNoDrivableLaneInPath(const lanelet::routing::LaneletPath & path) const;
   /**
-   * @brief Searches for a path between start and goal lanelets that does not include any
-   * no_drivable_lane. If there is more than one path fount, the function returns the shortest path
-   * that does not include any no_drivable_lane.
+   * @brief Searches for the shortest path between start and goal lanelets that does not include any
+   * no_drivable_lane.
    * @param start_lanelet start lanelet
    * @param goal_lanelet goal lanelet
-   * @param drivable_lane_path output path that does not include no_drivable_lane.
-   * @return true if a path without any no_drivable_lane found, false if this path is not found.
+   * @return the lanelet path (if found)
    */
-  bool findDrivableLanePath(
-    const lanelet::ConstLanelet & start_lanelet, const lanelet::ConstLanelet & goal_lanelet,
-    lanelet::routing::LaneletPath & drivable_lane_path) const;
+  std::optional<lanelet::routing::LaneletPath> findDrivableLanePath(
+    const lanelet::ConstLanelet & start_lanelet, const lanelet::ConstLanelet & goal_lanelet) const;
 };
+
+/// @brief custom routing cost with infinity cost for no drivable lanes
+class RoutingCostDrivable : public lanelet::routing::RoutingCostDistance
+{
+public:
+  RoutingCostDrivable() : lanelet::routing::RoutingCostDistance(10.0) {}
+  inline double getCostSucceeding(
+    const lanelet::traffic_rules::TrafficRules & trafficRules,
+    const lanelet::ConstLaneletOrArea & from, const lanelet::ConstLaneletOrArea & to) const
+  {
+    if (
+      (from.isLanelet() && RouteHandler::isNoDrivableLane(*from.lanelet())) ||
+      (to.isLanelet() && RouteHandler::isNoDrivableLane(*to.lanelet())))
+      return std::numeric_limits<double>::infinity();
+    return lanelet::routing::RoutingCostDistance::getCostSucceeding(trafficRules, from, to);
+  }
+  inline double getCostLaneChange(
+    const lanelet::traffic_rules::TrafficRules & trafficRules, const lanelet::ConstLanelets & from,
+    const lanelet::ConstLanelets & to) const noexcept
+  {
+    if (
+      std::any_of(from.begin(), from.end(), RouteHandler::isNoDrivableLane) ||
+      std::any_of(to.begin(), to.end(), RouteHandler::isNoDrivableLane))
+      return std::numeric_limits<double>::infinity();
+    return lanelet::routing::RoutingCostDistance::getCostLaneChange(trafficRules, from, to);
+  }
+};  // class RoutingCostDrivable
 }  // namespace route_handler
 #endif  // ROUTE_HANDLER__ROUTE_HANDLER_HPP_
diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp
index 76763821998bd..589a98d7d5ab4 100644
--- a/planning/route_handler/src/route_handler.cpp
+++ b/planning/route_handler/src/route_handler.cpp
@@ -157,8 +157,6 @@ void RouteHandler::setMap(const HADMapBin & map_msg)
   overall_graphs_ptr_ =
     std::make_shared<const lanelet::routing::RoutingGraphContainer>(overall_graphs);
   lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_);
-  road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets);
-  shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets);
 
   is_map_msg_ready_ = true;
   is_handler_ready_ = false;
@@ -646,7 +644,7 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence(
   const lanelet::ConstLanelets lanelet_sequence_forward = std::invoke([&]() {
     if (only_route_lanes) {
       return getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes);
-    } else if (lanelet::utils::contains(shoulder_lanelets_, lanelet)) {
+    } else if (isShoulderLanelet(lanelet)) {
       return getShoulderLaneletSequenceAfter(lanelet, forward_distance);
     }
     return lanelet::ConstLanelets{};
@@ -656,7 +654,7 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence(
     if (arc_coordinate.length < backward_distance) {
       if (only_route_lanes) {
         return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes);
-      } else if (lanelet::utils::contains(shoulder_lanelets_, lanelet)) {
+      } else if (isShoulderLanelet(lanelet)) {
         return getShoulderLaneletSequenceUpTo(lanelet, backward_distance);
       }
     }
@@ -712,110 +710,136 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence(
   return lanelet_sequence;
 }
 
-bool RouteHandler::getFollowingShoulderLanelet(
-  const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * following_lanelet) const
+lanelet::ConstLanelets RouteHandler::getRoadLaneletsAtPose(const Pose & pose) const
 {
-  for (const auto & shoulder_lanelet : shoulder_lanelets_) {
-    if (lanelet::geometry::follows(lanelet, shoulder_lanelet)) {
-      *following_lanelet = shoulder_lanelet;
-      return true;
-    }
+  lanelet::ConstLanelets road_lanelets_at_pose;
+  const lanelet::BasicPoint2d p{pose.position.x, pose.position.y};
+  const auto lanelets_at_pose = lanelet_map_ptr_->laneletLayer.search(lanelet::BoundingBox2d(p));
+  for (const auto & lanelet_at_pose : lanelets_at_pose) {
+    // confirm that the pose is inside the lanelet since "search" does an approximation with boxes
+    const auto is_pose_inside_lanelet = lanelet::geometry::inside(lanelet_at_pose, p);
+    if (is_pose_inside_lanelet && isRoadLanelet(lanelet_at_pose))
+      road_lanelets_at_pose.push_back(lanelet_at_pose);
   }
-  return false;
+  return road_lanelets_at_pose;
 }
 
-bool RouteHandler::getLeftShoulderLanelet(
-  const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet) const
+std::optional<lanelet::ConstLanelet> RouteHandler::getFollowingShoulderLanelet(
+  const lanelet::ConstLanelet & lanelet) const
 {
-  for (const auto & shoulder_lanelet : shoulder_lanelets_) {
-    if (lanelet::geometry::leftOf(shoulder_lanelet, lanelet)) {
-      *left_lanelet = shoulder_lanelet;
-      return true;
-    }
+  bool found = false;
+  const auto & search_point = lanelet.centerline().back().basicPoint2d();
+  const auto next_lanelet = lanelet_map_ptr_->laneletLayer.nearestUntil(
+    search_point, [&](const auto & bbox, const auto & ll) {
+      if (isShoulderLanelet(ll) && lanelet::geometry::follows(lanelet, ll)) found = true;
+      // stop search once next shoulder lanelet is found, or the bbox does not touch the search
+      // point
+      return found || lanelet::geometry::distance2d(bbox, search_point) > 1e-3;
+    });
+  if (found && next_lanelet.has_value()) return *next_lanelet;
+  return std::nullopt;
+}
+
+std::optional<lanelet::ConstLanelet> RouteHandler::getLeftShoulderLanelet(
+  const lanelet::ConstLanelet & lanelet) const
+{
+  for (const auto & other_lanelet :
+       lanelet_map_ptr_->laneletLayer.findUsages(lanelet.leftBound())) {
+    if (other_lanelet.rightBound() == lanelet.leftBound() && isShoulderLanelet(other_lanelet))
+      return other_lanelet;
   }
-  return false;
+  return std::nullopt;
 }
 
-bool RouteHandler::getRightShoulderLanelet(
-  const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const
+std::optional<lanelet::ConstLanelet> RouteHandler::getRightShoulderLanelet(
+  const lanelet::ConstLanelet & lanelet) const
 {
-  for (const auto & shoulder_lanelet : shoulder_lanelets_) {
-    if (lanelet::geometry::rightOf(shoulder_lanelet, lanelet)) {
-      *right_lanelet = shoulder_lanelet;
-      return true;
-    }
+  for (const auto & other_lanelet :
+       lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound())) {
+    if (other_lanelet.leftBound() == lanelet.rightBound() && isShoulderLanelet(other_lanelet))
+      return other_lanelet;
   }
-  return false;
+  return std::nullopt;
+}
+
+lanelet::ConstLanelets RouteHandler::getShoulderLaneletsAtPose(const Pose & pose) const
+{
+  lanelet::ConstLanelets lanelets_at_pose;
+  const lanelet::BasicPoint2d p{pose.position.x, pose.position.y};
+  const auto candidates_at_pose = lanelet_map_ptr_->laneletLayer.search(lanelet::BoundingBox2d(p));
+  for (const auto & candidate : candidates_at_pose) {
+    // confirm that the pose is inside the lanelet since "search" does an approximation with boxes
+    const auto is_pose_inside_lanelet = lanelet::geometry::inside(candidate, p);
+    if (is_pose_inside_lanelet && isShoulderLanelet(candidate))
+      lanelets_at_pose.push_back(candidate);
+  }
+  return lanelets_at_pose;
 }
 
 lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceAfter(
   const lanelet::ConstLanelet & lanelet, const double min_length) const
 {
   lanelet::ConstLanelets lanelet_sequence_forward;
-  if (!exists(shoulder_lanelets_, lanelet)) {
-    return lanelet_sequence_forward;
-  }
+  if (!isShoulderLanelet(lanelet)) return lanelet_sequence_forward;
 
   double length = 0;
   lanelet::ConstLanelet current_lanelet = lanelet;
   std::set<lanelet::Id> searched_ids{};
   while (rclcpp::ok() && length < min_length) {
-    lanelet::ConstLanelet next_lanelet;
-    if (!getFollowingShoulderLanelet(current_lanelet, &next_lanelet)) {
-      break;
-    }
-    lanelet_sequence_forward.push_back(next_lanelet);
-    if (searched_ids.find(next_lanelet.id()) != searched_ids.end()) {
+    const auto next_lanelet = getFollowingShoulderLanelet(current_lanelet);
+    if (!next_lanelet) break;
+    lanelet_sequence_forward.push_back(*next_lanelet);
+    if (searched_ids.find(next_lanelet->id()) != searched_ids.end()) {
       // loop shoulder detected
       break;
     }
-    searched_ids.insert(next_lanelet.id());
-    current_lanelet = next_lanelet;
+    searched_ids.insert(next_lanelet->id());
+    current_lanelet = *next_lanelet;
     length +=
-      static_cast<double>(boost::geometry::length(next_lanelet.centerline().basicLineString()));
+      static_cast<double>(boost::geometry::length(next_lanelet->centerline().basicLineString()));
   }
 
   return lanelet_sequence_forward;
 }
 
-bool RouteHandler::getPreviousShoulderLanelet(
-  const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const
+std::optional<lanelet::ConstLanelet> RouteHandler::getPreviousShoulderLanelet(
+  const lanelet::ConstLanelet & lanelet) const
 {
-  for (const auto & shoulder_lanelet : shoulder_lanelets_) {
-    if (lanelet::geometry::follows(shoulder_lanelet, lanelet)) {
-      *prev_lanelet = shoulder_lanelet;
-      return true;
-    }
-  }
-  return false;
+  bool found = false;
+  const auto & search_point = lanelet.centerline().front().basicPoint2d();
+  const auto previous_lanelet = lanelet_map_ptr_->laneletLayer.nearestUntil(
+    search_point, [&](const auto & bbox, const auto & ll) {
+      if (isShoulderLanelet(ll) && lanelet::geometry::follows(ll, lanelet)) found = true;
+      // stop search once prev shoulder lanelet is found, or the bbox does not touch the search
+      // point
+      return found || lanelet::geometry::distance2d(bbox, search_point) > 1e-3;
+    });
+  if (found && previous_lanelet.has_value()) return *previous_lanelet;
+  return std::nullopt;
 }
 
 lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceUpTo(
   const lanelet::ConstLanelet & lanelet, const double min_length) const
 {
   lanelet::ConstLanelets lanelet_sequence_backward;
-  if (!exists(shoulder_lanelets_, lanelet)) {
-    return lanelet_sequence_backward;
-  }
+  if (!isShoulderLanelet(lanelet)) return lanelet_sequence_backward;
 
   double length = 0;
   lanelet::ConstLanelet current_lanelet = lanelet;
   std::set<lanelet::Id> searched_ids{};
   while (rclcpp::ok() && length < min_length) {
-    lanelet::ConstLanelet prev_lanelet;
-    if (!getPreviousShoulderLanelet(current_lanelet, &prev_lanelet)) {
-      break;
-    }
+    const auto prev_lanelet = getPreviousShoulderLanelet(current_lanelet);
+    if (!prev_lanelet) break;
 
-    lanelet_sequence_backward.insert(lanelet_sequence_backward.begin(), prev_lanelet);
-    if (searched_ids.find(prev_lanelet.id()) != searched_ids.end()) {
+    lanelet_sequence_backward.insert(lanelet_sequence_backward.begin(), *prev_lanelet);
+    if (searched_ids.find(prev_lanelet->id()) != searched_ids.end()) {
       // loop shoulder detected
       break;
     }
-    searched_ids.insert(prev_lanelet.id());
-    current_lanelet = prev_lanelet;
+    searched_ids.insert(prev_lanelet->id());
+    current_lanelet = *prev_lanelet;
     length +=
-      static_cast<double>(boost::geometry::length(prev_lanelet.centerline().basicLineString()));
+      static_cast<double>(boost::geometry::length(prev_lanelet->centerline().basicLineString()));
   }
 
   return lanelet_sequence_backward;
@@ -826,7 +850,7 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequence(
   const double forward_distance) const
 {
   lanelet::ConstLanelets lanelet_sequence;
-  if (!exists(shoulder_lanelets_, lanelet)) {
+  if (!isShoulderLanelet(lanelet)) {
     return lanelet_sequence;
   }
 
@@ -1009,20 +1033,16 @@ std::optional<lanelet::ConstLanelet> RouteHandler::getRightLanelet(
 {
   // right road lanelet of shoulder lanelet
   if (isShoulderLanelet(lanelet)) {
-    for (const auto & road_lanelet : road_lanelets_) {
-      if (lanelet::geometry::rightOf(road_lanelet, lanelet)) {
-        return road_lanelet;
-      }
-    }
+    const auto right_lanelets = lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound());
+    for (const auto & right_lanelet : right_lanelets)
+      if (isRoadLanelet(right_lanelet)) return right_lanelet;
     return std::nullopt;
   }
 
   // right shoulder lanelet
   if (get_shoulder_lane) {
-    lanelet::ConstLanelet right_shoulder_lanelet;
-    if (getRightShoulderLanelet(lanelet, &right_shoulder_lanelet)) {
-      return right_shoulder_lanelet;
-    }
+    const auto right_shoulder_lanelet = getRightShoulderLanelet(lanelet);
+    if (right_shoulder_lanelet) return *right_shoulder_lanelet;
   }
 
   // routable lane
@@ -1090,20 +1110,16 @@ std::optional<lanelet::ConstLanelet> RouteHandler::getLeftLanelet(
 {
   // left road lanelet of shoulder lanelet
   if (isShoulderLanelet(lanelet)) {
-    for (const auto & road_lanelet : road_lanelets_) {
-      if (lanelet::geometry::leftOf(road_lanelet, lanelet)) {
-        return road_lanelet;
-      }
-    }
+    const auto left_lanelets = lanelet_map_ptr_->laneletLayer.findUsages(lanelet.leftBound());
+    for (const auto & left_lanelet : left_lanelets)
+      if (isRoadLanelet(left_lanelet)) return left_lanelet;
     return std::nullopt;
   }
 
   // left shoulder lanelet
   if (get_shoulder_lane) {
-    lanelet::ConstLanelet left_shoulder_lanelet;
-    if (getLeftShoulderLanelet(lanelet, &left_shoulder_lanelet)) {
-      return left_shoulder_lanelet;
-    }
+    const auto left_shoulder_lanelet = getLeftShoulderLanelet(lanelet);
+    if (left_shoulder_lanelet) return *left_shoulder_lanelet;
   }
 
   // routable lane
@@ -1510,39 +1526,33 @@ bool RouteHandler::getLeftLaneChangeTargetExceptPreferredLane(
   return false;
 }
 
-bool RouteHandler::getPullOverTarget(
-  const lanelet::ConstLanelets & lanelets, const Pose & goal_pose,
-  lanelet::ConstLanelet * target_lanelet)
+std::optional<lanelet::ConstLanelet> RouteHandler::getPullOverTarget(const Pose & goal_pose) const
 {
-  for (const auto & shoulder_lanelet : lanelets) {
-    if (lanelet::utils::isInLanelet(goal_pose, shoulder_lanelet, 0.1)) {
-      *target_lanelet = shoulder_lanelet;
-      return true;
-    }
+  const lanelet::BasicPoint2d p(goal_pose.position.x, goal_pose.position.y);
+  constexpr auto search_distance = 0.1;
+  const lanelet::BasicPoint2d offset(search_distance, search_distance);
+  const auto lanelets_in_range =
+    lanelet_map_ptr_->laneletLayer.search(lanelet::BoundingBox2d(p - offset, p + offset));
+  for (const auto & lanelet : lanelets_in_range) {
+    const auto is_in_lanelet = lanelet::utils::isInLanelet(goal_pose, lanelet, search_distance);
+    if (is_in_lanelet && isShoulderLanelet(lanelet)) return lanelet;
   }
-  return false;
+  return std::nullopt;
 }
 
-bool RouteHandler::getPullOutStartLane(
-  const lanelet::ConstLanelets & lanelets, const Pose & pose, const double vehicle_width,
-  lanelet::ConstLanelet * target_lanelet)
+std::optional<lanelet::ConstLanelet> RouteHandler::getPullOutStartLane(
+  const Pose & pose, const double vehicle_width) const
 {
-  for (const auto & shoulder_lanelet : lanelets) {
-    if (lanelet::utils::isInLanelet(pose, shoulder_lanelet, vehicle_width / 2.0)) {
-      *target_lanelet = shoulder_lanelet;
-      return true;
-    }
+  const lanelet::BasicPoint2d p(pose.position.x, pose.position.y);
+  const auto search_distance = vehicle_width / 2.0;
+  const lanelet::BasicPoint2d offset(search_distance, search_distance);
+  const auto lanelets_in_range =
+    lanelet_map_ptr_->laneletLayer.search(lanelet::BoundingBox2d(p - offset, p + offset));
+  for (const auto & lanelet : lanelets_in_range) {
+    const auto is_in_lanelet = lanelet::utils::isInLanelet(pose, lanelet, search_distance);
+    if (is_in_lanelet && isShoulderLanelet(lanelet)) return lanelet;
   }
-  return false;
-}
-
-lanelet::ConstLanelets RouteHandler::getClosestLaneletSequence(const Pose & pose) const
-{
-  lanelet::ConstLanelet lanelet;
-  if (!getClosestLaneletWithinRoute(pose, &lanelet)) {
-    return lanelet::ConstLanelets{};
-  }
-  return getLaneletSequence(lanelet);
+  return std::nullopt;
 }
 
 int RouteHandler::getNumLaneToPreferredLane(
@@ -1933,14 +1943,10 @@ lanelet::routing::RelationType RouteHandler::getRelation(
   return lanelet::routing::RelationType::None;
 }
 
-lanelet::ConstLanelets RouteHandler::getShoulderLanelets() const
-{
-  return shoulder_lanelets_;
-}
-
 bool RouteHandler::isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const
 {
-  return lanelet::utils::contains(shoulder_lanelets_, lanelet);
+  return lanelet.hasAttribute(lanelet::AttributeName::Subtype) &&
+         lanelet.attribute(lanelet::AttributeName::Subtype) == "road_shoulder";
 }
 
 bool RouteHandler::isRouteLanelet(const lanelet::ConstLanelet & lanelet) const
@@ -1948,6 +1954,12 @@ bool RouteHandler::isRouteLanelet(const lanelet::ConstLanelet & lanelet) const
   return lanelet::utils::contains(route_lanelets_, lanelet);
 }
 
+bool RouteHandler::isRoadLanelet(const lanelet::ConstLanelet & lanelet) const
+{
+  return lanelet.hasAttribute(lanelet::AttributeName::Subtype) &&
+         lanelet.attribute(lanelet::AttributeName::Subtype) == lanelet::AttributeValueString::Road;
+}
+
 lanelet::ConstLanelets RouteHandler::getPreviousLaneletSequence(
   const lanelet::ConstLanelets & lanelet_sequence) const
 {
@@ -2120,28 +2132,47 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
   lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes) const
 {
   // Find lanelets for start point. First, find all lanelets containing the start point to calculate
-  // all possible route later. It fails when the point is not located on any road_lanelet (e.g. the
+  // all possible route later. It fails when the point is not located on any road lanelet (e.g. the
   // start point is located out of any lanelets or road_shoulder lanelet which is not contained in
-  // road_lanelet). In that case, find the closest lanelet instead.
+  // road lanelet). In that case, find the closest lanelet instead (within some maximum range).
+  constexpr auto max_search_range = 20.0;
+  auto start_lanelets = getRoadLaneletsAtPose(start_checkpoint);
   lanelet::ConstLanelet start_lanelet;
-  lanelet::ConstLanelets start_lanelets;
-  if (!lanelet::utils::query::getCurrentLanelets(
-        road_lanelets_, start_checkpoint, &start_lanelets)) {
-    if (!lanelet::utils::query::getClosestLanelet(
-          road_lanelets_, start_checkpoint, &start_lanelet)) {
-      RCLCPP_WARN_STREAM(
-        logger_, "Failed to find current lanelet."
-                   << std::endl
-                   << " - start checkpoint: " << toString(start_checkpoint) << std::endl
-                   << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl);
-      return false;
-    }
-    start_lanelets = {start_lanelet};
+  if (start_lanelets.empty()) {
+    const lanelet::BasicPoint2d p(start_checkpoint.position.x, start_checkpoint.position.y);
+    const lanelet::BoundingBox2d bbox(
+      lanelet::BasicPoint2d(p.x() - max_search_range, p.y() - max_search_range),
+      lanelet::BasicPoint2d(p.x() + max_search_range, p.y() + max_search_range));
+    // std::as_const(*ptr) to use the const version of the search function
+    auto candidates = std::as_const(*lanelet_map_ptr_).laneletLayer.search(bbox);
+    candidates.erase(
+      std::remove_if(
+        candidates.begin(), candidates.end(), [&](const auto & l) { return !isRoadLanelet(l); }),
+      candidates.end());
+    if (lanelet::utils::query::getClosestLanelet(candidates, start_checkpoint, &start_lanelet))
+      start_lanelets = {start_lanelet};
+  }
+  if (start_lanelets.empty()) {
+    RCLCPP_WARN_STREAM(
+      logger_, "Failed to find current lanelet."
+                 << std::endl
+                 << " - start checkpoint: " << toString(start_checkpoint) << std::endl
+                 << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl);
+    return false;
   }
 
   // Find lanelets for goal point.
   lanelet::ConstLanelet goal_lanelet;
-  if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal_checkpoint, &goal_lanelet)) {
+  const lanelet::BasicPoint2d p(goal_checkpoint.position.x, goal_checkpoint.position.y);
+  const lanelet::BoundingBox2d bbox(
+    lanelet::BasicPoint2d(p.x() - max_search_range, p.y() - max_search_range),
+    lanelet::BasicPoint2d(p.x() + max_search_range, p.y() + max_search_range));
+  auto candidates = std::as_const(*lanelet_map_ptr_).laneletLayer.search(bbox);
+  candidates.erase(
+    std::remove_if(
+      candidates.begin(), candidates.end(), [&](const auto & l) { return !isRoadLanelet(l); }),
+    candidates.end());
+  if (!lanelet::utils::query::getClosestLanelet(candidates, goal_checkpoint, &goal_lanelet)) {
     RCLCPP_WARN_STREAM(
       logger_, "Failed to find closest lanelet."
                  << std::endl
@@ -2189,14 +2220,11 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
   if (is_route_found) {
     lanelet::routing::LaneletPath path;
     path = [&]() -> lanelet::routing::LaneletPath {
-      if (!consider_no_drivable_lanes) return shortest_path;
-      lanelet::routing::LaneletPath drivable_lane_path;
-      bool drivable_lane_path_found = false;
-      if (hasNoDrivableLaneInPath(shortest_path)) {
-        drivable_lane_path_found =
-          findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path);
+      if (consider_no_drivable_lanes && hasNoDrivableLaneInPath(shortest_path)) {
+        const auto drivable_lane_path = findDrivableLanePath(start_lanelet, goal_lanelet);
+        if (drivable_lane_path) return *drivable_lane_path;
       }
-      return (drivable_lane_path_found) ? drivable_lane_path : shortest_path;
+      return shortest_path;
     }();
 
     path_lanelets->reserve(path.size());
@@ -2248,49 +2276,28 @@ lanelet::ConstLanelets RouteHandler::getMainLanelets(
   return main_lanelets;
 }
 
-bool RouteHandler::hasNoDrivableLaneInPath(const lanelet::routing::LaneletPath & path) const
+bool RouteHandler::isNoDrivableLane(const lanelet::ConstLanelet & llt)
 {
-  for (const auto & llt : path) {
-    const std::string no_drivable_lane_attribute = llt.attributeOr("no_drivable_lane", "no");
-    if (no_drivable_lane_attribute == "yes") {
-      return true;
-    }
-  }
-
-  return false;
+  const std::string no_drivable_lane_attribute = llt.attributeOr("no_drivable_lane", "no");
+  return no_drivable_lane_attribute == "yes";
 }
 
-bool RouteHandler::findDrivableLanePath(
-  const lanelet::ConstLanelet & start_lanelet, const lanelet::ConstLanelet & goal_lanelet,
-  lanelet::routing::LaneletPath & drivable_lane_path) const
+bool RouteHandler::hasNoDrivableLaneInPath(const lanelet::routing::LaneletPath & path) const
 {
-  double drivable_lane_path_length2d = std::numeric_limits<double>::max();
-  bool drivable_lane_path_found = false;
-
-  for (const auto & llt : road_lanelets_) {
-    lanelet::ConstLanelets via_lanelet;
-    via_lanelet.push_back(llt);
-    const lanelet::Optional<lanelet::routing::Route> optional_route =
-      routing_graph_ptr_->getRouteVia(start_lanelet, via_lanelet, goal_lanelet, 0);
-
-    if ((optional_route) && (!hasNoDrivableLaneInPath(optional_route->shortestPath()))) {
-      if (optional_route->length2d() < drivable_lane_path_length2d) {
-        drivable_lane_path_length2d = optional_route->length2d();
-        drivable_lane_path = optional_route->shortestPath();
-        drivable_lane_path_found = true;
-      }
-    }
-    via_lanelet.clear();
-  }
-
-  return drivable_lane_path_found;
+  for (const auto & llt : path)
+    if (isNoDrivableLane(llt)) return true;
+  return false;
 }
 
-lanelet::ConstLanelets RouteHandler::getClosestLanelets(
-  const geometry_msgs::msg::Pose & target_pose) const
+std::optional<lanelet::routing::LaneletPath> RouteHandler::findDrivableLanePath(
+  const lanelet::ConstLanelet & start_lanelet, const lanelet::ConstLanelet & goal_lanelet) const
 {
-  lanelet::ConstLanelets target_lanelets;
-  lanelet::utils::query::getCurrentLanelets(road_lanelets_, target_pose, &target_lanelets);
-  return target_lanelets;
+  // we create a new routing graph with infinite cost on no drivable lanes
+  const auto drivable_routing_graph_ptr = lanelet::routing::RoutingGraph::build(
+    *lanelet_map_ptr_, *traffic_rules_ptr_,
+    lanelet::routing::RoutingCostPtrs{std::make_shared<RoutingCostDrivable>()});
+  const auto route = drivable_routing_graph_ptr->getRoute(start_lanelet, goal_lanelet, 0);
+  if (route) return route->shortestPath();
+  return {};
 }
 }  // namespace route_handler
diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp
index 750b2e68b16b0..ec24f47dacc27 100644
--- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp
+++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp
@@ -304,8 +304,9 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout
     } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) {
       const auto bag_centerline = generate_centerline_with_bag(*this);
       const auto start_lanelets =
-        route_handler_ptr_->getClosestLanelets(bag_centerline.front().pose);
-      const auto end_lanelets = route_handler_ptr_->getClosestLanelets(bag_centerline.back().pose);
+        route_handler_ptr_->getRoadLaneletsAtPose(bag_centerline.front().pose);
+      const auto end_lanelets =
+        route_handler_ptr_->getRoadLaneletsAtPose(bag_centerline.back().pose);
       if (start_lanelets.empty() || end_lanelets.empty()) {
         RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's centerline are not found.");
         return CenterlineWithRoute{};