check all co-located lanes for generating path

Signed-off-by: beyza <bnk@leodrive.ai>

planning/route_handler/src/route_handler.cpp

@@ -1967,35 +1967,48 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
   const Pose & start_checkpoint, const Pose & goal_checkpoint,
   lanelet::ConstLanelets * path_lanelets) const
 {
-  lanelet::Lanelet start_lanelet;
-  if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, start_checkpoint, &start_lanelet)) {
+  lanelet::ConstLanelets start_lanelets;
+  if (!lanelet::utils::query::getCurrentLanelets(road_lanelets_, start_checkpoint, &start_lanelets)) {
     return false;
   }
   lanelet::Lanelet goal_lanelet;
   if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal_checkpoint, &goal_lanelet)) {
     return false;
   }
 
-  // get all possible lanes that can be used to reach goal (including all possible lane change)
-  const lanelet::Optional<lanelet::routing::Route> optional_route =
-    routing_graph_ptr_->getRoute(start_lanelet, goal_lanelet, 0);
-  if (!optional_route) {
-    RCLCPP_ERROR_STREAM(
-      logger_, "Failed to find a proper path!"
-                 << std::endl
-                 << "start checkpoint: " << toString(start_checkpoint) << std::endl
-                 << "goal checkpoint: " << toString(goal_checkpoint) << std::endl
+  lanelet::Optional<lanelet::routing::Route> optional_route;
+  std::vector<lanelet::ConstLanelets> candidate_paths;
+  bool is_route_found = false;
+  for (const auto & start_lanelet : start_lanelets) {
+    optional_route =
+      routing_graph_ptr_->getRoute(start_lanelet, goal_lanelet, 0);
+    if (!optional_route) {
+      RCLCPP_ERROR_STREAM(
+        logger_, "Failed to find a proper path!"
+                   << std::endl
+                   << "start checkpoint: " << toString(start_checkpoint) << std::endl
+                   << "goal checkpoint: " << toString(goal_checkpoint) << std::endl
                  << "start lane id: " << start_lanelet.id() << std::endl
                  << "goal lane id: " << goal_lanelet.id() << std::endl);
-    return false;
-  }
+    } else {
+      is_route_found = true;
 
-  const auto shortest_path = optional_route->shortestPath();
-  path_lanelets->reserve(shortest_path.size());
-  for (const auto & llt : shortest_path) {
-    path_lanelets->push_back(llt);
+      const auto shortest_path = optional_route->shortestPath();
+      lanelet::ConstLanelets candidate_path_lanelets;
+      for (const auto & llt : shortest_path) {
+        candidate_path_lanelets.push_back(llt);
+      }
+      candidate_paths.push_back(candidate_path_lanelets);
+    }
   }
-  return true;
+  size_t shortest_path_length = std::numeric_limits<int>::max();
+  for (const auto & candidate_path : candidate_paths) {
+    if (candidate_path.size() < shortest_path_length) {
+      shortest_path_length = candidate_path.size();
+      *path_lanelets = candidate_path;
+    }
+  }
+  return is_route_found;
 }
 
 std::vector<LaneletSegment> RouteHandler::createMapSegments(