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): run avoidance and pull out simultaneously #3481

Merged
merged 2 commits into from
Apr 25, 2023
Merged

feat(behavior_path_planner): run avoidance and pull out simultaneously #3481

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8793c27
feat(behavior_path_planner): run avoidance and pull out simultaneously
kosuke55 Apr 20, 2023
c49d0d2
add ifdef for dirvable lanes
kosuke55 Apr 24, 2023
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
18 changes: 17 additions & 1 deletion planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,14 @@ bool AvoidanceModule::isExecutionRequested() const
{
DEBUG_PRINT("AVOIDANCE isExecutionRequested");

#ifndef USE_OLD_ARCHITECTURE
const auto is_driving_forward =
motion_utils::isDrivingForward(getPreviousModuleOutput().path->points);
if (!is_driving_forward || !(*is_driving_forward)) {
return false;
}
#endif

if (current_state_ == ModuleStatus::RUNNING) {
return true;
}
Expand Down Expand Up @@ -2384,6 +2392,7 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output
const auto & current_lanes = avoidance_data_.current_lanelets;
const auto & enable_opposite = parameters_->enable_avoidance_over_opposite_direction;

std::vector<DrivableLanes> current_drivable_lanes_vec{};
for (const auto & current_lane : current_lanes) {
DrivableLanes current_drivable_lanes;
current_drivable_lanes.left_lane = current_lane;
Expand Down Expand Up @@ -2511,9 +2520,16 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output
current_drivable_lanes.middle_lanes.push_back(current_lane);
}

output.drivable_lanes.push_back(current_drivable_lanes);
current_drivable_lanes_vec.push_back(current_drivable_lanes);
}

#ifdef USE_OLD_ARCHITECTURE
output.drivable_lanes = current_drivable_lanes_vec;
#else
output.drivable_lanes = utils::combineDrivableLanes(
getPreviousModuleOutput().drivable_lanes, current_drivable_lanes_vec);
#endif

{
const auto & p = planner_data_->parameters;
generateDrivableArea(
Expand Down
23 changes: 20 additions & 3 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 @@ -181,6 +181,7 @@ BehaviorModuleOutput PullOutModule::plan()
// the path of getCurrent() is generated by generateStopPath()
const PathWithLaneId stop_path = getCurrentPath();
output.path = std::make_shared<PathWithLaneId>(stop_path);
output.drivable_lanes = status_.lanes;
output.reference_path = getPreviousModuleOutput().reference_path;
path_candidate_ = std::make_shared<PathWithLaneId>(stop_path);
path_reference_ = getPreviousModuleOutput().reference_path;
Expand All @@ -203,6 +204,7 @@ BehaviorModuleOutput PullOutModule::plan()
path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_);

output.path = std::make_shared<PathWithLaneId>(path);
output.drivable_lanes = status_.lanes;
output.reference_path = getPreviousModuleOutput().reference_path;
output.turn_signal_info = calcTurnSignalInfo();
path_candidate_ = std::make_shared<PathWithLaneId>(getFullPath());
Expand Down Expand Up @@ -321,6 +323,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval()
}

output.path = std::make_shared<PathWithLaneId>(stop_path);
output.drivable_lanes = status_.lanes;
output.reference_path = getPreviousModuleOutput().reference_path;
output.turn_signal_info = calcTurnSignalInfo();
path_candidate_ = std::make_shared<PathWithLaneId>(getFullPath());
Expand Down Expand Up @@ -687,14 +690,28 @@ bool PullOutModule::hasFinishedPullOut() const
return false;
}

// check ego car is close enough to goal pose
const auto current_pose = planner_data_->self_odometry->pose.pose;

// keep running until returning to the path, considering that other modules (e.g avoidance)
// are also running at the same time.
const double lateral_offset_to_path =
motion_utils::calcLateralOffset(getCurrentPath().points, current_pose.position);
constexpr double lateral_offset_threshold = 0.5;
if (std::abs(lateral_offset_to_path) > lateral_offset_threshold) {
return false;
}
const double yaw_deviation =
motion_utils::calcYawDeviation(getCurrentPath().points, current_pose);
constexpr double yaw_deviation_threshold = 0.5;
if (std::abs(yaw_deviation) > yaw_deviation_threshold) {
return false;
}

// check that ego has passed pull out end point
const auto arclength_current =
lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose);
const auto arclength_pull_out_end =
lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.end_pose);

// has passed pull out end point
return arclength_current.length - arclength_pull_out_end.length > 0.0;
}

Expand Down