fix(route_handler): detect looped road shoulders in getShoulderLaneletSequence() (#6633)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -1856,6 +1856,8 @@ bool GoalPlannerModule::isCrossingPossible(
     Pose end_lane_pose{};
     end_lane_pose.orientation.w = 1.0;
     end_lane_pose.position = lanelet::utils::conversion::toGeomMsgPt(end_lane.centerline().front());
+    // NOTE: this line does not specify the /forward/backward length, so if the shoulders form a
+    // loop, this returns all shoulder lanes in the loop
     end_lane_sequence = route_handler->getShoulderLaneletSequence(end_lane, end_lane_pose);
   } else {
     const double dist = std::numeric_limits<double>::max();

planning/route_handler/src/route_handler.cpp

@@ -758,12 +758,18 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceAfter(
 
   double length = 0;
   lanelet::ConstLanelet current_lanelet = lanelet;
+  std::set<lanelet::Id> searched_ids{};
   while (rclcpp::ok() && length < min_length) {
     lanelet::ConstLanelet next_lanelet;
     if (!getFollowingShoulderLanelet(current_lanelet, &next_lanelet)) {
       break;
     }
     lanelet_sequence_forward.push_back(next_lanelet);
+    if (searched_ids.find(next_lanelet.id()) != searched_ids.end()) {
+      // loop shoulder detected
+      break;
+    }
+    searched_ids.insert(next_lanelet.id());
     current_lanelet = next_lanelet;
     length +=
       static_cast<double>(boost::geometry::length(next_lanelet.centerline().basicLineString()));
@@ -794,13 +800,19 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceUpTo(
 
   double length = 0;
   lanelet::ConstLanelet current_lanelet = lanelet;
+  std::set<lanelet::Id> searched_ids{};
   while (rclcpp::ok() && length < min_length) {
     lanelet::ConstLanelet prev_lanelet;
     if (!getPreviousShoulderLanelet(current_lanelet, &prev_lanelet)) {
       break;
     }
 
     lanelet_sequence_backward.insert(lanelet_sequence_backward.begin(), prev_lanelet);
+    if (searched_ids.find(prev_lanelet.id()) != searched_ids.end()) {
+      // loop shoulder detected
+      break;
+    }
+    searched_ids.insert(prev_lanelet.id());
     current_lanelet = prev_lanelet;
     length +=
       static_cast<double>(boost::geometry::length(prev_lanelet.centerline().basicLineString()));