perf(mission_planner): remove queries on all road and shoulder lanelets

planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

@@ -1,4 +1,4 @@
-// Copyright 2019 Autoware Foundation
+// Copyright 2019-2024 Autoware Foundation
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -185,9 +185,6 @@
   lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
   lanelet::utils::conversion::fromBinMsg(
     *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_);
-  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_);
-  road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets);
-  shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets);
   is_graph_ready_ = true;
 }
 
@@ -282,7 +279,7 @@
     lanelets.push_back(combined_prev_lanelet);
     lanelets.push_back(next_lane);
     lanelet::ConstLanelet combined_lanelets =
-      combine_lanelets_with_shoulder(lanelets, shoulder_lanelets_);
+      combine_lanelets_with_shoulder(lanelets, route_handler_);
 
     // if next lanelet length is longer than vehicle longitudinal offset
     if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) {
@@ -317,35 +314,49 @@
 
   // check if goal is in shoulder lanelet
   lanelet::Lanelet closest_shoulder_lanelet;
+  const auto shoulder_lanelets = route_handler_.getShoulderLaneletsAtPose(goal);
   if (lanelet::utils::query::getClosestLanelet(
-        shoulder_lanelets_, goal, &closest_shoulder_lanelet)) {
-    if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) {
-      const auto lane_yaw =
-        lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position);
-      const auto goal_yaw = tf2::getYaw(goal.orientation);
-      const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw);
-      const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg);
-      if (std::abs(angle_diff) < th_angle) {
-        return true;
-      }
+        shoulder_lanelets, goal, &closest_shoulder_lanelet)) {
+    const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position);
+    const auto goal_yaw = tf2::getYaw(goal.orientation);
+    const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw);
+    const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg);
+    if (std::abs(angle_diff) < th_angle) {
+      return true;
     }
   }
-
-  lanelet::Lanelet closest_lanelet;
-  if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) {
-    return false;
+  lanelet::ConstLanelet closest_lanelet;
+  const auto road_lanelets_at_goal = route_handler_.getRoadLaneletsAtPose(goal);
+  if (!lanelet::utils::query::getClosestLanelet(road_lanelets_at_goal, goal, &closest_lanelet)) {
+    // if no road lanelets directly at the goal, find the closest one
+    const lanelet::BasicPoint2d goal_point{goal.position.x, goal.position.y};
+    auto closest_dist = std::numeric_limits<double>::max();
+    const auto closest_road_lanelet_found = lanelet_map_ptr_->laneletLayer.nearestUntil(
+      goal_point, [&](const auto & bbox, const auto & ll) {
+        // this search is done by increasing distance between the bounding box and the goal
+        // we stop the search when the bounding box is further than the closest dist found
+        if (lanelet::geometry::distance2d(bbox, goal_point) > closest_dist)
+          return true;  // stop the search
+        const auto dist = lanelet::geometry::distance2d(goal_point, ll.polygon2d());
+        if (route_handler_.isRoadLanelet(ll) && dist < closest_dist) {
+          closest_dist = dist;
+          closest_lanelet = ll;
+        }
+        return false;  // continue the search
+      });
+    if (!closest_road_lanelet_found) return false;
   }
 
   const auto local_vehicle_footprint = vehicle_info_.createFootprint();
   tier4_autoware_utils::LinearRing2d goal_footprint =
     transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(goal));
   pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint));
   const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint);
 
   double next_lane_length = 0.0;
   // combine calculated route lanelets
   const lanelet::ConstLanelet combined_prev_lanelet =
-    combine_lanelets_with_shoulder(path_lanelets, shoulder_lanelets_);
+    combine_lanelets_with_shoulder(path_lanelets, route_handler_);
 
   // check if goal footprint exceeds lane when the goal isn't in parking_lot
   if (

planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp

@@ -1,4 +1,4 @@
-// Copyright 2019 Autoware Foundation
+// Copyright 2019-2024 Autoware Foundation
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -42,7 +42,7 @@ void insert_marker_array(
 }
 
 lanelet::ConstLanelet combine_lanelets_with_shoulder(
-  const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets)
+  const lanelet::ConstLanelets & lanelets, const route_handler::RouteHandler & route_handler)
 {
   lanelet::Points3d lefts;
   lanelet::Points3d rights;
@@ -57,30 +57,18 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder(
     }
   }
 
+  // lambda to add bound to target_bound
+  const auto add_bound = [](const auto & bound, auto & target_bound) {
+    std::transform(
+      bound.begin(), bound.end(), std::back_inserter(target_bound),
+      [](const auto & pt) { return lanelet::Point3d(pt); });
+  };
   for (const auto & llt : lanelets) {
-    // lambda to check if shoulder lane which share left bound with lanelets exist
-    const auto find_bound_shared_shoulder =
-      [&shoulder_lanelets](const auto & lanelet_bound, const auto & get_shoulder_bound) {
-        return std::find_if(
-          shoulder_lanelets.begin(), shoulder_lanelets.end(),
-          [&lanelet_bound, &get_shoulder_bound](const auto & shoulder_llt) {
-            return lanelet_bound.id() == get_shoulder_bound(shoulder_llt).id();
-          });
-      };
-
-    // lambda to add bound to target_bound
-    const auto add_bound = [](const auto & bound, auto & target_bound) {
-      std::transform(
-        bound.begin(), bound.end(), std::back_inserter(target_bound),
-        [](const auto & pt) { return lanelet::Point3d(pt); });
-    };
-
     // check if shoulder lanelets which has RIGHT bound same to LEFT bound of lanelet exist
-    const auto left_shared_shoulder_it = find_bound_shared_shoulder(
-      llt.leftBound(), [](const auto & shoulder_llt) { return shoulder_llt.rightBound(); });
-    if (left_shared_shoulder_it != shoulder_lanelets.end()) {
+    const auto left_shared_shoulder = route_handler.getLeftShoulderLanelet(llt);
+    if (left_shared_shoulder) {
       // if exist, add left bound of SHOULDER lanelet to lefts
-      add_bound(left_shared_shoulder_it->leftBound(), lefts);
+      add_bound(left_shared_shoulder->leftBound(), lefts);
     } else if (
       // if not exist, add left bound of lanelet to lefts
       // if the **left** of this lanelet does not match any of the **right** bounds of `lanelets`,
@@ -90,11 +78,10 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder(
     }
 
     // check if shoulder lanelets which has LEFT bound same to RIGHT bound of lanelet exist
-    const auto right_shared_shoulder_it = find_bound_shared_shoulder(
-      llt.rightBound(), [](const auto & shoulder_llt) { return shoulder_llt.leftBound(); });
-    if (right_shared_shoulder_it != shoulder_lanelets.end()) {
+    const auto right_shared_shoulder = route_handler.getRightShoulderLanelet(llt);
+    if (right_shared_shoulder) {
       // if exist, add right bound of SHOULDER lanelet to rights
-      add_bound(right_shared_shoulder_it->rightBound(), rights);
+      add_bound(right_shared_shoulder->rightBound(), rights);
     } else if (
       // if not exist, add right bound of lanelet to rights
       // if the **right** of this lanelet does not match any of the **left** bounds of `lanelets`,

planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp

@@ -1,4 +1,4 @@
-// Copyright 2019 Autoware Foundation
+// Copyright 2019-2024 Autoware Foundation
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -16,6 +16,7 @@
 #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_
 
 #include <rclcpp/rclcpp.hpp>
+#include <route_handler/route_handler.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
@@ -50,10 +51,10 @@ void insert_marker_array(
  * @brief create a single merged lanelet whose left/right bounds consist of the leftmost/rightmost
  * bound of the original lanelets or the left/right bound of its adjacent road_shoulder respectively
  * @param lanelets topologically sorted sequence of lanelets
- * @param shoulder_lanelets list of possible road_shoulder lanelets to combine with lanelets
+ * @param route_handler route handler to query the lanelet map
  */
 lanelet::ConstLanelet combine_lanelets_with_shoulder(
-  const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets);
+  const lanelet::ConstLanelets & lanelets, const route_handler::RouteHandler & route_handler);
 
 std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet);
 geometry_msgs::msg::Pose convertBasicPoint3dToPose(