fix(goal_planner): failed to generate candidate paths when entering another lane for avoidance (autowarefoundation#11560)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp

@@ -318,9 +318,26 @@ std::optional<lanelet::ConstLanelet> find_last_lane_change_completed_lanelet(
   const lanelet::routing::RoutingGraphConstPtr routing_graph);
 
 /**
- * @brief generate lanelets with which pull over path is aligned
- * @note if lane changing path is detected, this returns lanelets aligned with later part of the
- * lane changing path
+ * @brief Get reference road lane sequence that covers both the path length and goal search range
+ *
+ * @param path The path with lane IDs from upstream module
+ * @param planner_data Shared pointer to planner data containing route handler and parameters
+ * @param backward_length Distance to extend backward from lane_change_complete_lane
+ *                        Expected: backward_path_length + backward_goal_search_length
+ *                        - Covers both path length and goal search range backward
+ *                        - Sum is used because which is longer is unknown
+ *                        - Backward lanes are necessary for path generation
+ * @param forward_length Distance to extend forward from goal_lane
+ *                       Expected: forward_goal_search_length
+ *                       - Covers goal search range beyond the path length forward
+ *
+ * @return Continuous lanelet sequence from (lane_change_complete_lane - backward_length) to
+ *         (goal_lane + forward_length) if goal_lane is reachable from lane_change_complete_lane.
+ *         Otherwise, returns sequence from (lane_change_complete_lane - backward_length) to
+ *         the end of lane_change_complete_lane's sequence (until loop or no next lane).
+ *
+ * @note If lane changing path is detected, this returns lanelets aligned with the later part
+ *       of the lane changing path (lane_change_complete_lane)
  */
 lanelet::ConstLanelets get_reference_lanelets_for_pullover(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp

@@ -48,6 +48,7 @@ std::optional<PullOverPath> BezierPullOver::plan(
   [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output)
 {
   const auto & route_handler = planner_data->route_handler;
+  const double backward_path_length = planner_data->parameters.backward_path_length;
   const double min_jerk = parameters_.minimum_lateral_jerk;
   const double max_jerk = parameters_.maximum_lateral_jerk;
   const double backward_search_length = parameters_.backward_goal_search_length;
@@ -57,7 +58,8 @@ std::optional<PullOverPath> BezierPullOver::plan(
     std::abs(max_jerk - min_jerk) / shift_sampling_num;
 
   const auto road_lanes = goal_planner_utils::get_reference_lanelets_for_pullover(
-    upstream_module_output.path, planner_data, backward_search_length, forward_search_length);
+    upstream_module_output.path, planner_data, backward_path_length + backward_search_length,
+    forward_search_length);
 
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *route_handler, left_side_parking_, backward_search_length, forward_search_length);
@@ -92,10 +94,11 @@ std::vector<PullOverPath> BezierPullOver::plans(
   const int shift_sampling_num = parameters_.shift_sampling_num;
   [[maybe_unused]] const double jerk_resolution =
     std::abs(max_jerk - min_jerk) / shift_sampling_num;
+  const double backward_path_length = planner_data->parameters.backward_path_length;
 
-  const auto road_lanes = utils::getExtendedCurrentLanesFromPath(
-    upstream_module_output.path, planner_data, backward_search_length, forward_search_length,
-    /*forward_only_in_route*/ false);
+  const auto road_lanes = goal_planner_utils::get_reference_lanelets_for_pullover(
+    upstream_module_output.path, planner_data, backward_search_length + backward_path_length,
+    forward_search_length);
 
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *route_handler, left_side_parking_, backward_search_length, forward_search_length);

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp

@@ -48,11 +48,13 @@ std::optional<PullOverPath> GeometricPullOver::plan(
   [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output)
 {
   const auto & route_handler = planner_data->route_handler;
+  const double backward_path_length = planner_data->parameters.backward_path_length;
 
   const auto & goal_pose = modified_goal_pose.goal_pose;
   // prepare road nad shoulder lanes
   const auto road_lanes = goal_planner_utils::get_reference_lanelets_for_pullover(
-    upstream_module_output.path, planner_data, parameters_.backward_goal_search_length,
+    upstream_module_output.path, planner_data,
+    backward_path_length + parameters_.backward_goal_search_length,
     parameters_.forward_goal_search_length);
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *route_handler, left_side_parking_, parameters_.backward_goal_search_length,

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp

@@ -47,6 +47,7 @@ std::optional<PullOverPath> ShiftPullOver::plan(
   const BehaviorModuleOutput & upstream_module_output)
 {
   const auto & route_handler = planner_data->route_handler;
+  const double backward_path_length = planner_data->parameters.backward_path_length;
   const double min_jerk = parameters_.minimum_lateral_jerk;
   const double max_jerk = parameters_.maximum_lateral_jerk;
   const double backward_search_length = parameters_.backward_goal_search_length;
@@ -55,7 +56,8 @@ std::optional<PullOverPath> ShiftPullOver::plan(
   const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num;
 
   const auto road_lanes = goal_planner_utils::get_reference_lanelets_for_pullover(
-    upstream_module_output.path, planner_data, backward_search_length, forward_search_length);
+    upstream_module_output.path, planner_data, backward_path_length + backward_search_length,
+    forward_search_length);
 
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *route_handler, left_side_parking_, backward_search_length, forward_search_length);

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp

@@ -1142,36 +1142,70 @@ lanelet::ConstLanelets get_reference_lanelets_for_pullover(
 {
   const auto & routing_graph = planner_data->route_handler->getRoutingGraphPtr();
   const auto & lanelet_map = planner_data->route_handler->getLaneletMapPtr();
+  const auto & route_handler = planner_data->route_handler;
+
+  const auto goal_lane_id = planner_data->route_handler->getGoalLaneId();
+
   const auto lane_change_complete_lane =
     find_last_lane_change_completed_lanelet(path, lanelet_map, routing_graph);
+
   if (!lane_change_complete_lane) {
     return utils::getExtendedCurrentLanesFromPath(
       path, planner_data, backward_length, forward_length,
       /*forward_only_in_route*/ false);
   }
-  auto route_lanes = planner_data->route_handler->getLaneletSequence(
-    *lane_change_complete_lane, backward_length, forward_length);
-  const double remaining_distance =
-    forward_length + backward_length - lanelet::utils::getLaneletLength3d(route_lanes);
-  if (route_lanes.empty() || remaining_distance <= 0.0) {
-    return route_lanes;
+
+  const auto extend_forward = [&](
+                                const lanelet::ConstLanelet & start_lane, const double distance,
+                                lanelet::ConstLanelets & result) {
+    double acc_dist = 0.0;
+    auto current_lane = start_lane;
+    while (acc_dist < distance) {
+      const auto nexts = routing_graph->following(current_lane);
+      if (nexts.empty()) {
+        break;
+      }
+      current_lane = nexts.front();
+      if (lanelet::utils::contains(result, current_lane)) {
+        // loop detected
+        break;
+      }
+      result.push_back(current_lane);
+      acc_dist += lanelet::utils::getLaneletLength3d(current_lane);
+    }
+  };
+
+  lanelet::ConstLanelets route_lanes;
+
+  // Add backward lanes from lane_change_complete_lane
+  const auto backward_lanes = route_handler->getPrecedingLaneletSequence(
+    *lane_change_complete_lane, backward_length, {*lane_change_complete_lane});
+  for (auto it = backward_lanes.rbegin(); it != backward_lanes.rend(); ++it) {
+    route_lanes.insert(route_lanes.end(), it->begin(), it->end());
   }
-  double acc_dist = 0.0;
-  auto last_lanelet = route_lanes.back();
-  while (acc_dist < remaining_distance) {
-    const auto nexts = routing_graph->following(last_lanelet);
+
+  route_lanes.push_back(*lane_change_complete_lane);
+
+  // Extend forward from lane_change_complete_lane
+  auto current_lane = *lane_change_complete_lane;
+  while (true) {
+    const auto nexts = routing_graph->following(current_lane);
     if (nexts.empty()) {
       break;
     }
-    const auto & next = nexts.front();
-    if (lanelet::utils::contains(route_lanes, next)) {
-      // loop
+    current_lane = nexts.front();
+    if (lanelet::utils::contains(route_lanes, current_lane)) {
+      // loop detected
+      break;
+    }
+    route_lanes.push_back(current_lane);
+
+    if (current_lane.id() == goal_lane_id) {
+      extend_forward(current_lane, forward_length, route_lanes);
       break;
     }
-    last_lanelet = next;
-    route_lanes.push_back(next);
-    acc_dist += lanelet::utils::getLaneletLength3d(next);
   }
+
   return route_lanes;
 }
 }  // namespace autoware::behavior_path_planner::goal_planner_utils