diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp
index c7f86477ff404..88a4ef1656a23 100644
--- a/planning/behavior_velocity_planner/include/utilization/util.hpp
+++ b/planning/behavior_velocity_planner/include/utilization/util.hpp
@@ -313,36 +313,36 @@ std::vector<T> concatVector(const std::vector<T> & vec1, const std::vector<T> &
   return concat_vec;
 }
 
+boost::optional<int64_t> getNearestLaneId(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose,
+  boost::optional<size_t> & nearest_segment_idx);
+
 template <class T>
 std::unordered_map<typename std::shared_ptr<const T>, lanelet::ConstLanelet> getRegElemMapOnPath(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose)
 {
   std::unordered_map<typename std::shared_ptr<const T>, lanelet::ConstLanelet> reg_elem_map_on_path;
-  std::set<int64_t> unique_lane_ids;
-  auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex(
-    path.points, current_pose, std::numeric_limits<double>::max(), M_PI_2);
 
   // Add current lane id
-  lanelet::ConstLanelets current_lanes;
-  if (
-    lanelet::utils::query::getCurrentLanelets(
-      lanelet::utils::query::laneletLayer(lanelet_map), current_pose, &current_lanes) &&
-    nearest_segment_idx) {
-    for (const auto & ll : current_lanes) {
-      if (
-        ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0) ||
-        ll.id() == path.points.at(*nearest_segment_idx + 1).lane_ids.at(0)) {
-        unique_lane_ids.insert(ll.id());
-      }
-    }
+  boost::optional<size_t> nearest_segment_idx;
+  const auto nearest_lane_id =
+    getNearestLaneId(path, lanelet_map, current_pose, nearest_segment_idx);
+
+  std::vector<int64_t> unique_lane_ids;
+  if (nearest_lane_id) {
+    unique_lane_ids.emplace_back(*nearest_lane_id);
   }
 
   // Add forward path lane_id
-  const size_t start_idx = *nearest_segment_idx ? *nearest_segment_idx + 1 : 0;
+  const size_t start_idx = nearest_segment_idx ? *nearest_segment_idx + 1 : 0;
   for (size_t i = start_idx; i < path.points.size(); i++) {
-    unique_lane_ids.insert(
-      path.points.at(i).lane_ids.at(0));  // should we iterate ids? keep as it was.
+    const int64_t lane_id = path.points.at(i).lane_ids.at(0);
+    if (
+      std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) == unique_lane_ids.end()) {
+      unique_lane_ids.emplace_back(lane_id);
+    }
   }
 
   for (const auto lane_id : unique_lane_ids) {
diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp
index 9deaf97912d62..96f4d400b7f35 100644
--- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp
@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <scene_module/crosswalk/manager.hpp>
+#include <utilization/util.hpp>
 
 #include <memory>
 #include <set>
@@ -50,34 +51,28 @@ std::vector<lanelet::ConstLanelet> CrosswalkModuleManager::getCrosswalksOnPath(
   const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
 {
   std::vector<lanelet::ConstLanelet> crosswalks;
-  std::set<int64_t> unique_lane_ids;
 
   auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex(
     path.points, planner_data_->current_pose.pose, std::numeric_limits<double>::max(), M_PI_2);
 
   // Add current lane id
-  lanelet::ConstLanelets current_lanes;
-  if (
-    lanelet::utils::query::getCurrentLanelets(
-      lanelet::utils::query::laneletLayer(lanelet_map), planner_data_->current_pose.pose,
-      &current_lanes) &&
-    nearest_segment_idx) {
-    for (const auto & ll : current_lanes) {
-      if (
-        ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0) ||
-        ll.id() == path.points.at(*nearest_segment_idx + 1).lane_ids.at(0)) {
-        unique_lane_ids.insert(ll.id());
-      }
-    }
+  const auto nearest_lane_id = behavior_velocity_planner::planning_utils::getNearestLaneId(
+    path, lanelet_map, planner_data_->current_pose.pose, nearest_segment_idx);
+
+  std::vector<int64_t> unique_lane_ids;
+  if (nearest_lane_id) {
+    unique_lane_ids.emplace_back(*nearest_lane_id);
   }
 
   // Add forward path lane_id
-  const size_t start_idx = *nearest_segment_idx ? *nearest_segment_idx + 1 : 0;
+  const size_t start_idx = nearest_segment_idx ? *nearest_segment_idx + 1 : 0;
   for (size_t i = start_idx; i < path.points.size(); i++) {
-    unique_lane_ids.insert(
-      path.points.at(i).lane_ids.at(0));  // should we iterate ids? keep as it was.
+    const int64_t lane_id = path.points.at(i).lane_ids.at(0);
+    if (
+      std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) == unique_lane_ids.end()) {
+      unique_lane_ids.emplace_back(lane_id);
+    }
   }
-
   for (const auto lane_id : unique_lane_ids) {
     const auto ll = lanelet_map->laneletLayer.get(lane_id);
 
diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp
index 42d0b973d51a0..acbb6773c23ad 100644
--- a/planning/behavior_velocity_planner/src/utilization/util.cpp
+++ b/planning/behavior_velocity_planner/src/utilization/util.cpp
@@ -593,34 +593,66 @@ LineString2d extendLine(
     {(p1 - length * t).x(), (p1 - length * t).y()}, {(p2 + length * t).x(), (p2 + length * t).y()}};
 }
 
-std::vector<lanelet::ConstLanelet> getLaneletsOnPath(
+boost::optional<int64_t> getNearestLaneId(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose)
+  const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose,
+  boost::optional<size_t> & nearest_segment_idx)
 {
-  std::set<int64_t> unique_lane_ids;
-  auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex(
+  boost::optional<int64_t> nearest_lane_id;
+
+  nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex(
     path.points, current_pose, std::numeric_limits<double>::max(), M_PI_2);
 
-  // Add current lane id
+  if (!nearest_segment_idx) {
+    return boost::none;
+  }
+
   lanelet::ConstLanelets current_lanes;
   if (
     lanelet::utils::query::getCurrentLanelets(
       lanelet::utils::query::laneletLayer(lanelet_map), current_pose, &current_lanes) &&
     nearest_segment_idx) {
     for (const auto & ll : current_lanes) {
-      if (
-        ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0) ||
-        ll.id() == path.points.at(*nearest_segment_idx + 1).lane_ids.at(0)) {
-        unique_lane_ids.insert(ll.id());
+      if (ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0)) {
+        nearest_lane_id = ll.id();
+        return nearest_lane_id;
+      }
+    }
+
+    // if the lane_id of nearest_segment_idx does not involved in current_lanes,
+    // search the lane_id of nearest_segment_idx + 1
+    *nearest_segment_idx += 1;
+    for (const auto & ll : current_lanes) {
+      if (ll.id() == path.points.at(*nearest_segment_idx).lane_ids.at(0)) {
+        nearest_lane_id = ll.id();
+        return nearest_lane_id;
       }
     }
   }
+  return boost::none;
+}
+
+std::vector<lanelet::ConstLanelet> getLaneletsOnPath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose)
+{
+  boost::optional<size_t> nearest_segment_idx;
+  const auto nearest_lane_id =
+    getNearestLaneId(path, lanelet_map, current_pose, nearest_segment_idx);
+
+  std::vector<int64_t> unique_lane_ids;
+  if (nearest_lane_id) {
+    unique_lane_ids.emplace_back(*nearest_lane_id);
+  }
 
   // Add forward path lane_id
-  const size_t start_idx = *nearest_segment_idx ? *nearest_segment_idx + 1 : 0;
+  const size_t start_idx = nearest_segment_idx ? *nearest_segment_idx + 1 : 0;
   for (size_t i = start_idx; i < path.points.size(); i++) {
-    unique_lane_ids.insert(
-      path.points.at(i).lane_ids.at(0));  // should we iterate ids? keep as it was.
+    const int64_t lane_id = path.points.at(i).lane_ids.at(0);
+    if (
+      std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) == unique_lane_ids.end()) {
+      unique_lane_ids.emplace_back(lane_id);
+    }
   }
 
   std::vector<lanelet::ConstLanelet> lanelets;