feat(mission_planner): check shoulder lanelets for check_goal_footpri…

…nt (#6053)

Signed-off-by: kosuke55 <[email protected]>

planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

@@ -292,7 +292,8 @@ bool DefaultPlanner::check_goal_footprint(
     lanelet::ConstLanelets lanelets;
     lanelets.push_back(combined_prev_lanelet);
     lanelets.push_back(next_lane);
-    lanelet::ConstLanelet combined_lanelets = combine_lanelets(lanelets);
+    lanelet::ConstLanelet combined_lanelets =
+      combine_lanelets_with_shoulder(lanelets, shoulder_lanelets_);
 
     // if next lanelet length longer than vehicle longitudinal offset
     if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) {
@@ -351,7 +352,8 @@ bool DefaultPlanner::is_goal_valid(
 
   double next_lane_length = 0.0;
   // combine calculated route lanelets
-  lanelet::ConstLanelet combined_prev_lanelet = combine_lanelets(path_lanelets);
+  lanelet::ConstLanelet combined_prev_lanelet =
+    combine_lanelets_with_shoulder(path_lanelets, shoulder_lanelets_);
 
   // check if goal footprint exceeds lane when the goal isn't in parking_lot
   if (

planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp

@@ -54,7 +54,8 @@ void insert_marker_array(
   a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end());
 }
 
-lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets)
+lanelet::ConstLanelet combine_lanelets_with_shoulder(
+  const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets)
 {
   lanelet::Points3d lefts;
   lanelet::Points3d rights;
@@ -70,17 +71,48 @@ lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets)
   }
 
   for (const auto & llt : lanelets) {
-    if (std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) {
-      for (const auto & pt : llt.leftBound()) {
-        lefts.push_back(lanelet::Point3d(pt));
-      }
+    // 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()) {
+      // if exist, add left bound of SHOULDER lanelet to lefts
+      add_bound(left_shared_shoulder_it->leftBound(), lefts);
+    } else if (
+      // if not exist, add left bound of lanelet to lefts
+      std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) {
+      add_bound(llt.leftBound(), lefts);
     }
-    if (std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) {
-      for (const auto & pt : llt.rightBound()) {
-        rights.push_back(lanelet::Point3d(pt));
-      }
+
+    // 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()) {
+      // if exist, add right bound of SHOULDER lanelet to rights
+      add_bound(right_shared_shoulder_it->rightBound(), rights);
+    } else if (
+      // if not exist, add right bound of lanelet to rights
+      std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) {
+      add_bound(llt.rightBound(), rights);
     }
   }
+
   const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts);
   const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights);
   auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound);

planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp

@@ -49,7 +49,8 @@ void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, doub
 void insert_marker_array(
   visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2);
 
-lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets);
+lanelet::ConstLanelet combine_lanelets_with_shoulder(
+  const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets);
 std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet);
 geometry_msgs::msg::Pose convertBasicPoint3dToPose(
   const lanelet::BasicPoint3d & point, const double lane_yaw);