From 90688da9837c082c76afc7fb9f19ea4a1cb46aa1 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 16 May 2023 12:00:19 +0900 Subject: [PATCH] fix(avoidance): clear waiting approval only when all target module are unavoidable (#3662) (#482) Signed-off-by: satoshi-ota Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../scene_module/avoidance/avoidance_module.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 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 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()