From 8793c27c30f00998d75fd4d530ac071a88eb7983 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Thu, 20 Apr 2023 21:21:25 +0900 Subject: [PATCH 1/2] feat(behavior_path_planner): run avoidance and pull out simultaneously Signed-off-by: kosuke55 --- .../avoidance/avoidance_module.cpp | 13 ++++++++++- .../scene_module/pull_out/pull_out_module.cpp | 23 ++++++++++++++++--- 2 files changed, 32 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 23628721590e8..5c4eaa70a945d 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -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; } @@ -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 current_drivable_lanes_vec{}; for (const auto & current_lane : current_lanes) { DrivableLanes current_drivable_lanes; current_drivable_lanes.left_lane = current_lane; @@ -2511,8 +2520,10 @@ 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); } + output.drivable_lanes = utils::combineDrivableLanes( + getPreviousModuleOutput().drivable_lanes, current_drivable_lanes_vec); { const auto & p = planner_data_->parameters; 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 c56059eb1258a..9b3c997e2e81b 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 @@ -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(stop_path); + output.drivable_lanes = status_.lanes; output.reference_path = getPreviousModuleOutput().reference_path; path_candidate_ = std::make_shared(stop_path); path_reference_ = getPreviousModuleOutput().reference_path; @@ -203,6 +204,7 @@ BehaviorModuleOutput PullOutModule::plan() path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_); output.path = std::make_shared(path); + output.drivable_lanes = status_.lanes; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); @@ -321,6 +323,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() } output.path = std::make_shared(stop_path); + output.drivable_lanes = status_.lanes; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); @@ -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; } From c49d0d2d919a9db42306a64d94caecf3384c1725 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 24 Apr 2023 23:34:51 +0900 Subject: [PATCH 2/2] add ifdef for dirvable lanes Signed-off-by: kosuke55 --- .../src/scene_module/avoidance/avoidance_module.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 5c4eaa70a945d..ab6ce703a3720 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2522,8 +2522,13 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output 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;