fix(route_handler): check all co-located lanes for generating path (#…

…4013) * check all co-located lanes for generating path Signed-off-by: beyza <bnk@leodrive.ai> * style(pre-commit): autofix * fix the conflicts Signed-off-by: beyza <bnk@leodrive.ai> * style(pre-commit): autofix --------- Signed-off-by: beyza <bnk@leodrive.ai> Co-authored-by: beyza <bnk@leodrive.ai> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Jul 11, 2023 · 98b4f90 · 98b4f90
1 parent 42fa56c
commit 98b4f90
Show file tree

Hide file tree

Showing 2 changed files with 49 additions and 33 deletions.
diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp
@@ -442,7 +442,7 @@ class RouteHandler
    * @return true if a path without any no_drivable_lane found, false if this path is not found.
    */
   bool findDrivableLanePath(
-    const lanelet::Lanelet & start_lanelet, const lanelet::Lanelet & goal_lanelet,
+    const lanelet::ConstLanelet & start_lanelet, const lanelet::ConstLanelet & goal_lanelet,
     lanelet::routing::LaneletPath & drivable_lane_path) const;
 };
 }  // namespace route_handler

diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp
@@ -1971,52 +1971,68 @@ 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::ConstLanelet start_lanelet;
+  lanelet::ConstLanelets start_lanelets;
+  if (!lanelet::utils::query::getCurrentLanelets(
+        road_lanelets_, start_checkpoint, &start_lanelets)) {
     return false;
   }
-  lanelet::Lanelet goal_lanelet;
+  lanelet::ConstLanelet 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
-                 << "start lane id: " << start_lanelet.id() << std::endl
-                 << "goal lane id: " << goal_lanelet.id() << std::endl);
-    return false;
-  }
+  lanelet::Optional<lanelet::routing::Route> optional_route;
+  std::vector<lanelet::ConstLanelets> candidate_paths;
+  lanelet::routing::LaneletPath shortest_path;
+  bool is_route_found = false;
 
-  const lanelet::routing::LaneletPath shortest_path = optional_route->shortestPath();
-  bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path);
   lanelet::routing::LaneletPath drivable_lane_path;
   bool drivable_lane_path_found = false;
+  double shortest_path_length2d = std::numeric_limits<double>::max();
+
+  for (const auto & st_llt : start_lanelets) {
+    optional_route = routing_graph_ptr_->getRoute(st_llt, 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: " << st_llt.id() << std::endl
+                   << "goal lane id: " << goal_lanelet.id() << std::endl);
+    } else {
+      is_route_found = true;
 
-  if (shortest_path_has_no_drivable_lane) {
-    drivable_lane_path_found =
-      findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path);
+      if (optional_route->length2d() < shortest_path_length2d) {
+        shortest_path_length2d = optional_route->length2d();
+        shortest_path = optional_route->shortestPath();
+        start_lanelet = st_llt;
+      }
+    }
   }
 
-  lanelet::routing::LaneletPath path;
-  if (drivable_lane_path_found) {
-    path = drivable_lane_path;
-  } else {
-    path = shortest_path;
-  }
+  if (is_route_found) {
+    bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path);
+    if (shortest_path_has_no_drivable_lane) {
+      drivable_lane_path_found =
+        findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path);
+    }
 
-  path_lanelets->reserve(path.size());
-  for (const auto & llt : path) {
-    path_lanelets->push_back(llt);
+    lanelet::routing::LaneletPath path;
+    if (drivable_lane_path_found) {
+      path = drivable_lane_path;
+    } else {
+      path = shortest_path;
+    }
+
+    path_lanelets->reserve(path.size());
+    for (const auto & llt : path) {
+      path_lanelets->push_back(llt);
+    }
   }
 
-  return true;
+  return is_route_found;
 }
 
 std::vector<LaneletSegment> RouteHandler::createMapSegments(
@@ -2072,7 +2088,7 @@ bool RouteHandler::hasNoDrivableLaneInPath(const lanelet::routing::LaneletPath &
 }
 
 bool RouteHandler::findDrivableLanePath(
-  const lanelet::Lanelet & start_lanelet, const lanelet::Lanelet & goal_lanelet,
+  const lanelet::ConstLanelet & start_lanelet, const lanelet::ConstLanelet & goal_lanelet,
   lanelet::routing::LaneletPath & drivable_lane_path) const
 {
   double drivable_lane_path_length2d = std::numeric_limits<double>::max();