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

feat(behavior_path_planner): decrease pull_over hz #1819

Merged
merged 3 commits into from
Sep 9, 2022
Merged

feat(behavior_path_planner): decrease pull_over hz #1819

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8f54805
feat(behavior_path_planner): decrease pull_over hz
kosuke55 Sep 8, 2022
d0981be
generate drivable area at last in pull_out
kosuke55 Sep 8, 2022
802cedb
initialize struct to fix test
kosuke55 Sep 9, 2022
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
1 change: 1 addition & 0 deletions ...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 @@ -52,6 +52,7 @@ struct PullOutStatus
PathWithLaneId backward_path;
lanelet::ConstLanelets current_lanes;
lanelet::ConstLanelets pull_out_lanes;
lanelet::ConstLanelets lanes;
std::vector<uint64_t> lane_follow_lane_ids;
std::vector<uint64_t> pull_out_lane_ids;
bool is_safe = false;
Expand Down
27 changes: 17 additions & 10 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 @@ -168,6 +168,9 @@ BehaviorModuleOutput PullOutModule::plan()
} else {
path = status_.backward_path;
}
path.drivable_area = util::generateDrivableArea(
path, status_.lanes, planner_data_->parameters.drivable_area_resolution,
planner_data_->parameters.vehicle_length, planner_data_);

output.path = std::make_shared<PathWithLaneId>(path);
output.turn_signal_info =
Expand Down Expand Up @@ -235,9 +238,14 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval()
{
BehaviorModuleOutput output;
const auto current_lanes = getCurrentLanes();
const auto shoulder_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);
const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);
auto lanes = current_lanes;
lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end());

const auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path;
auto candidate_path = status_.back_finished ? getCurrentPath() : status_.backward_path;
candidate_path.drivable_area = util::generateDrivableArea(
candidate_path, lanes, planner_data_->parameters.drivable_area_resolution,
planner_data_->parameters.vehicle_length, planner_data_);
auto stop_path = candidate_path;
for (auto & p : stop_path.points) {
p.point.longitudinal_velocity_mps = 0.0;
Expand Down Expand Up @@ -354,7 +362,6 @@ void PullOutModule::planWithPriorityOnShortBackDistance(
void PullOutModule::updatePullOutStatus()
{
const auto & route_handler = planner_data_->route_handler;
const auto & common_parameters = planner_data_->parameters;
const auto & current_pose = planner_data_->self_pose->pose;
const auto & goal_pose = planner_data_->route_handler->getGoalPose();

Expand All @@ -365,6 +372,10 @@ void PullOutModule::updatePullOutStatus()
const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);
status_.pull_out_lanes = pull_out_lanes;

// combine road and shoulder lanes
status_.lanes = current_lanes;
status_.lanes.insert(status_.lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end());

// search pull out start candidates backward
std::vector<Pose> start_pose_candidates;
{
Expand Down Expand Up @@ -399,9 +410,6 @@ void PullOutModule::updatePullOutStatus()
status_.backward_path = pull_out_utils::getBackwardPath(
*route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose,
parameters_.backward_velocity);
status_.backward_path.drivable_area = util::generateDrivableArea(
status_.backward_path, pull_out_lanes, common_parameters.drivable_area_resolution,
common_parameters.vehicle_length, planner_data_);
}

// Update status
Expand Down Expand Up @@ -432,14 +440,13 @@ lanelet::ConstLanelets PullOutModule::getCurrentLanes() const
std::vector<Pose> PullOutModule::searchBackedPoses()
{
const auto current_pose = planner_data_->self_pose->pose;
const auto current_lanes = getCurrentLanes();
const auto pull_out_lanes = pull_out_utils::getPullOutLanes(current_lanes, planner_data_);

// get backward shoulder path
const auto arc_position_pose = lanelet::utils::getArcCoordinates(pull_out_lanes, current_pose);
const auto arc_position_pose =
lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose);
const double check_distance = parameters_.max_back_distance + 30.0; // buffer
auto backward_shoulder_path = planner_data_->route_handler->getCenterLinePath(
pull_out_lanes, arc_position_pose.length - check_distance,
status_.pull_out_lanes, arc_position_pose.length - check_distance,
arc_position_pose.length + check_distance);

// lateral shift to current_pose
Expand Down
11 changes: 6 additions & 5 deletions planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -624,9 +624,14 @@ BehaviorModuleOutput PullOverModule::plan()
-calcMinimumShiftPathDistance(), parameters_.deceleration_interval);
}
}
// generate drivable area
{
const auto p = planner_data_->parameters;
status_.path.drivable_area = util::generateDrivableArea(
status_.path, status_.lanes, p.drivable_area_resolution, p.vehicle_length, planner_data_);
}

BehaviorModuleOutput output;

// safe: use pull over path
if (status_.is_safe) {
output.path = std::make_shared<PathWithLaneId>(status_.path);
Expand Down Expand Up @@ -734,10 +739,6 @@ bool PullOverModule::planShiftPath()
return found_safe_path;
}

shift_parking_path_.path.drivable_area = util::generateDrivableArea(
shift_parking_path_.path, status_.lanes, common_parameters.drivable_area_resolution,
common_parameters.vehicle_length, planner_data_);

shift_parking_path_.path.header = planner_data_->route_handler->getRouteHeader();
return found_safe_path;
}
Expand Down
23 changes: 12 additions & 11 deletions planning/behavior_path_planner/src/scene_module/pull_over/util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,11 +99,21 @@ std::vector<ShiftParkingPath> getShiftParkingPaths(
const double jerk_resolution =
std::abs(maximum_lateral_jerk - minimum_lateral_jerk) / pull_over_sampling_num;

double distance_to_shoulder_lane_boundary =
const double distance_to_shoulder_lane_boundary =
util::getDistanceToShoulderBoundary(route_handler.getShoulderLanelets(), current_pose);
double offset_from_current_pose =
const double offset_from_current_pose =
distance_to_shoulder_lane_boundary + common_parameter.vehicle_width / 2 + margin;

// shift end point in shoulder lane
PathPointWithLaneId shift_end_point;
{
const auto arc_position_goal = lanelet::utils::getArcCoordinates(target_lanelets, goal_pose);
const double s_start = arc_position_goal.length - after_pull_over_straight_distance;
const double s_end = s_start + std::numeric_limits<double>::epsilon();
const auto path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end, true);
shift_end_point = path.points.front();
}

for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk;
lateral_jerk += jerk_resolution) {
PathShifter path_shifter;
Expand All @@ -124,15 +134,6 @@ std::vector<ShiftParkingPath> getShiftParkingPaths(
pull_over_distance - arc_position_pose.length;
}

// shift end point in shoulder lane
const auto shift_end_point = [&]() {
const auto arc_position_goal = lanelet::utils::getArcCoordinates(target_lanelets, goal_pose);
const double s_start = arc_position_goal.length - after_pull_over_straight_distance;
const double s_end = s_start + std::numeric_limits<double>::epsilon();
const auto path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end, true);
return path.points.front();
}();

PathWithLaneId road_lane_reference_path;
{
const auto arc_position = lanelet::utils::getArcCoordinates(original_lanelets, current_pose);
Expand Down
11 changes: 0 additions & 11 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 @@ -347,11 +347,6 @@ PathWithLaneId GeometricParallelParking::generateStraightPath(const Pose & start
auto path = planner_data_->route_handler->getCenterLinePath(
current_lanes, current_arc_position.length, start_arc_position.length, true);
path.header = planner_data_->route_handler->getRouteHeader();

path.drivable_area = util::generateDrivableArea(
path, current_lanes, planner_data_->parameters.drivable_area_resolution,
planner_data_->parameters.vehicle_length, planner_data_);

path.points.back().point.longitudinal_velocity_mps = 0;

return path;
Expand Down Expand Up @@ -429,9 +424,6 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
p.lane_ids.push_back(closest_shoulder_lanelet.id());
}
path_turn_left.header = planner_data_->route_handler->getRouteHeader();
path_turn_left.drivable_area = util::generateDrivableArea(
path_turn_left, lanes, common_params.drivable_area_resolution, common_params.vehicle_length,
planner_data_);

PathWithLaneId path_turn_right = generateArcPath(
Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, !is_forward, is_forward);
Expand All @@ -440,9 +432,6 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
p.lane_ids.push_back(closest_shoulder_lanelet.id());
}
path_turn_right.header = planner_data_->route_handler->getRouteHeader();
path_turn_right.drivable_area = util::generateDrivableArea(
path_turn_right, lanes, common_params.drivable_area_resolution, common_params.vehicle_length,
planner_data_);

// Need to add straight path to last right_turning for parking in parallel
if (std::abs(end_pose_offset) > 0) {
Expand Down