deactivate hardware from read/write return value

controller_manager/test/test_controller_manager_hardware_error_handling.cpp

@@ -21,6 +21,7 @@
 #include "controller_manager/controller_manager.hpp"
 #include "controller_manager_test_common.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
+#include "hardware_interface/types/test_hardware_interface_constants.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "test_controller/test_controller.hpp"
 
@@ -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,8 @@ 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 =
+    hardware_interface::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 +325,10 @@ 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 =
+    hardware_interface::test_constants::READ_FAIL_VALUE;
+  test_controller_system->set_first_command_interface_value_to =
+    hardware_interface::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 +450,8 @@ 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 =
+    hardware_interface::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 +520,10 @@ 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 =
+    hardware_interface::test_constants::WRITE_FAIL_VALUE;
+  test_controller_system->set_first_command_interface_value_to =
+    hardware_interface::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/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

hardware_interface/include/hardware_interface/types/test_hardware_interface_constants.hpp

@@ -0,0 +1,31 @@
+// Copyright (c) 2023, FZI Forschungszentrum Informatik
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+/// \author: Felix Exner <[email protected]>
+
+#ifndef HARDWARE_INTERFACE__TYPES__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_
+#define HARDWARE_INTERFACE__TYPES__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_
+namespace hardware_interface
+{
+namespace test_constants
+{
+/// Constants defining special values used inside tests to trigger things like deactivate or errors
+/// on read/write.
+constexpr double READ_FAIL_VALUE = 28282828.0;
+constexpr double WRITE_FAIL_VALUE = 23232323.0;
+constexpr double READ_DEACTIVATE_VALUE = 29292929.0;
+constexpr double WRITE_DEACTIVATE_VALUE = 24242424.0;
+}  // namespace test_constants
+}  // namespace hardware_interface
+#endif  // HARDWARE_INTERFACE__TYPES__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_

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);
+      }
     }
   };
 

hardware_interface/test/test_components/test_actuator.cpp

@@ -16,6 +16,7 @@
 #include <vector>
 
 #include "hardware_interface/actuator_interface.hpp"
+#include "hardware_interface/types/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_ == hardware_interface::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_ == hardware_interface::test_constants::READ_DEACTIVATE_VALUE)
+    {
+      return return_type::DEACTIVATED;
+    }
     // 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_ == hardware_interface::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_ == hardware_interface::test_constants::WRITE_DEACTIVATE_VALUE)
+    {
+      return return_type::DEACTIVATED;
+    }
     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 "hardware_interface/types/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] == hardware_interface::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] == hardware_interface::test_constants::READ_DEACTIVATE_VALUE)
+    {
+      return return_type::DEACTIVATED;
+    }
     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] == hardware_interface::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] == hardware_interface::test_constants::WRITE_DEACTIVATE_VALUE)
+    {
+      return return_type::DEACTIVATED;
+    }
     return return_type::OK;
   }