[ResourceManager] deactivate hardware from read/write return value (#884

)

* Deactivate hardware from read/write return value
* Added tests to DEACTIVATE return value
* Use constants for defining of special test-values for different HW behaviors

---------

Co-authored-by: Dr. Denis <[email protected]>
(cherry picked from commit bd6911d)

# Conflicts:
#	controller_manager/test/test_controller_manager_hardware_error_handling.cpp
#	hardware_interface/src/resource_manager.cpp
#	hardware_interface/test/test_components/test_actuator.cpp
#	hardware_interface/test/test_components/test_system.cpp
#	hardware_interface/test/test_resource_manager.cpp

hardware_interface/CMakeLists.txt

@@ -143,7 +143,9 @@ if(BUILD_TESTING)
     test/test_components/test_system.cpp)
   target_link_libraries(test_components ${PROJECT_NAME})
   ament_target_dependencies(test_components
-    pluginlib)
+    pluginlib
+    ros2_control_test_assets
+  )
   install(TARGETS test_components
     DESTINATION lib
   )

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,
+  DEACTIVATE = 2,
 };
 
 }  // namespace hardware_interface

hardware_interface/src/resource_manager.cpp

@@ -1089,6 +1089,7 @@ void ResourceManager::read(const rclcpp::Time & time, const rclcpp::Duration & p
 {
   for (auto & component : resource_storage_->actuators_)
   {
+<<<<<<< HEAD
     component.read(time, period);
   }
   for (auto & component : resource_storage_->sensors_)
@@ -1099,6 +1100,68 @@ void ResourceManager::read(const rclcpp::Time & time, const rclcpp::Duration & p
   {
     component.read(time, period);
   }
+=======
+    for (auto & component : components)
+    {
+      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::DEACTIVATE)
+      {
+        resource_storage_->deactivate_hardware(component);
+      }
+      // 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);
+      // }
+    }
+  };
+
+  read_components(resource_storage_->actuators_);
+  read_components(resource_storage_->sensors_);
+  read_components(resource_storage_->systems_);
+
+  return read_write_status;
+}
+
+// CM API: Called in "update"-thread
+HardwareReadWriteStatus ResourceManager::write(
+  const rclcpp::Time & time, const rclcpp::Duration & period)
+{
+  std::lock_guard<std::recursive_mutex> guard(resources_lock_);
+  read_write_status.ok = true;
+  read_write_status.failed_hardware_names.clear();
+
+  auto write_components = [&](auto & components)
+  {
+    for (auto & component : components)
+    {
+      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::DEACTIVATE)
+      {
+        resource_storage_->deactivate_hardware(component);
+      }
+    }
+  };
+
+  write_components(resource_storage_->actuators_);
+  write_components(resource_storage_->systems_);
+
+  return read_write_status;
+>>>>>>> bd6911d ([ResourceManager] deactivate hardware from read/write return value (#884))
 }
 
 void ResourceManager::write(const rclcpp::Time & time, const rclcpp::Duration & period)

hardware_interface/test/test_components/test_actuator.cpp

@@ -16,6 +16,7 @@
 #include <vector>
 
 #include "hardware_interface/actuator_interface.hpp"
+#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
 
 using hardware_interface::ActuatorInterface;
 using hardware_interface::CommandInterface;
@@ -75,6 +76,21 @@ class TestActuator : public ActuatorInterface
 
   return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
   {
+<<<<<<< HEAD
+=======
+    // simulate error on read
+    if (velocity_command_ == test_constants::READ_FAIL_VALUE)
+    {
+      // reset value to get out from error on the next call - simplifies CM tests
+      velocity_command_ = 0.0;
+      return return_type::ERROR;
+    }
+    // simulate deactivate on read
+    if (velocity_command_ == test_constants::READ_DEACTIVATE_VALUE)
+    {
+      return return_type::DEACTIVATE;
+    }
+>>>>>>> bd6911d ([ResourceManager] deactivate hardware from read/write return value (#884))
     // The next line is for the testing purposes. We need value to be changed to be sure that
     // the feedback from hardware to controllers in the chain is working as it should.
     // This makes value checks clearer and confirms there is no "state = command" line or some
@@ -85,6 +101,21 @@ class TestActuator : public ActuatorInterface
 
   return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
   {
+<<<<<<< HEAD
+=======
+    // simulate error on write
+    if (velocity_command_ == test_constants::WRITE_FAIL_VALUE)
+    {
+      // reset value to get out from error on the next call - simplifies CM tests
+      velocity_command_ = 0.0;
+      return return_type::ERROR;
+    }
+    // simulate deactivate on write
+    if (velocity_command_ == test_constants::WRITE_DEACTIVATE_VALUE)
+    {
+      return return_type::DEACTIVATE;
+    }
+>>>>>>> bd6911d ([ResourceManager] deactivate hardware from read/write return value (#884))
     return return_type::OK;
   }
 

hardware_interface/test/test_components/test_system.cpp

@@ -18,6 +18,7 @@
 
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
 
 using hardware_interface::CommandInterface;
 using hardware_interface::return_type;
@@ -80,11 +81,41 @@ class TestSystem : public SystemInterface
 
   return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
   {
+<<<<<<< HEAD
+=======
+    // simulate error on read
+    if (velocity_command_[0] == test_constants::READ_FAIL_VALUE)
+    {
+      // reset value to get out from error on the next call - simplifies CM tests
+      velocity_command_[0] = 0.0;
+      return return_type::ERROR;
+    }
+    // simulate deactivate on read
+    if (velocity_command_[0] == test_constants::READ_DEACTIVATE_VALUE)
+    {
+      return return_type::DEACTIVATE;
+    }
+>>>>>>> bd6911d ([ResourceManager] deactivate hardware from read/write return value (#884))
     return return_type::OK;
   }
 
   return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
   {
+<<<<<<< HEAD
+=======
+    // simulate error on write
+    if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE)
+    {
+      // reset value to get out from error on the next call - simplifies CM tests
+      velocity_command_[0] = 0.0;
+      return return_type::ERROR;
+    }
+    // simulate deactivate on write
+    if (velocity_command_[0] == test_constants::WRITE_DEACTIVATE_VALUE)
+    {
+      return return_type::DEACTIVATE;
+    }
+>>>>>>> bd6911d ([ResourceManager] deactivate hardware from read/write return value (#884))
     return return_type::OK;
   }