feat(routing-no-drivable-lanes): proposing solution for routing no_dr…

…ivable_lane (autowarefoundation#3954)

* feat(routing-no-drivable-lanes): proposing solution for routing no-drivable lanes using getRouteVia method

Signed-off-by: AhmedEbrahim <ahmed.a.d.ebrahim@gmail.com>

* style(pre-commit): autofix

* feat(routing-no-drivable-lanes): fixing review comments / adding comments to added functions / proper naming

Signed-off-by: AhmedEbrahim <ahmed.a.d.ebrahim@gmail.com>

* style(pre-commit): autofix

---------

Signed-off-by: AhmedEbrahim <ahmed.a.d.ebrahim@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/route_handler/include/route_handler/route_handler.hpp

@@ -425,6 +425,25 @@ class RouteHandler
   // for path
 
   PathWithLaneId updatePathTwist(const PathWithLaneId & path) const;
+  /**
+   * @brief Checks if a path has a no_drivable_lane or not
+   * @param path lanelet path
+   * @return true if the lanelet path includes at least one no_drivable_lane, false if it does not
+   * include any.
+   */
+  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.
+   * @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.
+   */
+  bool findDrivableLanePath(
+    const lanelet::Lanelet & start_lanelet, const lanelet::Lanelet & goal_lanelet,
+    lanelet::routing::LaneletPath & drivable_lane_path) const;
 };
 }  // namespace route_handler
 #endif  // ROUTE_HANDLER__ROUTE_HANDLER_HPP_

planning/route_handler/src/route_handler.cpp

@@ -1994,11 +1994,28 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints(
     return false;
   }
 
-  const auto shortest_path = optional_route->shortestPath();
-  path_lanelets->reserve(shortest_path.size());
-  for (const auto & llt : shortest_path) {
+  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;
+
+  if (shortest_path_has_no_drivable_lane) {
+    drivable_lane_path_found =
+      findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path);
+  }
+
+  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;
 }
 
@@ -2042,4 +2059,42 @@ lanelet::ConstLanelets RouteHandler::getMainLanelets(
   return main_lanelets;
 }
 
+bool RouteHandler::hasNoDrivableLaneInPath(const lanelet::routing::LaneletPath & path) const
+{
+  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;
+}
+
+bool RouteHandler::findDrivableLanePath(
+  const lanelet::Lanelet & start_lanelet, const lanelet::Lanelet & goal_lanelet,
+  lanelet::routing::LaneletPath & drivable_lane_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;
+}
+
 }  // namespace route_handler