Added tests for DEACTIVATE return value

hardware_interface/include/hardware_interface/types/test_hardware_interface_constants.hpp

@@ -24,6 +24,8 @@ namespace test_constants
 /// 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/test/test_components/test_actuator.cpp

@@ -83,6 +83,11 @@ class TestActuator : public ActuatorInterface
       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
@@ -100,6 +105,11 @@ class TestActuator : public ActuatorInterface
       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

@@ -88,6 +88,11 @@ class TestSystem : public SystemInterface
       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;
   }
 
@@ -100,6 +105,11 @@ class TestSystem : public SystemInterface
       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;
   }
 

hardware_interface/test/test_resource_manager.cpp

@@ -1539,6 +1539,116 @@ 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);
+    {
+      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);
+
+      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);
+    {
+      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);
+      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);
+    {
+      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);
+      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;
@@ -1573,6 +1683,18 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write)
     hardware_interface::test_constants::WRITE_FAIL_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),
+    hardware_interface::test_constants::WRITE_DEACTIVATE_VALUE);
+}
+
 TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware)
 {
   TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false);