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 aa64e2741e52d..53b18545c3c79 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 @@ -2877,24 +2877,35 @@ CandidateOutput AvoidanceModule::planCandidate() const BehaviorModuleOutput AvoidanceModule::planWaitingApproval() { + const auto & data = avoidance_data_; + // we can execute the plan() since it handles the approval appropriately. BehaviorModuleOutput out = plan(); + #ifndef USE_OLD_ARCHITECTURE if (path_shifter_.getShiftLines().empty()) { out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } #endif + + const auto all_unavoidable = std::all_of( + data.target_objects.begin(), data.target_objects.end(), + [](const auto & o) { return !o.is_avoidable; }); + const auto candidate = planCandidate(); - constexpr double threshold_to_update_status = -1.0e-03; - if (candidate.start_distance_to_path_change > threshold_to_update_status) { + if (!avoidance_data_.safe_new_sl.empty()) { updateCandidateRTCStatus(candidate); waitApproval(); + } else if (all_unavoidable) { + waitApproval(); } else { clearWaitingApproval(); removeCandidateRTCStatus(); } + path_candidate_ = std::make_shared(candidate.path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; + return out; } @@ -3142,6 +3153,7 @@ void AvoidanceModule::updateData() void AvoidanceModule::processOnEntry() { initVariables(); + waitApproval(); } void AvoidanceModule::processOnExit()