perf(route_handler): simplify queries on the road and shoulder lanele…

…ts (autowarefoundation#6885)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Signed-off-by: vividf <yihsiang.fang@tier4.jp>

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -447,12 +447,10 @@ bool GoalPlannerModule::isExecutionRequested() const
 
   // check that goal is in current neighbor shoulder lane
   const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() {
-    lanelet::ConstLanelet neighbor_shoulder_lane{};
     for (const auto & lane : current_lanes) {
-      const bool has_shoulder_lane =
-        left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane, &neighbor_shoulder_lane)
-                           : route_handler->getRightShoulderLanelet(lane, &neighbor_shoulder_lane);
-      if (has_shoulder_lane && lanelet::utils::isInLanelet(goal_pose, neighbor_shoulder_lane)) {
+      const auto shoulder_lane = left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane)
+                                                    : route_handler->getRightShoulderLanelet(lane);
+      if (shoulder_lane && lanelet::utils::isInLanelet(goal_pose, *shoulder_lane)) {
         return true;
       }
     }
@@ -1897,20 +1895,11 @@ bool GoalPlannerModule::isCrossingPossible(
   // Lambda function to get the neighboring lanelet based on left_side_parking_
   auto getNeighboringLane =
     [&](const lanelet::ConstLanelet & lane) -> std::optional<lanelet::ConstLanelet> {
-    lanelet::ConstLanelet neighboring_lane{};
-    if (left_side_parking_) {
-      if (route_handler->getLeftShoulderLanelet(lane, &neighboring_lane)) {
-        return neighboring_lane;
-      } else {
-        return route_handler->getLeftLanelet(lane);
-      }
-    } else {
-      if (route_handler->getRightShoulderLanelet(lane, &neighboring_lane)) {
-        return neighboring_lane;
-      } else {
-        return route_handler->getRightLanelet(lane);
-      }
-    }
+    const auto neighboring_lane = left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane)
+                                                     : route_handler->getRightShoulderLanelet(lane);
+    if (neighboring_lane) return neighboring_lane;
+    return left_side_parking_ ? route_handler->getLeftLanelet(lane)
+                              : route_handler->getRightLanelet(lane);
   };
 
   // Iterate through start_lane_sequence to find a path to end_lane_sequence

planning/behavior_path_goal_planner_module/src/util.cpp

@@ -55,12 +55,11 @@ lanelet::ConstLanelets getPullOverLanes(
   // todo(kosuek55): automatically calculates this distance.
   const double backward_distance_with_buffer = backward_distance + 100;
 
-  lanelet::ConstLanelet target_shoulder_lane{};
-  if (route_handler::RouteHandler::getPullOverTarget(
-        route_handler.getShoulderLanelets(), goal_pose, &target_shoulder_lane)) {
+  const auto target_shoulder_lane = route_handler.getPullOverTarget(goal_pose);
+  if (target_shoulder_lane) {
     // pull over on shoulder lane
     return route_handler.getShoulderLaneletSequence(
-      target_shoulder_lane, goal_pose, backward_distance_with_buffer, forward_distance);
+      *target_shoulder_lane, goal_pose, backward_distance_with_buffer, forward_distance);
   }
 
   lanelet::ConstLanelet closest_lane{};

planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp

@@ -354,11 +354,9 @@ TargetObjectsOnLane createTargetObjectsOnLane(
   };
 
   const auto update_left_shoulder_lanelet = [&](const lanelet::ConstLanelet & target_lane) {
-    lanelet::ConstLanelet neighbor_shoulder_lane{};
-    const bool shoulder_lane_is_found =
-      route_handler->getLeftShoulderLanelet(target_lane, &neighbor_shoulder_lane);
-    if (shoulder_lane_is_found) {
-      all_left_shoulder_lanelets.insert(all_left_shoulder_lanelets.end(), neighbor_shoulder_lane);
+    const auto neighbor_shoulder_lane = route_handler->getLeftShoulderLanelet(target_lane);
+    if (neighbor_shoulder_lane) {
+      all_left_shoulder_lanelets.insert(all_left_shoulder_lanelets.end(), *neighbor_shoulder_lane);
     }
   };
 

planning/behavior_path_planner_common/src/utils/path_utils.cpp

@@ -645,15 +645,12 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr<const PlannerDat
   const auto & modified_goal = planner_data->prev_modified_goal;
 
   const Pose goal_pose = modified_goal ? modified_goal->pose : route_handler->getGoalPose();
-  const auto shoulder_lanes = route_handler->getShoulderLanelets();
 
   lanelet::ConstLanelet goal_lane;
-  const bool is_failed_getting_lanelet = std::invoke([&]() {
-    if (isInLanelets(goal_pose, shoulder_lanes)) {
-      return !lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane);
-    }
-    return !route_handler->getGoalLanelet(&goal_lane);
-  });
+  const auto shoulder_goal_lanes = route_handler->getShoulderLaneletsAtPose(goal_pose);
+  if (!shoulder_goal_lanes.empty()) goal_lane = shoulder_goal_lanes.front();
+  const auto is_failed_getting_lanelet =
+    shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane);
   if (is_failed_getting_lanelet) {
     return output;
   }

planning/behavior_path_planner_common/src/utils/utils.cpp

@@ -496,15 +496,12 @@ bool isEgoOutOfRoute(
   const Pose & goal_pose = (modified_goal && modified_goal->uuid == route_handler->getRouteUuid())
                              ? modified_goal->pose
                              : route_handler->getGoalPose();
-  const auto shoulder_lanes = route_handler->getShoulderLanelets();
 
   lanelet::ConstLanelet goal_lane;
-  const bool is_failed_getting_lanelet = std::invoke([&]() {
-    if (utils::isInLanelets(goal_pose, shoulder_lanes)) {
-      return !lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane);
-    }
-    return !route_handler->getGoalLanelet(&goal_lane);
-  });
+  const auto shoulder_goal_lanes = route_handler->getShoulderLaneletsAtPose(goal_pose);
+  if (!shoulder_goal_lanes.empty()) goal_lane = shoulder_goal_lanes.front();
+  const auto is_failed_getting_lanelet =
+    shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane);
   if (is_failed_getting_lanelet) {
     RCLCPP_WARN_STREAM(
       rclcpp::get_logger("behavior_path_planner").get_child("util"), "cannot find goal lanelet");
@@ -527,14 +524,7 @@ bool isEgoOutOfRoute(
 
   // If ego vehicle is out of the closest lanelet, return true
   // Check if ego vehicle is in shoulder lane
-  const bool is_in_shoulder_lane = std::invoke([&]() {
-    lanelet::Lanelet closest_shoulder_lanelet;
-    if (!lanelet::utils::query::getClosestLanelet(
-          shoulder_lanes, self_pose, &closest_shoulder_lanelet)) {
-      return false;
-    }
-    return lanelet::utils::isInLanelet(self_pose, closest_shoulder_lanelet);
-  });
+  const bool is_in_shoulder_lane = !route_handler->getShoulderLaneletsAtPose(self_pose).empty();
   // Check if ego vehicle is in road lane
   const bool is_in_road_lane = std::invoke([&]() {
     lanelet::ConstLanelet closest_road_lane;
@@ -1662,13 +1652,6 @@ bool isAllowedGoalModification(const std::shared_ptr<RouteHandler> & route_handl
 bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & route_handler)
 {
   const Pose & goal_pose = route_handler->getOriginalGoalPose();
-  const auto shoulder_lanes = route_handler->getShoulderLanelets();
-
-  lanelet::ConstLanelet closest_shoulder_lane{};
-  if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) {
-    return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1);
-  }
-
-  return false;
+  return !route_handler->getShoulderLaneletsAtPose(goal_pose).empty();
 }
 }  // namespace behavior_path_planner::utils

planning/behavior_path_start_planner_module/src/util.cpp

@@ -90,11 +90,10 @@ lanelet::ConstLanelets getPullOutLanes(
   const auto & route_handler = planner_data->route_handler;
   const auto start_pose = planner_data->route_handler->getOriginalStartPose();
 
-  lanelet::ConstLanelet current_shoulder_lane;
-  if (route_handler->getPullOutStartLane(
-        route_handler->getShoulderLanelets(), start_pose, vehicle_width, &current_shoulder_lane)) {
+  const auto current_shoulder_lane = route_handler->getPullOutStartLane(start_pose, vehicle_width);
+  if (current_shoulder_lane) {
     // pull out from shoulder lane
-    return route_handler->getShoulderLaneletSequence(current_shoulder_lane, start_pose);
+    return route_handler->getShoulderLaneletSequence(*current_shoulder_lane, start_pose);
   }
 
   // pull out from road lane

planning/route_handler/include/route_handler/route_handler.hpp

@@ -27,6 +27,7 @@
 #include <lanelet2_core/Forward.h>
 #include <lanelet2_core/primitives/Lanelet.h>
 #include <lanelet2_routing/Forward.h>
+#include <lanelet2_routing/RoutingCost.h>
 #include <lanelet2_traffic_rules/TrafficRules.h>
 
 #include <limits>
@@ -77,6 +78,7 @@ class RouteHandler
   lanelet::traffic_rules::TrafficRulesPtr getTrafficRulesPtr() const;
   std::shared_ptr<const lanelet::routing::RoutingGraphContainer> getOverallGraphPtr() const;
   lanelet::LaneletMapPtr getLaneletMapPtr() const;
+  static bool isNoDrivableLane(const lanelet::ConstLanelet & llt);
 
   // for routing
   bool planPathLaneletsBetweenCheckpoints(
@@ -324,9 +326,9 @@ class RouteHandler
     const double check_length) const;
   lanelet::routing::RelationType getRelation(
     const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const;
-  lanelet::ConstLanelets getShoulderLanelets() const;
   bool isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const;
   bool isRouteLanelet(const lanelet::ConstLanelet & lanelet) const;
+  bool isRoadLanelet(const lanelet::ConstLanelet & lanelet) const;
   lanelet::ConstLanelets getPreferredLanelets() const;
 
   // for path
@@ -341,33 +343,29 @@ class RouteHandler
     const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const;
   bool getLeftLaneChangeTargetExceptPreferredLane(
     const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const;
-  static bool getPullOverTarget(
-    const lanelet::ConstLanelets & lanelets, const Pose & goal_pose,
-    lanelet::ConstLanelet * target_lanelet);
-  static bool getPullOutStartLane(
-    const lanelet::ConstLanelets & lanelets, const Pose & pose, const double vehicle_width,
-    lanelet::ConstLanelet * target_lanelet);
+  std::optional<lanelet::ConstLanelet> getPullOverTarget(const Pose & goal_pose) const;
+  std::optional<lanelet::ConstLanelet> getPullOutStartLane(
+    const Pose & pose, const double vehicle_width) const;
   double getLaneChangeableDistance(const Pose & current_pose, const Direction & direction) const;
-  bool getLeftShoulderLanelet(
-    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet) const;
-  bool getRightShoulderLanelet(
-    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const;
+  lanelet::ConstLanelets getRoadLaneletsAtPose(const Pose & pose) const;
+  std::optional<lanelet::ConstLanelet> getLeftShoulderLanelet(
+    const lanelet::ConstLanelet & lanelet) const;
+  std::optional<lanelet::ConstLanelet> getRightShoulderLanelet(
+    const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const;
   lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const;
   bool isPreferredLane(const lanelet::ConstLanelet & lanelet) const;
-  lanelet::ConstLanelets getClosestLanelets(const geometry_msgs::msg::Pose & target_pose) const;
 
 private:
   // MUST
   lanelet::routing::RoutingGraphPtr routing_graph_ptr_;
   lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_;
   std::shared_ptr<const lanelet::routing::RoutingGraphContainer> overall_graphs_ptr_;
   lanelet::LaneletMapPtr lanelet_map_ptr_;
-  lanelet::ConstLanelets road_lanelets_;
   lanelet::ConstLanelets route_lanelets_;
   lanelet::ConstLanelets preferred_lanelets_;
   lanelet::ConstLanelets start_lanelets_;
   lanelet::ConstLanelets goal_lanelets_;
-  lanelet::ConstLanelets shoulder_lanelets_;
   std::shared_ptr<LaneletRoute> route_ptr_{nullptr};
 
   rclcpp::Logger logger_{rclcpp::get_logger("route_handler")};
@@ -409,19 +407,18 @@ class RouteHandler
     const lanelet::ConstLanelet & lanelet,
     const double min_length = std::numeric_limits<double>::max(),
     const bool only_route_lanes = true) const;
-  bool getFollowingShoulderLanelet(
-    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * following_lanelet) const;
+  std::optional<lanelet::ConstLanelet> getFollowingShoulderLanelet(
+    const lanelet::ConstLanelet & lanelet) const;
   lanelet::ConstLanelets getShoulderLaneletSequenceAfter(
     const lanelet::ConstLanelet & lanelet,
     const double min_length = std::numeric_limits<double>::max()) const;
-  bool getPreviousShoulderLanelet(
-    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const;
+  std::optional<lanelet::ConstLanelet> getPreviousShoulderLanelet(
+    const lanelet::ConstLanelet & lanelet) const;
   lanelet::ConstLanelets getShoulderLaneletSequenceUpTo(
     const lanelet::ConstLanelet & lanelet,
     const double min_length = std::numeric_limits<double>::max()) const;
   lanelet::ConstLanelets getPreviousLaneletSequence(
     const lanelet::ConstLanelets & lanelet_sequence) const;
-  lanelet::ConstLanelets getClosestLaneletSequence(const Pose & pose) const;
   lanelet::ConstLanelets getLaneChangeTargetLanes(const Pose & pose) const;
   lanelet::ConstLanelets getLaneSequenceUpTo(const lanelet::ConstLanelet & lanelet) const;
   lanelet::ConstLanelets getLaneSequenceAfter(const lanelet::ConstLanelet & lanelet) const;
@@ -441,17 +438,41 @@ class RouteHandler
    */
   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.
+   * @brief Searches for the shortest path between start and goal lanelets 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.
+   * @return the lanelet path (if found)
    */
-  bool findDrivableLanePath(
-    const lanelet::ConstLanelet & start_lanelet, const lanelet::ConstLanelet & goal_lanelet,
-    lanelet::routing::LaneletPath & drivable_lane_path) const;
+  std::optional<lanelet::routing::LaneletPath> findDrivableLanePath(
+    const lanelet::ConstLanelet & start_lanelet, const lanelet::ConstLanelet & goal_lanelet) const;
 };
+
+/// @brief custom routing cost with infinity cost for no drivable lanes
+class RoutingCostDrivable : public lanelet::routing::RoutingCostDistance
+{
+public:
+  RoutingCostDrivable() : lanelet::routing::RoutingCostDistance(10.0) {}
+  inline double getCostSucceeding(
+    const lanelet::traffic_rules::TrafficRules & trafficRules,
+    const lanelet::ConstLaneletOrArea & from, const lanelet::ConstLaneletOrArea & to) const
+  {
+    if (
+      (from.isLanelet() && RouteHandler::isNoDrivableLane(*from.lanelet())) ||
+      (to.isLanelet() && RouteHandler::isNoDrivableLane(*to.lanelet())))
+      return std::numeric_limits<double>::infinity();
+    return lanelet::routing::RoutingCostDistance::getCostSucceeding(trafficRules, from, to);
+  }
+  inline double getCostLaneChange(
+    const lanelet::traffic_rules::TrafficRules & trafficRules, const lanelet::ConstLanelets & from,
+    const lanelet::ConstLanelets & to) const noexcept
+  {
+    if (
+      std::any_of(from.begin(), from.end(), RouteHandler::isNoDrivableLane) ||
+      std::any_of(to.begin(), to.end(), RouteHandler::isNoDrivableLane))
+      return std::numeric_limits<double>::infinity();
+    return lanelet::routing::RoutingCostDistance::getCostLaneChange(trafficRules, from, to);
+  }
+};  // class RoutingCostDrivable
 }  // namespace route_handler
 #endif  // ROUTE_HANDLER__ROUTE_HANDLER_HPP_