fix(start_planner): fix path generation for separated lanes and extended loop lanes (#4418)

* fix(start_planner): fix path generation for separated lanes and extended loop lanes

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

* Add until goal lanes option

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

* add commnets

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

---------

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

Author: kosuke55

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -383,7 +383,7 @@ lanelet::ConstLanelets getExtendedCurrentLanes(
 
 lanelet::ConstLanelets getExtendedCurrentLanes(
   const std::shared_ptr<const PlannerData> & planner_data, const double backward_length,
-  const double forward_length);
+  const double forward_length, const bool until_goal_lane);
 
 lanelet::ConstLanelets calcLaneAroundPose(
   const std::shared_ptr<RouteHandler> route_handler, const geometry_msgs::msg::Pose & pose,

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -153,7 +153,8 @@ void GoalPlannerModule::onTimer()
   // generate valid pull over path candidates and calculate closest start pose
   const auto current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_->backward_goal_search_length,
-    parameters_->forward_goal_search_length);
+    parameters_->forward_goal_search_length,
+    /*until_goal_lane*/ false);
   std::vector<PullOverPath> path_candidates{};
   std::optional<Pose> closest_start_pose{};
   double min_start_arc_length = std::numeric_limits<double>::max();
@@ -590,7 +591,8 @@ void GoalPlannerModule::setLanes()
 {
   status_.current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_->backward_goal_search_length,
-    parameters_->forward_goal_search_length);
+    parameters_->forward_goal_search_length,
+    /*until_goal_lane*/ false);
   status_.pull_over_lanes =
     goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_);
   status_.lanes =

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -115,7 +115,8 @@ bool StartPlannerModule::isExecutionRequested() const
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
   const auto current_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max());
+    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    /*until_goal_lane*/ true);
   const auto pull_out_lanes =
     start_planner_utils::getPullOutLanes(planner_data_, backward_path_length);
   auto lanes = current_lanes;
@@ -326,7 +327,8 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
   const auto current_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max());
+    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    /*until_goal_lane*/ true);
 
   const auto pull_out_lanes =
     start_planner_utils::getPullOutLanes(planner_data_, backward_path_length);
@@ -562,7 +564,8 @@ void StartPlannerModule::updatePullOutStatus()
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
   status_.current_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max());
+    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    /*until_goal_lane*/ true);
   status_.pull_out_lanes =
     start_planner_utils::getPullOutLanes(planner_data_, backward_path_length);
 

planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp

@@ -45,7 +45,8 @@ boost::optional<PullOverPath> GeometricPullOver::plan(const Pose & goal_pose)
 
   // prepare road nad shoulder lanes
   const auto road_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length);
+    planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length,
+    /*until_goal_lane*/ false);
   const auto shoulder_lanes =
     goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_);
   if (road_lanes.empty() || shoulder_lanes.empty()) {

planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp

@@ -54,7 +54,9 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose)
 
   const auto pull_over_lanes =
     goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_);
-  auto lanes = utils::getExtendedCurrentLanes(planner_data_, backward_length, forward_length);
+  auto lanes = utils::getExtendedCurrentLanes(
+    planner_data_, backward_length, forward_length,
+    /*until_goal_lane*/ false);
   lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end());
 
   const auto goal_arc_coords =

planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp

@@ -46,8 +46,9 @@ boost::optional<PullOverPath> ShiftPullOver::plan(const Pose & goal_pose)
   const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num;
 
   // get road and shoulder lanes
-  const auto road_lanes =
-    utils::getExtendedCurrentLanes(planner_data_, backward_search_length, forward_search_length);
+  const auto road_lanes = utils::getExtendedCurrentLanes(
+    planner_data_, backward_search_length, forward_search_length,
+    /*until_goal_lane*/ false);
   const auto shoulder_lanes =
     goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_);
   if (road_lanes.empty() || shoulder_lanes.empty()) {

planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp

@@ -43,7 +43,8 @@ boost::optional<PullOutPath> GeometricPullOut::plan(Pose start_pose, Pose goal_p
   const double backward_path_length =
     planner_data_->parameters.backward_path_length + parameters_.max_back_distance;
   const auto road_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max());
+    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    /*until_goal_lane*/ true);
   const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length);
   auto lanes = road_lanes;
   for (const auto & pull_out_lane : pull_out_lanes) {

planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp

@@ -53,8 +53,8 @@ boost::optional<PullOutPath> ShiftPullOut::plan(Pose start_pose, Pose goal_pose)
   }
 
   const auto road_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max());
-
+    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
+    /*until_goal_lane*/ true);
   // find candidate paths
   auto pull_out_paths = calcPullOutPaths(
     *route_handler, road_lanes, start_pose, goal_pose, common_parameters, parameters_);

planning/behavior_path_planner/src/utils/start_planner/util.cpp

@@ -114,6 +114,8 @@ lanelet::ConstLanelets getPullOutLanes(
 
   // pull out from road lane
   return utils::getExtendedCurrentLanes(
-    planner_data, backward_length, /*forward_length*/ std::numeric_limits<double>::max());
+    planner_data, backward_length,
+    /*forward_length*/ std::numeric_limits<double>::max(),
+    /*until_goal_lane*/ true);
 }
 }  // namespace behavior_path_planner::start_planner_utils

planning/behavior_path_planner/src/utils/utils.cpp

@@ -2864,7 +2864,14 @@ lanelet::ConstLanelets extendNextLane(
   // Add next lane
   const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back());
   if (!next_lanes.empty()) {
-    extended_lanes.push_back(next_lanes.front());
+    // use the next lane in route if it exists
+    auto target_next_lane = next_lanes.front();
+    for (const auto & next_lane : next_lanes) {
+      if (route_handler->isRouteLanelet(next_lane)) {
+        target_next_lane = next_lane;
+      }
+    }
+    extended_lanes.push_back(target_next_lane);
   }
 
   return extended_lanes;
@@ -2878,9 +2885,15 @@ lanelet::ConstLanelets extendPrevLane(
   // Add previous lane
   const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front());
   if (!prev_lanes.empty()) {
-    extended_lanes.insert(extended_lanes.begin(), prev_lanes.front());
+    // use the previous lane in route if it exists
+    auto target_prev_lane = prev_lanes.front();
+    for (const auto & prev_lane : prev_lanes) {
+      if (route_handler->isRouteLanelet(prev_lane)) {
+        target_prev_lane = prev_lane;
+      }
+    }
+    extended_lanes.insert(extended_lanes.begin(), target_prev_lane);
   }
-
   return extended_lanes;
 }
 
@@ -2895,26 +2908,24 @@ lanelet::ConstLanelets extendLanes(
 
 lanelet::ConstLanelets getExtendedCurrentLanes(
   const std::shared_ptr<const PlannerData> & planner_data, const double backward_length,
-  const double forward_length)
+  const double forward_length, const bool until_goal_lane)
 {
   auto lanes = getCurrentLanes(planner_data);
   if (lanes.empty()) return lanes;
+  const auto start_lane = lanes.front();
 
   double forward_length_sum = 0.0;
   double backward_length_sum = 0.0;
 
-  const auto is_loop = [&](const auto & target_lane) {
-    auto it = std::find_if(lanes.begin(), lanes.end(), [&](const lanelet::ConstLanelet & lane) {
-      return lane.id() == target_lane.id();
-    });
-
-    return it != lanes.end();
-  };
-
   while (backward_length_sum < backward_length) {
     auto extended_lanes = extendPrevLane(planner_data->route_handler, lanes);
-
-    if (extended_lanes.empty() || is_loop(extended_lanes.front())) {
+    if (extended_lanes.empty()) {
+      return lanes;
+    }
+    // loop check
+    // if current map lanes is looping and has a very large value for backward_length,
+    // the extending process will not finish.
+    if (extended_lanes.front().id() == start_lane.id()) {
       return lanes;
     }
 
@@ -2927,9 +2938,25 @@ lanelet::ConstLanelets getExtendedCurrentLanes(
   }
 
   while (forward_length_sum < forward_length) {
-    auto extended_lanes = extendNextLane(planner_data->route_handler, lanes);
+    // stop extending if the goal lane is included
+    // if forward_length is a very large value, set it to true,
+    // as it may continue to extend lanes outside the route ahead of goal forever.
+    if (until_goal_lane) {
+      lanelet::ConstLanelet goal_lane;
+      planner_data->route_handler->getGoalLanelet(&goal_lane);
+      if (lanes.back().id() == goal_lane.id()) {
+        return lanes;
+      }
+    }
 
-    if (extended_lanes.empty() || is_loop(extended_lanes.back())) {
+    auto extended_lanes = extendNextLane(planner_data->route_handler, lanes);
+    if (extended_lanes.empty()) {
+      return lanes;
+    }
+    // loop check
+    // if current map lanes is looping and has a very large value for forward_length,
+    // the extending process will not finish.
+    if (extended_lanes.back().id() == start_lane.id()) {
       return lanes;
     }