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 0f8b439c5e64d..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,6 +369,18 @@ 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();