diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 6b11d490e8c23..194e5ecdaf86c 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -26,6 +26,7 @@ #include #include +#include #include #include #include @@ -138,9 +139,15 @@ class AvoidanceModule : public SceneModuleInterface void removeCandidateRTCStatus() { if (rtc_interface_ptr_map_.at("left")->isRegistered(candidate_uuid_)) { - rtc_interface_ptr_map_.at("left")->removeCooperateStatus(candidate_uuid_); - } else if (rtc_interface_ptr_map_.at("right")->isRegistered(candidate_uuid_)) { - rtc_interface_ptr_map_.at("right")->removeCooperateStatus(candidate_uuid_); + rtc_interface_ptr_map_.at("left")->updateCooperateStatus( + candidate_uuid_, true, State::FAILED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } + + if (rtc_interface_ptr_map_.at("right")->isRegistered(candidate_uuid_)) { + rtc_interface_ptr_map_.at("right")->updateCooperateStatus( + candidate_uuid_, true, State::FAILED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); } } @@ -362,10 +369,21 @@ class AvoidanceModule : public SceneModuleInterface unlockNewModuleLaunch(); + for (const auto & left_shift : left_shift_array_) { + rtc_interface_ptr_map_.at("left")->updateCooperateStatus( + left_shift.uuid, true, State::FAILED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } + + for (const auto & right_shift : right_shift_array_) { + rtc_interface_ptr_map_.at("right")->updateCooperateStatus( + right_shift.uuid, true, State::FAILED, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + } + if (!path_shifter_.getShiftLines().empty()) { left_shift_array_.clear(); right_shift_array_.clear(); - removeRTCStatus(); } generator_.reset(); diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 6858b3809a73b..51fe19e4b3237 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1283,7 +1283,6 @@ void AvoidanceModule::updateData() void AvoidanceModule::processOnEntry() { initVariables(); - removeRTCStatus(); } void AvoidanceModule::processOnExit()