Revert "fix(avoidance): transit rtc cooperate state to FAILED from RU…

…NNING (autowarefoundation#6998)" This reverts commit affc2e7.
tier4 · Jun 11, 2024 · 049e9dc · 049e9dc
1 parent 016adba
commit 049e9dc
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 22 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
@@ -26,7 +26,6 @@
 #include <rclcpp/node.hpp>
 #include <rclcpp/time.hpp>
 
-#include <limits>
 #include <memory>
 #include <string>
 #include <unordered_map>
@@ -139,15 +138,9 @@ class AvoidanceModule : public SceneModuleInterface
   void removeCandidateRTCStatus()
   {
     if (rtc_interface_ptr_map_.at("left")->isRegistered(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());
+      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_);
     }
   }
 
@@ -369,21 +362,10 @@ 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
@@ -1283,6 +1283,7 @@ void AvoidanceModule::updateData()
 void AvoidanceModule::processOnEntry()
 {
   initVariables();
+  removeRTCStatus();
 }
 
 void AvoidanceModule::processOnExit()