fix(behavior_path_planner): fix path distortion of no back pull out (a…

…utowarefoundation#2829) (autowarefoundation#273)

* fix(behavior_path_planner): fix path distortion of no back pull out



* Update planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp

---------

Signed-off-by: kosuke55 <[email protected]>
Signed-off-by: tomoya.kimura <[email protected]>
Co-authored-by: Kosuke Takeuchi <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp

@@ -105,7 +105,7 @@ class PullOutModule : public SceneModuleInterface
   std::shared_ptr<PullOutPlannerBase> getCurrentPlanner() const;
   PathWithLaneId getFullPath() const;
   ParallelParkingParameters getGeometricPullOutParameters() const;
-  std::vector<Pose> searchBackedPoses();
+  std::vector<Pose> searchPullOutStartPoses();
 
   std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
 

planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp

@@ -533,16 +533,7 @@ void PullOutModule::updatePullOutStatus()
     util::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_out_lanes);
 
   // search pull out start candidates backward
-  std::vector<Pose> start_pose_candidates;
-  {
-    if (parameters_.enable_back) {
-      // the first element is current_pose
-      start_pose_candidates = searchBackedPoses();
-    } else {
-      // pull_out_start candidate is only current pose
-      start_pose_candidates.push_back(current_pose);
-    }
-  }
+  std::vector<Pose> start_pose_candidates = searchPullOutStartPoses();
 
   if (parameters_.search_priority == "efficient_path") {
     planWithPriorityOnEfficientPath(start_pose_candidates, goal_pose);
@@ -579,8 +570,9 @@ void PullOutModule::updatePullOutStatus()
 }
 
 // make this class?
-std::vector<Pose> PullOutModule::searchBackedPoses()
+std::vector<Pose> PullOutModule::searchPullOutStartPoses()
 {
+  std::vector<Pose> pull_out_start_pose{};
   const Pose & current_pose = planner_data_->self_pose->pose;
 
   // get backward shoulder path
@@ -597,9 +589,18 @@ std::vector<Pose> PullOutModule::searchBackedPoses()
     p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0);
   }
 
+  // if backward driving is disable, just refine current pose to the lanes
+  if (!parameters_.enable_back) {
+    const auto refined_pose =
+      calcLongitudinalOffsetPose(backward_shoulder_path.points, current_pose.position, 0);
+    if (refined_pose) {
+      pull_out_start_pose.push_back(*refined_pose);
+    }
+    return pull_out_start_pose;
+  }
+
   // check collision between footprint and object at the backed pose
   const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_);
-  std::vector<Pose> backed_poses;
   for (double back_distance = 0.0; back_distance <= parameters_.max_back_distance;
        back_distance += parameters_.backward_search_resolution) {
     const auto backed_pose = calcLongitudinalOffsetPose(
@@ -629,9 +630,9 @@ std::vector<Pose> PullOutModule::searchBackedPoses()
       break;  // poses behind this has a collision, so break.
     }
 
-    backed_poses.push_back(*backed_pose);
+    pull_out_start_pose.push_back(*backed_pose);
   }
-  return backed_poses;
+  return pull_out_start_pose;
 }
 
 bool PullOutModule::isOverlappedWithLane(

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

@@ -164,8 +164,6 @@ bool GeometricParallelParking::planPullOver(
   const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
   const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward)
 {
-  clearPaths();
-
   const auto & common_params = planner_data_->parameters;
   const double end_pose_offset = is_forward ? -parameters_.after_forward_parking_straight_distance
                                             : parameters_.after_backward_parking_straight_distance;
@@ -228,8 +226,6 @@ bool GeometricParallelParking::planPullOut(
   const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
   const lanelet::ConstLanelets & shoulder_lanes)
 {
-  clearPaths();
-
   constexpr bool is_forward = false;         // parking backward means departing forward
   constexpr double start_pose_offset = 0.0;  // start_pose is current_pose
   constexpr double max_offset = 10.0;
@@ -268,7 +264,7 @@ bool GeometricParallelParking::planPullOut(
     }
 
     // get road center line path from departing end to goal, and combine after the second arc path
-    const double s_start = getArcCoordinates(road_lanes, *end_pose).length + 1.0;  // need buffer?
+    const double s_start = getArcCoordinates(road_lanes, *end_pose).length;
     const double s_goal = getArcCoordinates(road_lanes, goal_pose).length;
     const double road_lanes_length = std::accumulate(
       road_lanes.begin(), road_lanes.end(), 0.0, [](const double sum, const auto & lane) {
@@ -279,6 +275,16 @@ bool GeometricParallelParking::planPullOut(
     PathWithLaneId road_center_line_path =
       planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true);
 
+    // check the continuity of straight path and arc path
+    const Pose & road_path_first_pose = road_center_line_path.points.front().point.pose;
+    const Pose & arc_path_last_pose = arc_paths.back().points.back().point.pose;
+    const double yaw_diff = tier4_autoware_utils::normalizeRadian(
+      tf2::getYaw(road_path_first_pose.orientation), tf2::getYaw(arc_path_last_pose.orientation));
+    const double distance = calcDistance2d(road_path_first_pose, arc_path_last_pose);
+    if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) {
+      continue;
+    }
+
     // set departing velocity to arc paths and 0 velocity to end point
     constexpr bool set_stop_end = false;
     setVelocityToArcPaths(arc_paths, parameters_.departing_velocity, set_stop_end);
@@ -287,7 +293,8 @@ bool GeometricParallelParking::planPullOut(
     // combine the road center line path with the second arc path
     auto paths = arc_paths;
     paths.back().points.insert(
-      paths.back().points.end(), road_center_line_path.points.begin(),
+      paths.back().points.end(),
+      road_center_line_path.points.begin() + 1,  // to avoid overlapped point
       road_center_line_path.points.end());
     removeOverlappingPoints(paths.back());
 
@@ -351,6 +358,8 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
   const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
   const bool is_forward, const double end_pose_offset, const double lane_departure_margin)
 {
+  clearPaths();
+
   const auto common_params = planner_data_->parameters;
 
   const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0);