From 555a74473dee5ea0bbc617fed137f1963509b19d Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Tue, 14 May 2024 18:49:29 +0900
Subject: [PATCH] fix(avoidance): transit to FAILED from RUNNING when the
 module reverts avoidance path

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../behavior_path_avoidance_module/scene.hpp  | 26 ++++++++++++++++---
 .../src/scene.cpp                             |  1 -
 2 files changed, 22 insertions(+), 5 deletions(-)

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 <rclcpp/node.hpp>
 #include <rclcpp/time.hpp>
 
+#include <limits>
 #include <memory>
 #include <string>
 #include <unordered_map>
@@ -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<double>::lowest(),
+        std::numeric_limits<double>::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<double>::lowest(),
+        std::numeric_limits<double>::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<double>::lowest(),
+        std::numeric_limits<double>::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<double>::lowest(),
+        std::numeric_limits<double>::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()