fix(goal_planner): support following lanes over the next lane

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

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

@@ -127,13 +127,14 @@ class GeometricParallelParking
     const Pose & center, const double radius, const double yaw, const bool is_left_turn,
     const bool is_forward);
   boost::optional<Pose> calcStartPose(
-    const Pose & goal_pose, const double start_pose_offset, const double R_E_r,
-    const bool is_forward);
+    const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
+    const double start_pose_offset, const double R_E_r, const bool is_forward);
   std::vector<PathWithLaneId> generatePullOverPaths(
     const Pose & start_pose, const Pose & goal_pose, const double R_E_r,
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
     const bool is_forward, const double end_pose_offset, const double velocity);
-  PathWithLaneId generateStraightPath(const Pose & start_pose);
+  PathWithLaneId generateStraightPath(
+    const Pose & start_pose, const lanelet::ConstLanelets & road_lanes);
   void setVelocityToArcPaths(
     std::vector<PathWithLaneId> & arc_paths, const double velocity, const bool set_stop_end);
 

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

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

planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp

@@ -138,7 +138,7 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
   setVelocityToArcPaths(arc_paths, velocity, set_stop_end);
 
   // straight path from current to parking start
-  const auto straight_path = generateStraightPath(start_pose);
+  const auto straight_path = generateStraightPath(start_pose, road_lanes);
 
   // check the continuity of straight path and arc path
   const Pose & road_path_last_pose = straight_path.points.back().point.pose;
@@ -187,7 +187,8 @@ bool GeometricParallelParking::planPullOver(
     for (double steer = parameters_.forward_parking_max_steer_angle; steer > min_steer_rad;
          steer -= steer_interval) {
       const double R_E_r = common_params.wheel_base / std::tan(steer);
-      const auto start_pose = calcStartPose(arc_end_pose, start_pose_offset, R_E_r, is_forward);
+      const auto start_pose =
+        calcStartPose(arc_end_pose, road_lanes, start_pose_offset, R_E_r, is_forward);
       if (!start_pose) {
         continue;
       }
@@ -208,7 +209,8 @@ bool GeometricParallelParking::planPullOver(
     constexpr double offset_interval = 1.0;
     for (double start_pose_offset = 0; start_pose_offset < max_offset;
          start_pose_offset += offset_interval) {
-      const auto start_pose = calcStartPose(arc_end_pose, start_pose_offset, R_E_min_, is_forward);
+      const auto start_pose =
+        calcStartPose(arc_end_pose, road_lanes, start_pose_offset, R_E_min_, is_forward);
       if (!start_pose) {
         continue;
       }
@@ -238,7 +240,8 @@ bool GeometricParallelParking::planPullOut(
   for (double end_pose_offset = 0; end_pose_offset < max_offset;
        end_pose_offset += offset_interval) {
     // pull_out end pose which is the second arc path end
-    const auto end_pose = calcStartPose(start_pose, end_pose_offset, R_E_min_, is_forward);
+    const auto end_pose =
+      calcStartPose(start_pose, road_lanes, end_pose_offset, R_E_min_, is_forward);
     if (!end_pose) {
       continue;
     }
@@ -315,11 +318,10 @@ bool GeometricParallelParking::planPullOut(
 }
 
 boost::optional<Pose> GeometricParallelParking::calcStartPose(
-  const Pose & goal_pose, const double start_pose_offset, const double R_E_r, const bool is_forward)
+  const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const double start_pose_offset,
+  const double R_E_r, const bool is_forward)
 {
-  // Not use shoulder lanes.
-  const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
-  const auto arc_coordinates = lanelet::utils::getArcCoordinates(current_lanes, goal_pose);
+  const auto arc_coordinates = lanelet::utils::getArcCoordinates(road_lanes, goal_pose);
 
   // todo
   // When forwarding, the turning radius of the right and left will be the same.
@@ -339,17 +341,17 @@ boost::optional<Pose> GeometricParallelParking::calcStartPose(
   return start_pose;
 }
 
-PathWithLaneId GeometricParallelParking::generateStraightPath(const Pose & start_pose)
+PathWithLaneId GeometricParallelParking::generateStraightPath(
+  const Pose & start_pose, const lanelet::ConstLanelets & road_lanes)
 {
   // get straight path before parking.
-  const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
-  const auto start_arc_position = lanelet::utils::getArcCoordinates(current_lanes, start_pose);
+  const auto start_arc_position = lanelet::utils::getArcCoordinates(road_lanes, start_pose);
 
   const Pose current_pose = planner_data_->self_odometry->pose.pose;
-  const auto current_arc_position = lanelet::utils::getArcCoordinates(current_lanes, current_pose);
+  const auto current_arc_position = lanelet::utils::getArcCoordinates(road_lanes, current_pose);
 
   auto path = planner_data_->route_handler->getCenterLinePath(
-    current_lanes, current_arc_position.length, start_arc_position.length, true);
+    road_lanes, current_arc_position.length, start_arc_position.length, true);
   path.header = planner_data_->route_handler->getRouteHeader();
   if (!path.points.empty()) {
     path.points.back().point.longitudinal_velocity_mps = 0;

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

@@ -44,7 +44,8 @@ boost::optional<PullOverPath> GeometricPullOver::plan(const Pose & goal_pose)
   const auto & route_handler = planner_data_->route_handler;
 
   // prepare road nad shoulder lanes
-  const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_);
+  const auto road_lanes = utils::getExtendedCurrentLanes(
+    planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length);
   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,7 @@ 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_);
+  auto lanes = utils::getExtendedCurrentLanes(planner_data_, backward_length, forward_length);
   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

@@ -40,11 +40,14 @@ boost::optional<PullOverPath> ShiftPullOver::plan(const Pose & goal_pose)
   const auto & route_handler = planner_data_->route_handler;
   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;
+  const double forward_search_length = parameters_.forward_goal_search_length;
   const int shift_sampling_num = parameters_.shift_sampling_num;
   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_);
+  const auto road_lanes =
+    utils::getExtendedCurrentLanes(planner_data_, backward_search_length, forward_search_length);
   const auto shoulder_lanes =
     goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_);
   if (road_lanes.empty() || shoulder_lanes.empty()) {