use route_handler to get the left/right shoulder lanelet

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

Author: maxime-clem

planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

@@ -281,7 +281,7 @@ bool DefaultPlanner::check_goal_footprint_inside_lanes(
     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) {
@@ -352,7 +352,7 @@ 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 (

planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp

@@ -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

@@ -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(