hotfix: cherry pick start planner and goal planner (#655)

* fix(start_planner): fix path cut by U turn goal (autowarefoundation#4109)

* fix(start_planner): fix path cut by U turn goal

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

* cut path

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

---------

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

* feat(behavior_path_planner): add getExtendedCurrentLanes with length args (autowarefoundation#4166)

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

* fix(goal_planner): support following lanes over the next lane (autowarefoundation#4167)

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

* feat(behavior_path_planner): add getCurrentLanes with length args (autowarefoundation#4108)

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

---------

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Co-authored-by: Kosuke Takeuchi <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/include/behavior_path_planner/utils/utils.hpp

@@ -350,17 +350,30 @@ BehaviorModuleOutput getReferencePath(
 // object label
 std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification);
 
+lanelet::ConstLanelets getCurrentLanes(
+  const std::shared_ptr<const PlannerData> & planner_data, const double backward_path_length,
+  const double forward_path_length);
 lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr<const PlannerData> & planner_data);
 
 lanelet::ConstLanelets getCurrentLanesFromPath(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data);
 
+lanelet::ConstLanelets extendNextLane(
+  const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes);
+
+lanelet::ConstLanelets extendPrevLane(
+  const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes);
+
 lanelet::ConstLanelets extendLanes(
   const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes);
 
 lanelet::ConstLanelets getExtendedCurrentLanes(
   const std::shared_ptr<const PlannerData> & planner_data);
 
+lanelet::ConstLanelets getExtendedCurrentLanes(
+  const std::shared_ptr<const PlannerData> & planner_data, const double backward_length,
+  const double forward_length);
+
 lanelet::ConstLanelets calcLaneAroundPose(
   const std::shared_ptr<RouteHandler> route_handler, const geometry_msgs::msg::Pose & pose,
   const double forward_length, const double backward_length,

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

@@ -160,7 +160,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();
@@ -599,7 +601,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/scene_module/start_planner/start_planner_module.cpp

@@ -146,7 +146,12 @@ bool StartPlannerModule::isExecutionRequested() const
     tier4_autoware_utils::pose2transform(planner_data_->self_odometry->pose.pose));
 
   // Check if ego is not out of lanes
-  const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
+  // const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
+  const double backward_path_length =
+    planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
+  const auto current_lanes =
+    utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());
+
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_);
   auto lanes = current_lanes;
   lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end());
@@ -352,7 +357,12 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
     return output;
   }
 
-  const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
+  // const auto current_lanes = utils::getExtendedCurrentLanes(planner_data_);
+  const double backward_path_length =
+    planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
+  const auto current_lanes =
+    utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());
+
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_);
   auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path;
   const auto drivable_lanes =
@@ -586,7 +596,10 @@ void StartPlannerModule::updatePullOutStatus()
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
   const auto & goal_pose = planner_data_->route_handler->getGoalPose();
 
-  status_.current_lanes = utils::getExtendedCurrentLanes(planner_data_);
+  const double backward_path_length =
+    planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
+  status_.current_lanes =
+    utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());
   status_.pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_);
 
   // combine road and shoulder 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;
     }
@@ -316,11 +319,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.
@@ -340,17 +342,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()) {

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

@@ -40,7 +40,11 @@ boost::optional<PullOutPath> GeometricPullOut::plan(Pose start_pose, Pose goal_p
   PullOutPath output;
 
   // combine road lane and shoulder lane
-  const auto road_lanes = utils::getExtendedCurrentLanes(planner_data_);
+  const double backward_path_length =
+    planner_data_->parameters.backward_path_length + parameters_.max_back_distance;
+  const auto road_lanes =
+    utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());
+
   const auto shoulder_lanes = getPullOutLanes(planner_data_);
   auto lanes = road_lanes;
   lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end());

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

@@ -44,12 +44,16 @@ boost::optional<PullOutPath> ShiftPullOut::plan(Pose start_pose, Pose goal_pose)
   const auto & route_handler = planner_data_->route_handler;
   const auto & common_parameters = planner_data_->parameters;
   const auto & dynamic_objects = planner_data_->dynamic_object;
-  const auto & road_lanes = utils::getExtendedCurrentLanes(planner_data_);
-  const auto & shoulder_lanes = getPullOutLanes(planner_data_);
+  const auto shoulder_lanes = getPullOutLanes(planner_data_);
   if (shoulder_lanes.empty()) {
     return boost::none;
   }
 
+  const double backward_path_length =
+    planner_data_->parameters.backward_path_length + parameters_.max_back_distance;
+  const auto road_lanes =
+    utils::getCurrentLanes(planner_data_, backward_path_length, std::numeric_limits<double>::max());
+
   // find candidate paths
   auto pull_out_paths = calcPullOutPaths(
     *route_handler, road_lanes, shoulder_lanes, start_pose, goal_pose, common_parameters,
@@ -135,6 +139,7 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   }
 
   // rename parameter
+  const double forward_path_length = common_parameter.forward_path_length;
   const double backward_path_length = common_parameter.backward_path_length;
   const double shift_pull_out_velocity = parameter.shift_pull_out_velocity;
   const double minimum_shift_pull_out_distance = parameter.minimum_shift_pull_out_distance;
@@ -148,15 +153,25 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   const auto arc_position_start = getArcCoordinates(road_lanes, start_pose);
   const double s_start = std::max(arc_position_start.length - backward_path_length, 0.0);
   const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose);
-  const double road_lanes_length = std::accumulate(
-    road_lanes.begin(), road_lanes.end(), 0.0, [](const double sum, const auto & lane) {
-      return sum + lanelet::utils::getLaneletLength2d(lane);
-    });
-  // if goal is behind start pose,
+
+  // if goal is behind start pose, use path with forward_path_length
   const bool goal_is_behind = arc_position_goal.length < s_start;
-  const double s_end = goal_is_behind ? road_lanes_length : arc_position_goal.length;
-  PathWithLaneId road_lane_reference_path =
-    utils::resamplePathWithSpline(route_handler.getCenterLinePath(road_lanes, s_start, s_end), 1.0);
+  const double s_forward_length = s_start + forward_path_length;
+  const double s_end =
+    goal_is_behind ? s_forward_length : std::min(arc_position_goal.length, s_forward_length);
+
+  constexpr double RESAMPLE_INTERVAL = 1.0;
+  PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline(
+    route_handler.getCenterLinePath(road_lanes, s_start, s_end), RESAMPLE_INTERVAL);
+
+  // non_shifted_path for when shift length or pull out distance is too short
+  const PullOutPath non_shifted_path = std::invoke([&]() {
+    PullOutPath non_shifted_path{};
+    non_shifted_path.partial_paths.push_back(road_lane_reference_path);
+    non_shifted_path.start_pose = start_pose;
+    non_shifted_path.end_pose = start_pose;
+    return non_shifted_path;
+  });
 
   bool has_non_shifted_path = false;
   for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk;