diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index bff6f7f23e..67ddaf4ca0 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -112,12 +112,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ std::vector available_state_interfaces() const; - /// Checks whether a state interface is registered under the given key. - /** - * \return true if interface exist, false otherwise. - */ - bool state_interface_exists(const std::string & key) const; - /// Checks whether a state interface is available under the given key. /** * \return true if interface is available, false otherwise. @@ -212,13 +206,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ std::vector available_command_interfaces() const; - /// Checks whether a command interface is registered under the given key. - /** - * \param[in] key string identifying the interface to check. - * \return true if interface exist, false otherwise. - */ - bool command_interface_exists(const std::string & key) const; - /// Checks whether a command interface is available under the given name. /** * \param[in] name string identifying the interface to check. @@ -374,6 +361,19 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ void activate_all_components(); + /// Checks whether a command interface is registered under the given key. + /** + * \param[in] key string identifying the interface to check. + * \return true if interface exist, false otherwise. + */ + bool command_interface_exists(const std::string & key) const; + + /// Checks whether a state interface is registered under the given key. + /** + * \return true if interface exist, false otherwise. + */ + bool state_interface_exists(const std::string & key) const; + private: void validate_storage(const std::vector & hardware_info) const; @@ -383,6 +383,11 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager mutable std::recursive_mutex resource_interfaces_lock_; mutable std::recursive_mutex claimed_command_interfaces_lock_; +<<<<<<< HEAD +======= + mutable std::recursive_mutex resources_lock_; + +>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816)) std::unique_ptr resource_storage_; bool is_urdf_loaded__ = false; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index da3176e953..cd24694c83 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -665,6 +665,7 @@ ResourceManager::ResourceManager( } } +// CM API: Called in "callback/slow"-thread void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { is_urdf_loaded__ = true; @@ -701,8 +702,11 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac } } +<<<<<<< HEAD bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } +======= +>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816)) // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) { @@ -715,6 +719,7 @@ LoanedStateInterface ResourceManager::claim_state_interface(const std::string & return LoanedStateInterface(resource_storage_->state_interface_map_.at(key)); } +// CM API: Called in "callback/slow"-thread std::vector ResourceManager::state_interface_keys() const { std::vector keys; @@ -726,19 +731,14 @@ std::vector ResourceManager::state_interface_keys() const return keys; } +// CM API: Called in "update"-thread std::vector ResourceManager::available_state_interfaces() const { std::lock_guard guard(resource_interfaces_lock_); return resource_storage_->available_state_interfaces_; } -bool ResourceManager::state_interface_exists(const std::string & key) const -{ - std::lock_guard guard(resource_interfaces_lock_); - return resource_storage_->state_interface_map_.find(key) != - resource_storage_->state_interface_map_.end(); -} - +// CM API: Called in "update"-thread (indirectly through `claim_state_interface`) bool ResourceManager::state_interface_is_available(const std::string & name) const { std::lock_guard guard(resource_interfaces_lock_); @@ -748,6 +748,7 @@ bool ResourceManager::state_interface_is_available(const std::string & name) con name) != resource_storage_->available_state_interfaces_.end(); } +// CM API: Called in "callback/slow"-thread void ResourceManager::import_controller_reference_interfaces( const std::string & controller_name, std::vector & interfaces) { @@ -757,12 +758,14 @@ void ResourceManager::import_controller_reference_interfaces( resource_storage_->controllers_reference_interfaces_map_[controller_name] = interface_names; } +// CM API: Called in "callback/slow"-thread std::vector ResourceManager::get_controller_reference_interface_names( const std::string & controller_name) { return resource_storage_->controllers_reference_interfaces_map_.at(controller_name); } +// CM API: Called in "update"-thread void ResourceManager::make_controller_reference_interfaces_available( const std::string & controller_name) { @@ -774,6 +777,7 @@ void ResourceManager::make_controller_reference_interfaces_available( interface_names.end()); } +// CM API: Called in "update"-thread void ResourceManager::make_controller_reference_interfaces_unavailable( const std::string & controller_name) { @@ -796,6 +800,7 @@ void ResourceManager::make_controller_reference_interfaces_unavailable( } } +// CM API: Called in "callback/slow"-thread void ResourceManager::remove_controller_reference_interfaces(const std::string & controller_name) { auto interface_names = @@ -807,6 +812,56 @@ void ResourceManager::remove_controller_reference_interfaces(const std::string & resource_storage_->remove_command_interfaces(interface_names); } +<<<<<<< HEAD +======= +// CM API: Called in "callback/slow"-thread +void ResourceManager::cache_controller_to_hardware( + const std::string & controller_name, const std::vector & interfaces) +{ + for (const auto & interface : interfaces) + { + bool found = false; + for (const auto & [hw_name, hw_info] : resource_storage_->hardware_info_map_) + { + auto cmd_itf_it = + std::find(hw_info.command_interfaces.begin(), hw_info.command_interfaces.end(), interface); + if (cmd_itf_it != hw_info.command_interfaces.end()) + { + found = true; + } + auto state_itf_it = + std::find(hw_info.state_interfaces.begin(), hw_info.state_interfaces.end(), interface); + if (state_itf_it != hw_info.state_interfaces.end()) + { + found = true; + } + + if (found) + { + // check if controller exist already in the list and if not add it + auto controllers = resource_storage_->hardware_used_by_controllers_[hw_name]; + auto ctrl_it = std::find(controllers.begin(), controllers.end(), controller_name); + if (ctrl_it == controllers.end()) + { + // add because it does not exist + controllers.reserve(controllers.size() + 1); + controllers.push_back(controller_name); + } + resource_storage_->hardware_used_by_controllers_[hw_name] = controllers; + break; + } + } + } +} + +// CM API: Called in "update"-thread +std::vector ResourceManager::get_cached_controllers_to_hardware( + const std::string & hardware_name) +{ + return resource_storage_->hardware_used_by_controllers_[hardware_name]; +} + +>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816)) // CM API: Called in "update"-thread bool ResourceManager::command_interface_is_claimed(const std::string & key) const { @@ -848,6 +903,7 @@ void ResourceManager::release_command_interface(const std::string & key) resource_storage_->claimed_command_interface_map_[key] = false; } +// CM API: Called in "callback/slow"-thread std::vector ResourceManager::command_interface_keys() const { std::vector keys; @@ -859,20 +915,14 @@ std::vector ResourceManager::command_interface_keys() const return keys; } +// CM API: Called in "update"-thread std::vector ResourceManager::available_command_interfaces() const { std::lock_guard guard(resource_interfaces_lock_); return resource_storage_->available_command_interfaces_; } -bool ResourceManager::command_interface_exists(const std::string & key) const -{ - std::lock_guard guard(resource_interfaces_lock_); - return resource_storage_->command_interface_map_.find(key) != - resource_storage_->command_interface_map_.end(); -} - -// CM API +// CM API: Called in "callback/slow"-thread bool ResourceManager::command_interface_is_available(const std::string & name) const { std::lock_guard guard(resource_interfaces_lock_); @@ -882,16 +932,6 @@ bool ResourceManager::command_interface_is_available(const std::string & name) c name) != resource_storage_->available_command_interfaces_.end(); } -size_t ResourceManager::actuator_components_size() const -{ - return resource_storage_->actuators_.size(); -} - -size_t ResourceManager::sensor_components_size() const -{ - return resource_storage_->sensors_.size(); -} - void ResourceManager::import_component( std::unique_ptr actuator, const HardwareInfo & hardware_info) { @@ -910,12 +950,7 @@ void ResourceManager::import_component( resource_storage_->initialize_system(std::move(system), hardware_info); } -size_t ResourceManager::system_components_size() const -{ - return resource_storage_->systems_.size(); -} -// End of "used only in tests" - +// CM API: Called in "callback/slow"-thread std::unordered_map ResourceManager::get_components_status() { for (auto & component : resource_storage_->actuators_) @@ -934,6 +969,7 @@ std::unordered_map ResourceManager::get_comp return resource_storage_->hardware_info_map_; } +// CM API: Called in "callback/slow"-thread bool ResourceManager::prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) @@ -979,6 +1015,7 @@ bool ResourceManager::prepare_command_mode_switch( return true; } +// CM API: Called in "update"-thread bool ResourceManager::perform_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) @@ -1006,6 +1043,7 @@ bool ResourceManager::perform_command_mode_switch( return true; } +// CM API: Called in "callback/slow"-thread return_type ResourceManager::set_component_state( const std::string & component_name, rclcpp_lifecycle::State & target_state) { @@ -1089,7 +1127,13 @@ return_type ResourceManager::set_component_state( return result; } +<<<<<<< HEAD void ResourceManager::read(const rclcpp::Time & time, const rclcpp::Duration & period) +======= +// CM API: Called in "update"-thread +HardwareReadWriteStatus ResourceManager::read( + const rclcpp::Time & time, const rclcpp::Duration & period) +>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816)) { for (auto & component : resource_storage_->actuators_) { @@ -1105,7 +1149,13 @@ void ResourceManager::read(const rclcpp::Time & time, const rclcpp::Duration & p } } +<<<<<<< HEAD void ResourceManager::write(const rclcpp::Time & time, const rclcpp::Duration & period) +======= +// CM API: Called in "update"-thread +HardwareReadWriteStatus ResourceManager::write( + const rclcpp::Time & time, const rclcpp::Duration & period) +>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816)) { for (auto & component : resource_storage_->actuators_) { @@ -1117,6 +1167,39 @@ void ResourceManager::write(const rclcpp::Time & time, const rclcpp::Duration & } } +// BEGIN: "used only in tests and locally" +size_t ResourceManager::actuator_components_size() const +{ + return resource_storage_->actuators_.size(); +} + +size_t ResourceManager::sensor_components_size() const +{ + return resource_storage_->sensors_.size(); +} + +size_t ResourceManager::system_components_size() const +{ + return resource_storage_->systems_.size(); +} + +bool ResourceManager::command_interface_exists(const std::string & key) const +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->command_interface_map_.find(key) != + resource_storage_->command_interface_map_.end(); +} + +bool ResourceManager::state_interface_exists(const std::string & key) const +{ + std::lock_guard guard(resource_interfaces_lock_); + return resource_storage_->state_interface_map_.find(key) != + resource_storage_->state_interface_map_.end(); +} +// END: "used only in tests and locally" + +// BEGIN: private methods + void ResourceManager::validate_storage( const std::vector & hardware_info) const { @@ -1189,7 +1272,9 @@ void ResourceManager::validate_storage( } } -// Temporary method to keep old interface and reduce framework changes in PRs +// END: private methods + +// Temporary method to keep old interface and reduce framework changes in the PRs void ResourceManager::activate_all_components() { using lifecycle_msgs::msg::State; diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 1896831460..845ba21d51 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -40,9 +40,14 @@ const auto COMPARE_DELTA = 0.0001; class TestGenericSystem : public ::testing::Test { public: +<<<<<<< HEAD void test_generic_system_with_mimic_joint(const std::string & urdf); void test_generic_system_with_mock_sensor_commands(const std::string & urdf); void test_generic_system_with_mock_gpio_commands(const std::string & urdf); +======= + void test_generic_system_with_fake_sensor_commands(std::string & urdf); + void test_generic_system_with_mimic_joint(std::string & urdf); +>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816)) protected: void SetUp() override @@ -632,9 +637,41 @@ class TestableResourceManager : public hardware_interface::ResourceManager } }; +// Forward declaration +namespace hardware_interface +{ +class ResourceStorage; +} + +class TestableResourceManager : public hardware_interface::ResourceManager +{ +public: + friend TestGenericSystem; + + FRIEND_TEST(TestGenericSystem, generic_fake_system_2dof_symetric_interfaces); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_symetric_interfaces); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_asymetric_interfaces); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_other_interfaces); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_fake_command); + FRIEND_TEST(TestGenericSystem, generic_system_2dof_sensor_fake_command_True); + FRIEND_TEST(TestGenericSystem, hardware_system_2dof_with_mimic_joint); + FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio); + FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command); + FRIEND_TEST(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command); + + TestableResourceManager() : hardware_interface::ResourceManager() {} + + TestableResourceManager( + const std::string & urdf, bool validate_interfaces = true, bool activate_all = false) + : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) + { + } +}; + void set_components_state( - hardware_interface::ResourceManager & rm, const std::vector & components, - const uint8_t state_id, const std::string & state_name) + TestableResourceManager & rm, const std::vector & components, const uint8_t state_id, + const std::string & state_name) { for (const auto & component : components) { @@ -644,7 +681,7 @@ void set_components_state( } auto configure_components = []( - hardware_interface::ResourceManager & rm, + TestableResourceManager & rm, const std::vector & components = {"GenericSystem2dof"}) { set_components_state( @@ -653,7 +690,7 @@ auto configure_components = []( }; auto activate_components = []( - hardware_interface::ResourceManager & rm, + TestableResourceManager & rm, const std::vector & components = {"GenericSystem2dof"}) { set_components_state( @@ -662,7 +699,7 @@ auto activate_components = []( }; auto deactivate_components = []( - hardware_interface::ResourceManager & rm, + TestableResourceManager & rm, const std::vector & components = {"GenericSystem2dof"}) { set_components_state( @@ -674,7 +711,7 @@ TEST_F(TestGenericSystem, load_generic_system_2dof) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ + ros2_control_test_assets::urdf_tail; - ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); + ASSERT_NO_THROW(TestableResourceManager rm(urdf)); } // REMOVE THIS TEST ONCE FAKE COMPONENTS ARE REMOVED @@ -682,7 +719,7 @@ TEST_F(TestGenericSystem, generic_fake_system_2dof_symetric_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_fake_system_2dof_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -713,7 +750,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_symetric_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -744,7 +781,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_asymetric_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -790,7 +827,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_asymetric_interfaces) void generic_system_functional_test(const std::string & urdf, const double offset = 0) { - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // check is hardware is configured auto status_map = rm.get_components_status(); EXPECT_EQ( @@ -900,7 +937,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_other_interfaces) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_other_interface_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -983,7 +1020,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) { auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1079,9 +1116,13 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) ASSERT_EQ(0.33, j2p_c.get_value()); } +<<<<<<< HEAD void TestGenericSystem::test_generic_system_with_mock_sensor_commands(const std::string & urdf) +======= +void TestGenericSystem::test_generic_system_with_fake_sensor_commands(std::string & urdf) +>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816)) { - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1218,9 +1259,13 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) test_generic_system_with_mock_sensor_commands(urdf); } +<<<<<<< HEAD void TestGenericSystem::test_generic_system_with_mimic_joint(const std::string & urdf) +======= +void TestGenericSystem::test_generic_system_with_mimic_joint(std::string & urdf) +>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816)) { - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1319,7 +1364,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i const double offset = -3; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // check is hardware is configured auto status_map = rm.get_components_status(); @@ -1429,11 +1474,11 @@ TEST_F(TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_i hardware_interface::lifecycle_state_names::INACTIVE); } -TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_) +TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio) { auto urdf = ros2_control_test_assets::urdf_head + valid_urdf_ros2_control_system_robot_with_gpio_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // check is hardware is started auto status_map = rm.get_components_status(); @@ -1527,8 +1572,16 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_) generic_system_functional_test(urdf); } +<<<<<<< HEAD void TestGenericSystem::test_generic_system_with_mock_gpio_commands(const std::string & urdf) { +======= +TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command) +{ + auto urdf = ros2_control_test_assets::urdf_head + + valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ + + ros2_control_test_assets::urdf_tail; +>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816)) TestableResourceManager rm(urdf); // check is hardware is started diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 12cc597dc7..64d88ef7c6 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -78,9 +78,15 @@ class TestableResourceManager : public hardware_interface::ResourceManager } }; +<<<<<<< HEAD std::vector set_components_state( hardware_interface::ResourceManager & rm, const std::vector & components, const uint8_t state_id, const std::string & state_name) +======= +void set_components_state( + TestableResourceManager & rm, const std::vector & components, const uint8_t state_id, + const std::string & state_name) +>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816)) { auto int_components = components; if (int_components.empty()) @@ -98,7 +104,7 @@ std::vector set_components_state( } auto configure_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, @@ -106,7 +112,7 @@ auto configure_components = }; auto activate_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, @@ -114,7 +120,7 @@ auto activate_components = }; auto deactivate_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, @@ -122,7 +128,7 @@ auto deactivate_components = }; auto cleanup_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, @@ -130,7 +136,7 @@ auto cleanup_components = }; auto shutdown_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { return set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, @@ -139,25 +145,24 @@ auto shutdown_components = TEST_F(ResourceManagerTest, initialization_empty) { - ASSERT_ANY_THROW(hardware_interface::ResourceManager rm("")); + ASSERT_ANY_THROW(TestableResourceManager rm("")); } TEST_F(ResourceManagerTest, initialization_with_urdf) { - ASSERT_NO_THROW( - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf)); + ASSERT_NO_THROW(TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf)); } TEST_F(ResourceManagerTest, post_initialization_with_urdf) { - hardware_interface::ResourceManager rm; + TestableResourceManager rm; ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf)); } TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) { // we validate the results manually - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); EXPECT_EQ(1u, rm.actuator_components_size()); EXPECT_EQ(1u, rm.sensor_components_size()); @@ -178,13 +183,16 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } +<<<<<<< HEAD TEST_F(ResourceManagerTest, when_missing_state_keys_expect_hw_initialization_fails) +======= +TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) +>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816)) { // missing state keys { EXPECT_THROW( - hardware_interface::ResourceManager rm( - ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), std::exception); } } @@ -194,8 +202,7 @@ TEST_F(ResourceManagerTest, when_missing_command_keys_expect_hw_initialization_f // missing command keys { EXPECT_THROW( - hardware_interface::ResourceManager rm( - ros2_control_test_assets::minimal_robot_missing_command_keys_urdf), + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_missing_command_keys_urdf), std::exception); } } @@ -203,7 +210,7 @@ TEST_F(ResourceManagerTest, when_missing_command_keys_expect_hw_initialization_f TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) { // we validate the results manually - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); auto command_interface_keys = rm.command_interface_keys(); for (const auto & key : command_interface_keys) @@ -219,6 +226,7 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) } } +<<<<<<< HEAD TEST_F(ResourceManagerTest, no_load_urdf_function_called) { TestableResourceManager rm; @@ -247,9 +255,11 @@ TEST_F(ResourceManagerTest, can_load_urdf_later) ASSERT_TRUE(rm.is_urdf_already_loaded()); } +======= +>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816)) TEST_F(ResourceManagerTest, resource_claiming) { - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available activate_components(rm); @@ -361,7 +371,7 @@ class ExternalComponent : public hardware_interface::ActuatorInterface TEST_F(ResourceManagerTest, post_initialization_add_components) { // we validate the results manually - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); // Activate components to get all interfaces available activate_components(rm); @@ -404,7 +414,7 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) TEST_F(ResourceManagerTest, default_prepare_perform_switch) { - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available activate_components(rm); @@ -438,7 +448,7 @@ const auto command_mode_urdf = std::string(ros2_control_test_assets::urdf_head) TEST_F(ResourceManagerTest, custom_prepare_perform_switch) { - hardware_interface::ResourceManager rm(command_mode_urdf); + TestableResourceManager rm(command_mode_urdf); // Scenarios defined by example criteria std::vector empty_keys = {}; std::vector irrelevant_keys = {"elbow_joint/position", "should_joint/position"}; @@ -474,7 +484,7 @@ TEST_F(ResourceManagerTest, custom_prepare_perform_switch) TEST_F(ResourceManagerTest, resource_status) { - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); auto status_map = rm.get_components_status(); @@ -546,7 +556,7 @@ TEST_F(ResourceManagerTest, resource_status) TEST_F(ResourceManagerTest, lifecycle_all_resources) { - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // All resources start as UNCONFIGURED { @@ -689,7 +699,7 @@ TEST_F(ResourceManagerTest, lifecycle_all_resources) TEST_F(ResourceManagerTest, lifecycle_individual_resources) { - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // All resources start as UNCONFIGURED { @@ -902,7 +912,7 @@ TEST_F(ResourceManagerTest, lifecycle_individual_resources) TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { using std::placeholders::_1; - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); auto check_interfaces = [](const std::vector & interface_names, auto check_method, bool expected_result) @@ -946,50 +956,44 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) check_interfaces( command_interface_names, - std::bind(&hardware_interface::ResourceManager::command_interface_is_claimed, &rm, _1), - expected_result); + std::bind(&TestableResourceManager::command_interface_is_claimed, &rm, _1), expected_result); }; // All resources start as UNCONFIGURED - All interfaces are imported but not available { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::command_interface_exists, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::command_interface_exists, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), false); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), false); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); } // Nothing can be claimed @@ -1006,28 +1010,23 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( {"joint1/position"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( {"joint1/max_velocity"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), false); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); } // Can claim Actuator's interfaces @@ -1045,24 +1044,20 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), false); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); } // Can claim all Actuator's state interfaces and command interfaces @@ -1078,20 +1073,20 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::command_interface_exists, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::command_interface_exists, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); } // When Sensor and System are configured their state- @@ -1100,26 +1095,23 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( {"joint2/velocity", "joint3/velocity"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( {"joint2/max_acceleration", "configuration/max_tcp_jerk"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); } // Can claim: @@ -1141,22 +1133,20 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); } // Can claim everything @@ -1176,26 +1166,23 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( {"joint1/position"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( {"joint1/max_velocity"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); } // Can claim everything @@ -1216,27 +1203,23 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( {"joint1/position"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( {"joint1/max_velocity"}, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_is_available, &rm, _1), - true); + std::bind(&TestableResourceManager::command_interface_is_available, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), - false); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), false); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_is_available, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_is_available, &rm, _1), true); } // Can claim everything @@ -1256,26 +1239,26 @@ TEST_F(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle) { check_interfaces( TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::command_interface_exists, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, - std::bind(&hardware_interface::ResourceManager::command_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::command_interface_exists, &rm, _1), true); check_interfaces( TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_SENSOR_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); check_interfaces( TEST_SYSTEM_HARDWARE_STATE_INTERFACES, - std::bind(&hardware_interface::ResourceManager::state_interface_exists, &rm, _1), true); + std::bind(&TestableResourceManager::state_interface_exists, &rm, _1), true); } } TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) { - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); std::string CONTROLLER_NAME = "test_controller"; std::vector REFERENCE_INTERFACE_NAMES = {"input1", "input2", "input3"}; @@ -1388,3 +1371,266 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) EXPECT_THROW( rm.make_controller_reference_interfaces_unavailable("unknown_controller"), std::out_of_range); } +<<<<<<< HEAD +======= + +class ResourceManagerTestReadWriteError : public ResourceManagerTest +{ +public: + void setup_resource_manager_and_do_initial_checks() + { + rm = std::make_shared( + ros2_control_test_assets::minimal_robot_urdf, false); + activate_components(*rm); + + 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_ACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + claimed_itfs.push_back( + rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); + claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); + + check_if_interface_available(true, true); + // with default values read and write should run without any problems + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + { + auto [ok, failed_hardware_names] = rm->write(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + check_if_interface_available(true, true); + } + + // check if all interfaces are available + void check_if_interface_available(const bool actuator_interfaces, const bool system_interfaces) + { + for (const auto & interface : TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), system_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), system_interfaces); + } + }; + + using FunctionT = + std::function; + + void check_read_or_write_failure( + FunctionT method_that_fails, FunctionT other_method, const double fail_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); + + // read failure for TEST_ACTUATOR_HARDWARE_NAME + claimed_itfs[0].set_value(fail_value); + claimed_itfs[1].set_value(fail_value - 10.0); + { + auto [ok, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_FALSE(ok); + EXPECT_FALSE(failed_hardware_names.empty()); + ASSERT_THAT( + failed_hardware_names, + testing::ElementsAreArray(std::vector({TEST_ACTUATOR_HARDWARE_NAME}))); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(false, 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); + } + + // read failure for TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(fail_value - 10.0); + claimed_itfs[1].set_value(fail_value); + { + auto [ok, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_FALSE(ok); + EXPECT_FALSE(failed_hardware_names.empty()); + ASSERT_THAT( + failed_hardware_names, + testing::ElementsAreArray(std::vector({TEST_SYSTEM_HARDWARE_NAME}))); + 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_UNCONFIGURED); + check_if_interface_available(true, false); + 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); + } + + // read failure for both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(fail_value); + claimed_itfs[1].set_value(fail_value); + { + auto [ok, failed_hardware_names] = method_that_fails(time, duration); + EXPECT_FALSE(ok); + EXPECT_FALSE(failed_hardware_names.empty()); + ASSERT_THAT( + failed_hardware_names, testing::ElementsAreArray(std::vector( + {TEST_ACTUATOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME}))); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + check_if_interface_available(false, false); + 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; + + const rclcpp::Time time = rclcpp::Time(0); + 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) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // 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); +} + +TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // 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); +} + +TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) +{ + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); + activate_components(rm); + + static const std::string TEST_CONTROLLER_ACTUATOR_NAME = "test_controller_actuator"; + static const std::string TEST_CONTROLLER_SYSTEM_NAME = "test_controller_system"; + static const std::string TEST_BROADCASTER_ALL_NAME = "test_broadcaster_all"; + static const std::string TEST_BROADCASTER_SENSOR_NAME = "test_broadcaster_sensor"; + + rm.cache_controller_to_hardware( + TEST_CONTROLLER_ACTUATOR_NAME, TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES); + rm.cache_controller_to_hardware( + TEST_BROADCASTER_ALL_NAME, TEST_ACTUATOR_HARDWARE_STATE_INTERFACES); + + rm.cache_controller_to_hardware( + TEST_CONTROLLER_SYSTEM_NAME, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES); + rm.cache_controller_to_hardware(TEST_BROADCASTER_ALL_NAME, TEST_SYSTEM_HARDWARE_STATE_INTERFACES); + + rm.cache_controller_to_hardware( + TEST_BROADCASTER_SENSOR_NAME, TEST_SENSOR_HARDWARE_STATE_INTERFACES); + rm.cache_controller_to_hardware(TEST_BROADCASTER_ALL_NAME, TEST_SENSOR_HARDWARE_STATE_INTERFACES); + + { + auto controllers = rm.get_cached_controllers_to_hardware(TEST_ACTUATOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::ElementsAreArray(std::vector( + {TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = rm.get_cached_controllers_to_hardware(TEST_SYSTEM_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::ElementsAreArray(std::vector( + {TEST_CONTROLLER_SYSTEM_NAME, TEST_BROADCASTER_ALL_NAME}))); + } + + { + auto controllers = rm.get_cached_controllers_to_hardware(TEST_SENSOR_HARDWARE_NAME); + ASSERT_THAT( + controllers, testing::ElementsAreArray(std::vector( + {TEST_BROADCASTER_SENSOR_NAME, TEST_BROADCASTER_ALL_NAME}))); + } +} +>>>>>>> 88c74ae (Cleanup Resource Manager a bit to increase clarity. (#816))