diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 8b5c7524e7a25..927b423873d5f 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -425,25 +425,6 @@ 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_ diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index f3db1cadf02a1..534bd344f6443 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1994,28 +1994,11 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints( return 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; - - 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) { + const auto shortest_path = optional_route->shortestPath(); + path_lanelets->reserve(shortest_path.size()); + for (const auto & llt : shortest_path) { path_lanelets->push_back(llt); } - return true; } @@ -2059,42 +2042,4 @@ 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::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 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