Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(behavior_path_planner): fix path distortion of no back pull out (#2829) #273

Merged
merged 1 commit into from
Feb 8, 2023
Merged

fix(behavior_path_planner): fix path distortion of no back pull out (#2829) #273

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ...vior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_;

Expand Down
29 changes: 15 additions & 14 deletions planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand All @@ -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(
Expand Down Expand Up @@ -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(
Expand Down
21 changes: 15 additions & 6 deletions planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand All @@ -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);
Expand All @@ -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());

Expand Down Expand Up @@ -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);
Expand Down