From eb782af48a42e9e27cade063157f0862d5a23702 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 8 Feb 2023 02:26:13 +0900 Subject: [PATCH] fix(behavior_path_planner): fix path distortion of no back pull out (#2829) * fix(behavior_path_planner): fix path distortion of no back pull out Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp --------- Signed-off-by: kosuke55 --- .../scene_module/pull_out/pull_out_module.hpp | 2 +- .../scene_module/pull_out/pull_out_module.cpp | 30 ++++++++++--------- .../utils/geometric_parallel_parking.cpp | 21 +++++++++---- 3 files changed, 32 insertions(+), 21 deletions(-) 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 ac8241d691675..2daa1ecf153d6 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 @@ -534,16 +534,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); @@ -580,8 +571,10 @@ 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_odometry->pose.pose; // get backward shoulder path @@ -598,9 +591,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( @@ -630,9 +632,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 a2e0568379854..4a681445594f4 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 @@ -163,8 +163,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; @@ -227,8 +225,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; @@ -267,7 +263,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) { @@ -278,6 +274,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); @@ -286,7 +292,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()); @@ -350,6 +357,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);