From 88c74ae09f9d067542c84f8ec9bd056e7c4a38dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 24 Sep 2022 09:34:20 +0200 Subject: [PATCH] Cleanup Resource Manager a bit to increase clarity. (#816) --- .../hardware_interface/resource_manager.hpp | 27 +- hardware_interface/src/resource_manager.cpp | 89 +++--- .../mock_components/test_generic_system.cpp | 78 ++++-- .../test/test_resource_manager.cpp | 265 +++++++++--------- 4 files changed, 258 insertions(+), 201 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 9ae1d41fd0..1693e85574 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -108,12 +108,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. @@ -228,13 +222,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. @@ -390,6 +377,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; @@ -400,6 +400,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager mutable std::recursive_mutex resource_interfaces_lock_; mutable std::recursive_mutex claimed_command_interfaces_lock_; mutable std::recursive_mutex resources_lock_; + std::unique_ptr resource_storage_; // Structure to store read and write status so it is not initialized in the real-time loop diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index d3dea6c402..4b80bc09dd 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -591,6 +591,7 @@ ResourceManager::ResourceManager( } } +// CM API: Called in "callback/slow"-thread void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { const std::string system_type = "system"; @@ -631,6 +632,7 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac resource_storage_->systems_.size()); } +// CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) { if (!state_interface_is_available(key)) @@ -642,6 +644,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; @@ -653,19 +656,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_); @@ -675,6 +673,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) { @@ -684,12 +683,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) { @@ -701,6 +702,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) { @@ -723,6 +725,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 = @@ -734,6 +737,7 @@ void ResourceManager::remove_controller_reference_interfaces(const std::string & resource_storage_->remove_command_interfaces(interface_names); } +// CM API: Called in "callback/slow"-thread void ResourceManager::cache_controller_to_hardware( const std::string & controller_name, const std::vector & interfaces) { @@ -773,6 +777,7 @@ void ResourceManager::cache_controller_to_hardware( } } +// CM API: Called in "update"-thread std::vector ResourceManager::get_cached_controllers_to_hardware( const std::string & hardware_name) { @@ -820,6 +825,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; @@ -831,20 +837,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_); @@ -854,16 +854,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) { @@ -894,12 +884,7 @@ void ResourceManager::import_component( resource_storage_->systems_.size()); } -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_) @@ -918,6 +903,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) @@ -963,6 +949,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) @@ -990,6 +977,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) { @@ -1073,6 +1061,7 @@ return_type ResourceManager::set_component_state( return result; } +// CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -1100,6 +1089,7 @@ HardwareReadWriteStatus ResourceManager::read( return read_write_status; } +// CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::write( const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -1126,6 +1116,39 @@ HardwareReadWriteStatus ResourceManager::write( return read_write_status; } +// 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 { @@ -1181,7 +1204,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 52518a52e8..fe0a062fc1 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -38,6 +38,10 @@ const auto PERIOD = rclcpp::Duration::from_seconds(0.01); class TestGenericSystem : public ::testing::Test { +public: + void test_generic_system_with_fake_sensor_commands(std::string & urdf); + void test_generic_system_with_mimic_joint(std::string & urdf); + protected: void SetUp() override { @@ -429,9 +433,41 @@ class TestGenericSystem : public ::testing::Test std::string valid_urdf_ros2_control_system_robot_with_gpio_fake_command_; }; +// 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) { @@ -441,7 +477,7 @@ void set_components_state( } auto configure_components = []( - hardware_interface::ResourceManager & rm, + TestableResourceManager & rm, const std::vector & components = {"GenericSystem2dof"}) { set_components_state( @@ -450,7 +486,7 @@ auto configure_components = []( }; auto activate_components = []( - hardware_interface::ResourceManager & rm, + TestableResourceManager & rm, const std::vector & components = {"GenericSystem2dof"}) { set_components_state( @@ -459,7 +495,7 @@ auto activate_components = []( }; auto deactivate_components = []( - hardware_interface::ResourceManager & rm, + TestableResourceManager & rm, const std::vector & components = {"GenericSystem2dof"}) { set_components_state( @@ -471,7 +507,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 @@ -479,7 +515,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); @@ -510,7 +546,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); @@ -541,7 +577,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); @@ -587,7 +623,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( @@ -697,7 +733,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); @@ -780,7 +816,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); @@ -876,9 +912,9 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) ASSERT_EQ(0.33, j2p_c.get_value()); } -void test_generic_system_with_fake_sensor_commands(std::string urdf) +void TestGenericSystem::test_generic_system_with_fake_sensor_commands(std::string & urdf) { - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1015,9 +1051,9 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_fake_command_True) test_generic_system_with_fake_sensor_commands(urdf); } -void test_generic_system_with_mimic_joint(std::string urdf) +void TestGenericSystem::test_generic_system_with_mimic_joint(std::string & urdf) { - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1116,7 +1152,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(); @@ -1226,11 +1262,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(); @@ -1324,12 +1360,12 @@ TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_) generic_system_functional_test(urdf); } -TEST_F(TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command_) +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; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // check is hardware is started auto status_map = rm.get_components_status(); diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 22ba2d1a9b..7674213903 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -45,7 +45,7 @@ using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES; using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE; -class TestResourceManager : public ::testing::Test +class ResourceManagerTest : public ::testing::Test { public: static void SetUpTestCase() {} @@ -53,9 +53,34 @@ class TestResourceManager : public ::testing::Test void SetUp() {} }; +// Forward declaration +namespace hardware_interface +{ +class ResourceStorage; +} + +class TestableResourceManager : public hardware_interface::ResourceManager +{ +public: + friend ResourceManagerTest; + + FRIEND_TEST(ResourceManagerTest, initialization_with_urdf_manual_validation); + FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); + FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); + FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); + + 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) { auto int_components = components; if (int_components.empty()) @@ -70,7 +95,7 @@ void set_components_state( } auto configure_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, @@ -78,7 +103,7 @@ auto configure_components = }; auto activate_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, @@ -86,7 +111,7 @@ auto activate_components = }; auto deactivate_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, @@ -94,7 +119,7 @@ auto deactivate_components = }; auto cleanup_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, @@ -102,34 +127,33 @@ auto cleanup_components = }; auto shutdown_components = - [](hardware_interface::ResourceManager & rm, const std::vector & components = {}) + [](TestableResourceManager & rm, const std::vector & components = {}) { set_components_state( rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, hardware_interface::lifecycle_state_names::FINALIZED); }; -TEST_F(TestResourceManager, initialization_empty) +TEST_F(ResourceManagerTest, initialization_empty) { - ASSERT_ANY_THROW(hardware_interface::ResourceManager rm("")); + ASSERT_ANY_THROW(TestableResourceManager rm("")); } -TEST_F(TestResourceManager, initialization_with_urdf) +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(TestResourceManager, post_initialization_with_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(TestResourceManager, initialization_with_urdf_manual_validation) +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()); @@ -150,28 +174,26 @@ TEST_F(TestResourceManager, initialization_with_urdf_manual_validation) EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } -TEST_F(TestResourceManager, initialization_with_wrong_urdf) +TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) { // 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); } // 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); } } -TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) +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) @@ -187,9 +209,9 @@ TEST_F(TestResourceManager, initialization_with_urdf_unclaimed) } } -TEST_F(TestResourceManager, resource_claiming) +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); @@ -298,10 +320,10 @@ class ExternalComponent : public hardware_interface::ActuatorInterface } }; -TEST_F(TestResourceManager, post_initialization_add_components) +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); @@ -342,9 +364,9 @@ TEST_F(TestResourceManager, post_initialization_add_components) EXPECT_NO_THROW(rm.claim_command_interface("external_joint/external_command_interface")); } -TEST_F(TestResourceManager, default_prepare_perform_switch) +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); @@ -376,9 +398,9 @@ const auto command_mode_urdf = std::string(ros2_control_test_assets::urdf_head) std::string(hardware_resources_command_modes) + std::string(ros2_control_test_assets::urdf_tail); -TEST_F(TestResourceManager, custom_prepare_perform_switch) +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"}; @@ -412,9 +434,9 @@ TEST_F(TestResourceManager, custom_prepare_perform_switch) EXPECT_FALSE(rm.perform_command_mode_switch(empty_keys, legal_keys_position)); } -TEST_F(TestResourceManager, resource_status) +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(); @@ -484,9 +506,9 @@ TEST_F(TestResourceManager, resource_status) status_map[TEST_SYSTEM_HARDWARE_NAME].state_interfaces, TEST_SYSTEM_HARDWARE_STATE_INTERFACES); } -TEST_F(TestResourceManager, lifecycle_all_resources) +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 { @@ -627,9 +649,9 @@ TEST_F(TestResourceManager, lifecycle_all_resources) } } -TEST_F(TestResourceManager, lifecycle_individual_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 { @@ -840,10 +862,10 @@ TEST_F(TestResourceManager, lifecycle_individual_resources) } } -TEST_F(TestResourceManager, resource_availability_and_claiming_in_lifecycle) +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) @@ -887,50 +909,44 @@ TEST_F(TestResourceManager, 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 @@ -947,28 +963,23 @@ TEST_F(TestResourceManager, 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 @@ -986,24 +997,20 @@ TEST_F(TestResourceManager, 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 @@ -1019,20 +1026,20 @@ TEST_F(TestResourceManager, 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- @@ -1041,26 +1048,23 @@ TEST_F(TestResourceManager, 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: @@ -1082,22 +1086,20 @@ TEST_F(TestResourceManager, 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 @@ -1117,26 +1119,23 @@ TEST_F(TestResourceManager, 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 @@ -1157,27 +1156,23 @@ TEST_F(TestResourceManager, 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 @@ -1197,26 +1192,26 @@ TEST_F(TestResourceManager, 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(TestResourceManager, managing_controllers_reference_interfaces) +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"}; @@ -1330,12 +1325,12 @@ TEST_F(TestResourceManager, managing_controllers_reference_interfaces) rm.make_controller_reference_interfaces_unavailable("unknown_controller"), std::out_of_range); } -class TestResourceManagerReadWriteError : public TestResourceManager +class ResourceManagerTestReadWriteError : public ResourceManagerTest { public: void setup_resource_manager_and_do_initial_checks() { - rm = std::make_shared( + rm = std::make_shared( ros2_control_test_assets::minimal_robot_urdf, false); activate_components(*rm); @@ -1512,7 +1507,7 @@ class TestResourceManagerReadWriteError : public TestResourceManager } public: - std::shared_ptr rm; + std::shared_ptr rm; std::vector claimed_itfs; const rclcpp::Time time = rclcpp::Time(0); @@ -1523,31 +1518,31 @@ class TestResourceManagerReadWriteError : public TestResourceManager static constexpr double WRITE_FAIL_VALUE = 23232323.0; }; -TEST_F(TestResourceManagerReadWriteError, handle_error_on_hardware_read) +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(&hardware_interface::ResourceManager::read, rm, _1, _2), - std::bind(&hardware_interface::ResourceManager::write, rm, _1, _2), READ_FAIL_VALUE); + std::bind(&TestableResourceManager::read, rm, _1, _2), + std::bind(&TestableResourceManager::write, rm, _1, _2), READ_FAIL_VALUE); } -TEST_F(TestResourceManagerReadWriteError, handle_error_on_hardware_write) +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(&hardware_interface::ResourceManager::write, rm, _1, _2), - std::bind(&hardware_interface::ResourceManager::read, rm, _1, _2), WRITE_FAIL_VALUE); + std::bind(&TestableResourceManager::write, rm, _1, _2), + std::bind(&TestableResourceManager::read, rm, _1, _2), WRITE_FAIL_VALUE); } -TEST_F(TestResourceManager, test_caching_of_controllers_to_hardware) +TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) { - hardware_interface::ResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); activate_components(rm); static const std::string TEST_CONTROLLER_ACTUATOR_NAME = "test_controller_actuator";