deactivate hardware from read/write return value

controller_manager/test/test_controller_manager_hardware_error_handling.cpp

@@ -22,6 +22,7 @@
 #include "controller_manager_test_common.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
+#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
 #include "test_controller/test_controller.hpp"
 
 using ::testing::_;
@@ -162,10 +163,6 @@ class TestControllerManagerWithTestableCM
       {}, strictness);
   }
 
-  // values to set to hardware to simulate failure on read and write
-  static constexpr double READ_FAIL_VALUE = 28282828.0;
-  static constexpr double WRITE_FAIL_VALUE = 23232323.0;
-
   static constexpr char TEST_CONTROLLER_ACTUATOR_NAME[] = "test_controller_actuator";
   static constexpr char TEST_CONTROLLER_SYSTEM_NAME[] = "test_controller_system";
   static constexpr char TEST_BROADCASTER_ALL_NAME[] = "test_broadcaster_all";
@@ -258,7 +255,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er
 
   // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to
   // READ_FAIL_VALUE
-  test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE;
+  test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE;
   {
     auto new_counter = test_controller_actuator->internal_counter + 1;
     EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD));
@@ -327,8 +324,8 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er
 
   // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME
   // by setting first command interface to READ_FAIL_VALUE
-  test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE;
-  test_controller_system->set_first_command_interface_value_to = READ_FAIL_VALUE;
+  test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE;
+  test_controller_system->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE;
   {
     auto previous_counter_lower = test_controller_actuator->internal_counter + 1;
     auto previous_counter_higher = test_controller_system->internal_counter + 1;
@@ -450,7 +447,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e
 
   // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to
   // WRITE_FAIL_VALUE
-  test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE;
+  test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE;
   {
     auto new_counter = test_controller_actuator->internal_counter + 1;
     EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD));
@@ -519,8 +516,8 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e
 
   // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME
   // by setting first command interface to WRITE_FAIL_VALUE
-  test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE;
-  test_controller_system->set_first_command_interface_value_to = WRITE_FAIL_VALUE;
+  test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE;
+  test_controller_system->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE;
   {
     auto previous_counter_lower = test_controller_actuator->internal_counter + 1;
     auto previous_counter_higher = test_controller_system->internal_counter + 1;

hardware_interface/CMakeLists.txt

@@ -101,7 +101,9 @@ if(BUILD_TESTING)
     test/test_components/test_system.cpp)
   target_link_libraries(test_components hardware_interface)
   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

@@ -1205,12 +1205,24 @@ 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::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);
+      // }
     }
   };
 
@@ -1233,12 +1245,17 @@ 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::DEACTIVATE)
+      {
+        resource_storage_->deactivate_hardware(component);
+      }
     }
   };
 

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;
@@ -76,12 +77,17 @@ class TestActuator : public ActuatorInterface
   return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
   {
     // simulate error on read
-    if (velocity_command_ == 28282828.0)
+    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;
+    }
     // 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
@@ -93,12 +99,17 @@ class TestActuator : public ActuatorInterface
   return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
   {
     // simulate error on write
-    if (velocity_command_ == 23232323.0)
+    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;
+    }
     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;
@@ -81,24 +82,34 @@ class TestSystem : public SystemInterface
   return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
   {
     // simulate error on read
-    if (velocity_command_[0] == 28282828)
+    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;
+    }
     return return_type::OK;
   }
 
   return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
   {
     // simulate error on write
-    if (velocity_command_[0] == 23232323)
+    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;
+    }
     return return_type::OK;
   }
 

hardware_interface/test/test_resource_manager.cpp

@@ -28,6 +28,7 @@
 #include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "ros2_control_test_assets/descriptions.hpp"
+#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
 
 using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES;
 using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME;
@@ -1538,6 +1539,122 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest
     }
   }
 
+  void check_read_or_write_deactivate(
+    FunctionT method_that_deactivates, FunctionT other_method, const double deactivate_value)
+  {
+    // define state to set components to
+    rclcpp_lifecycle::State state_active(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+      hardware_interface::lifecycle_state_names::ACTIVE);
+
+    // deactivate for TEST_ACTUATOR_HARDWARE_NAME
+    claimed_itfs[0].set_value(deactivate_value);
+    claimed_itfs[1].set_value(deactivate_value - 10.0);
+    {
+      // deactivate on error
+      auto [ok, failed_hardware_names] = method_that_deactivates(time, duration);
+      EXPECT_TRUE(ok);
+      EXPECT_TRUE(failed_hardware_names.empty());
+      auto status_map = rm->get_components_status();
+      EXPECT_EQ(
+        status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
+        lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+      EXPECT_EQ(
+        status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
+        lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+      check_if_interface_available(true, true);
+
+      // reactivate
+      rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active);
+      status_map = rm->get_components_status();
+      EXPECT_EQ(
+        status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
+        lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+      EXPECT_EQ(
+        status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
+        lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+      check_if_interface_available(true, true);
+    }
+    // write is sill OK
+    {
+      auto [ok, failed_hardware_names] = other_method(time, duration);
+      EXPECT_TRUE(ok);
+      EXPECT_TRUE(failed_hardware_names.empty());
+      check_if_interface_available(true, true);
+    }
+
+    // deactivate for TEST_SYSTEM_HARDWARE_NAME
+    claimed_itfs[0].set_value(deactivate_value - 10.0);
+    claimed_itfs[1].set_value(deactivate_value);
+    {
+      // deactivate on flag
+      auto [ok, failed_hardware_names] = method_that_deactivates(time, duration);
+      EXPECT_TRUE(ok);
+      EXPECT_TRUE(failed_hardware_names.empty());
+      auto status_map = rm->get_components_status();
+      EXPECT_EQ(
+        status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
+        lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+      EXPECT_EQ(
+        status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
+        lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+      check_if_interface_available(true, true);
+      // re-activate
+      rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active);
+      status_map = rm->get_components_status();
+      EXPECT_EQ(
+        status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
+        lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+      EXPECT_EQ(
+        status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
+        lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+      check_if_interface_available(true, true);
+    }
+    // write is sill OK
+    {
+      auto [ok, failed_hardware_names] = other_method(time, duration);
+      EXPECT_TRUE(ok);
+      EXPECT_TRUE(failed_hardware_names.empty());
+      check_if_interface_available(true, true);
+    }
+
+    // deactivate both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME
+    claimed_itfs[0].set_value(deactivate_value);
+    claimed_itfs[1].set_value(deactivate_value);
+    {
+      // deactivate on flag
+      auto [ok, failed_hardware_names] = method_that_deactivates(time, duration);
+      EXPECT_TRUE(ok);
+      EXPECT_TRUE(failed_hardware_names.empty());
+      auto status_map = rm->get_components_status();
+      EXPECT_EQ(
+        status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
+        lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+      EXPECT_EQ(
+        status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
+        lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+      check_if_interface_available(true, true);
+      // re-activate
+      rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active);
+      rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active);
+      status_map = rm->get_components_status();
+      EXPECT_EQ(
+        status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(),
+        lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+      EXPECT_EQ(
+        status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(),
+        lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
+      check_if_interface_available(true, true);
+    }
+    // write is sill OK
+    {
+      auto [ok, failed_hardware_names] = other_method(time, duration);
+      EXPECT_TRUE(ok);
+      EXPECT_TRUE(failed_hardware_names.empty());
+      check_if_interface_available(true, true);
+    }
+  }
+
 public:
   std::shared_ptr<TestableResourceManager> rm;
   std::vector<hardware_interface::LoanedCommandInterface> claimed_itfs;
@@ -1546,8 +1663,6 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest
   const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01);
 
   // values to set to hardware to simulate failure on read and write
-  static constexpr double READ_FAIL_VALUE = 28282828.0;
-  static constexpr double WRITE_FAIL_VALUE = 23232323.0;
 };
 
 TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read)
@@ -1558,7 +1673,7 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read)
   // check read methods failures
   check_read_or_write_failure(
     std::bind(&TestableResourceManager::read, rm, _1, _2),
-    std::bind(&TestableResourceManager::write, rm, _1, _2), READ_FAIL_VALUE);
+    std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_FAIL_VALUE);
 }
 
 TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write)
@@ -1569,7 +1684,29 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write)
   // check write methods failures
   check_read_or_write_failure(
     std::bind(&TestableResourceManager::write, rm, _1, _2),
-    std::bind(&TestableResourceManager::read, rm, _1, _2), WRITE_FAIL_VALUE);
+    std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_FAIL_VALUE);
+}
+
+TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_read)
+{
+  setup_resource_manager_and_do_initial_checks();
+
+  using namespace std::placeholders;
+  // check read methods failures
+  check_read_or_write_deactivate(
+    std::bind(&TestableResourceManager::read, rm, _1, _2),
+    std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_DEACTIVATE_VALUE);
+}
+
+TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_write)
+{
+  setup_resource_manager_and_do_initial_checks();
+
+  using namespace std::placeholders;
+  // check write methods failures
+  check_read_or_write_deactivate(
+    std::bind(&TestableResourceManager::write, rm, _1, _2),
+    std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_DEACTIVATE_VALUE);
 }
 
 TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware)