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

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

Author: maxime-clem

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.
@@ -98,7 +98,7 @@ bool is_in_parking_lot(
 }
 
 double project_goal_to_map(
-  const lanelet::Lanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point)
+  const lanelet::ConstLanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point)
 {
   const lanelet::ConstLineString3d center_line =
     lanelet::utils::generateFineCenterline(lanelet_component);
@@ -182,12 +182,6 @@ bool DefaultPlanner::ready() const
 void DefaultPlanner::map_callback(const HADMapBin::ConstSharedPtr msg)
 {
   route_handler_.setMap(*msg);
-  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;
 }
 
@@ -199,7 +193,7 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
 
   for (const auto & route_section : route.segments) {
     for (const auto & lane_id : route_section.primitives) {
-      auto lanelet = lanelet_map_ptr_->laneletLayer.get(lane_id.id);
+      auto lanelet = route_handler_.getLaneletsFromId(lane_id.id);
       route_lanelets.push_back(lanelet);
       if (route_section.preferred_primitive.id == lane_id.id) {
         goal_lanelets.push_back(lanelet);
@@ -274,15 +268,15 @@ bool DefaultPlanner::check_goal_footprint_inside_lanes(
   if (boost::geometry::within(goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon())) {
     return true;
   }
-
+  const auto following = route_handler_.getNextLanelets(current_lanelet);
   // check if goal footprint is in between many lanelets in depth-first search manner
-  for (const auto & next_lane : routing_graph_ptr_->following(current_lanelet)) {
+  for (const auto & next_lane : following) {
     next_lane_length += lanelet::utils::getLaneletLength2d(next_lane);
     lanelet::ConstLanelets lanelets;
     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,23 +311,38 @@ bool DefaultPlanner::is_goal_valid(
 
   // 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 =
+      route_handler_.getLaneletMapPtr()->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();
@@ -345,15 +354,15 @@ bool DefaultPlanner::is_goal_valid(
   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 (
     param_.check_footprint_inside_lanes &&
     !check_goal_footprint_inside_lanes(
       closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) &&
     !is_in_parking_lot(
-      lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_),
+      lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr()),
       lanelet::utils::conversion::toLaneletPoint(goal.position))) {
     RCLCPP_WARN(logger, "Goal's footprint exceeds lane!");
     return false;
@@ -371,13 +380,15 @@ bool DefaultPlanner::is_goal_valid(
   }
 
   // check if goal is in parking space
-  const auto parking_spaces = lanelet::utils::query::getAllParkingSpaces(lanelet_map_ptr_);
+  const auto parking_spaces =
+    lanelet::utils::query::getAllParkingSpaces(route_handler_.getLaneletMapPtr());
   if (is_in_parking_space(parking_spaces, goal_lanelet_pt)) {
     return true;
   }
 
   // check if goal is in parking lot
-  const auto parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_);
+  const auto parking_lots =
+    lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr());
   if (is_in_parking_lot(parking_lots, goal_lanelet_pt)) {
     return true;
   }
@@ -424,7 +435,8 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points)
   auto goal_pose = points.back();
   if (param_.enable_correct_goal_pose) {
     goal_pose = get_closest_centerline_pose(
-      lanelet::utils::query::laneletLayer(lanelet_map_ptr_), goal_pose, vehicle_info_);
+      lanelet::utils::query::laneletLayer(route_handler_.getLaneletMapPtr()), goal_pose,
+      vehicle_info_);
   }
 
   if (!is_goal_valid(goal_pose, all_route_lanelets)) {
@@ -451,7 +463,7 @@ geometry_msgs::msg::Pose DefaultPlanner::refine_goal_height(
   const Pose & goal, const RouteSections & route_sections)
 {
   const auto goal_lane_id = route_sections.back().preferred_primitive.id;
-  lanelet::Lanelet goal_lanelet = lanelet_map_ptr_->laneletLayer.get(goal_lane_id);
+  const auto goal_lanelet = route_handler_.getLaneletsFromId(goal_lane_id);
   const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position);
   const auto goal_height = project_goal_to_map(goal_lanelet, goal_lanelet_pt);
 

planning/mission_planner/src/lanelet2_plugins/default_planner.hpp

@@ -58,11 +58,6 @@ class DefaultPlanner : public mission_planner::PlannerPlugin
   using RouteSections = std::vector<autoware_planning_msgs::msg::LaneletSegment>;
   using Pose = geometry_msgs::msg::Pose;
   bool is_graph_ready_;
-  lanelet::LaneletMapPtr lanelet_map_ptr_;
-  lanelet::routing::RoutingGraphPtr routing_graph_ptr_;
-  lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_;
-  lanelet::ConstLanelets road_lanelets_;
-  lanelet::ConstLanelets shoulder_lanelets_;
   route_handler::RouteHandler route_handler_;
 
   DefaultPlannerParameters param_;

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(