From d149da6f35f1c80eca150045f8af6c2b52f80ce6 Mon Sep 17 00:00:00 2001
From: Felix Exner <exner@fzi.de>
Date: Tue, 20 Dec 2022 09:01:42 +0000
Subject: [PATCH] Deactivate hardware from read/write return value

This is the first step to deactivate a hw component when writing to that
component stops working (e.g. because of some interpreter program stopped
running).

The idea is that a hw component can go back to "configured", where the
communication is working, but commanding the robot is not possible.
---
 .../hardware_interface_return_values.hpp      |  1 +
 hardware_interface/src/resource_manager.cpp   | 27 +++++++++++++++++--
 2 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp
index 5c3ea22ca0..fdb9ebdc1b 100644
--- a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp
+++ b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp
@@ -23,6 +23,7 @@ enum class return_type : std::uint8_t
 {
   OK = 0,
   ERROR = 1,
+  DEACTIVATED = 2,
 };
 
 }  // namespace hardware_interface
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
index 96c87f5806..831a31d334 100644
--- a/hardware_interface/src/resource_manager.cpp
+++ b/hardware_interface/src/resource_manager.cpp
@@ -1205,12 +1205,27 @@ HardwareReadWriteStatus ResourceManager::read(
   {
     for (auto & component : components)
     {
-      if (component.read(time, period) != return_type::OK)
+      auto ret_val = component.read(time, period);
+      if (ret_val == return_type::ERROR)
       {
         read_write_status.ok = false;
         read_write_status.failed_hardware_names.push_back(component.get_name());
         resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name());
       }
+      else if (ret_val == return_type::DEACTIVATED)
+      {
+        using lifecycle_msgs::msg::State;
+        rclcpp_lifecycle::State state(
+          State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE);
+        set_component_state(component.get_name(), state);
+      }
+      // If desired: automatic re-activation. We could add a flag for this...
+      // else
+      // {
+      // using lifecycle_msgs::msg::State;
+      // rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE);
+      // set_component_state(component.get_name(), state);
+      // }
     }
   };
 
@@ -1233,12 +1248,20 @@ HardwareReadWriteStatus ResourceManager::write(
   {
     for (auto & component : components)
     {
-      if (component.write(time, period) != return_type::OK)
+      auto ret_val = component.write(time, period);
+      if (ret_val == return_type::ERROR)
       {
         read_write_status.ok = false;
         read_write_status.failed_hardware_names.push_back(component.get_name());
         resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name());
       }
+      else if (ret_val == return_type::DEACTIVATED)
+      {
+        using lifecycle_msgs::msg::State;
+        rclcpp_lifecycle::State state(
+          State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE);
+        set_component_state(component.get_name(), state);
+      }
     }
   };