diff --git a/hardware_interface/include/hardware_interface/types/test_hardware_interface_constants.hpp b/hardware_interface/include/hardware_interface/types/test_hardware_interface_constants.hpp index c62c32e765..bdd59b7c7d 100644 --- a/hardware_interface/include/hardware_interface/types/test_hardware_interface_constants.hpp +++ b/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_ diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 946a77e3ed..cbe7c3c782 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/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; } diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 0ca15353a1..364e2d60d5 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/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; } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index f0209a7b35..6b11e6d694 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/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 rm; std::vector 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);