feat(rtc_inteface): add function to check force deactivate (autowaref…

…oundation#8221) add function to check for deactivation Signed-off-by: Go Sakayori <[email protected]>
go-sakayori · Jul 29, 2024 · 687aa93 · 687aa93
1 parent 936703d
commit 687aa93
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 0 deletions.
diff --git a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp
@@ -60,6 +60,7 @@ class RTCInterface
   void clearCooperateStatus();
   bool isActivated(const UUID & uuid) const;
   bool isForceActivated(const UUID & uuid) const;
+  bool isForceDeactivated(const UUID & UUID) const;
   bool isRegistered(const UUID & uuid) const;
   bool isRTCEnabled(const UUID & uuid) const;
   bool isTerminated(const UUID & uuid) const;

diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp
@@ -370,6 +370,30 @@ bool RTCInterface::isForceActivated(const UUID & uuid) const
   return false;
 }
 
+bool RTCInterface::isForceDeactivated(const UUID & uuid) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  const auto itr = std::find_if(
+    registered_status_.statuses.begin(), registered_status_.statuses.end(),
+    [uuid](const auto & s) { return s.uuid == uuid; });
+
+  if (itr != registered_status_.statuses.end()) {
+    if (itr->state.type != State::RUNNING) {
+      return false;
+    }
+    if (itr->command_status.type == Command::DEACTIVATE && itr->safe && !itr->auto_mode) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  RCLCPP_WARN_STREAM(
+    getLogger(),
+    "[isForceDeactivated] uuid : " << uuid_to_string(uuid) << " is not found" << std::endl);
+  return false;
+}
+
 bool RTCInterface::isRegistered(const UUID & uuid) const
 {
   std::lock_guard<std::mutex> lock(mutex_);