diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 7c47caf2dc90d..342f2c8983cac 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/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 getCurrentPlanner() const; PathWithLaneId getFullPath() const; ParallelParkingParameters getGeometricPullOutParameters() const; - std::vector searchBackedPoses(); + std::vector searchPullOutStartPoses(); std::shared_ptr lane_departure_checker_; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 87e522bae875d..67822ccefb319 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/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 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 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 PullOutModule::searchBackedPoses() +std::vector PullOutModule::searchPullOutStartPoses() { + std::vector pull_out_start_pose{}; const Pose & current_pose = planner_data_->self_pose->pose; // get backward shoulder path @@ -597,9 +589,18 @@ std::vector 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 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 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( diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index 6ed412b74b2b2..6da97e1cb510a 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/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 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);