diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 26eb80d158..44dcb57e72 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -118,12 +118,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. @@ -239,13 +233,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. @@ -401,6 +388,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; @@ -411,6 +411,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 2fd1578535..75ff60fc6f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -675,6 +675,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; @@ -730,6 +731,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; @@ -741,19 +743,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_); @@ -763,6 +760,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) { @@ -772,12 +770,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) { @@ -789,6 +789,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) { @@ -811,6 +812,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 = @@ -822,6 +824,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) { @@ -861,6 +864,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) { @@ -908,6 +912,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; @@ -919,20 +924,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_); @@ -942,16 +941,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) { @@ -982,12 +971,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_) @@ -1006,6 +990,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) @@ -1051,6 +1036,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) @@ -1078,6 +1064,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) { @@ -1161,6 +1148,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) { @@ -1188,6 +1176,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) { @@ -1214,6 +1203,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 { @@ -1286,7 +1308,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..92851303b3 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -611,7 +611,6 @@ 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); @@ -633,8 +632,8 @@ class TestableResourceManager : public hardware_interface::ResourceManager }; 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 +643,7 @@ void set_components_state( } auto configure_components = []( - hardware_interface::ResourceManager & rm, + TestableResourceManager & rm, const std::vector & components = {"GenericSystem2dof"}) { set_components_state( @@ -653,7 +652,7 @@ auto configure_components = []( }; auto activate_components = []( - hardware_interface::ResourceManager & rm, + TestableResourceManager & rm, const std::vector & components = {"GenericSystem2dof"}) { set_components_state( @@ -662,7 +661,7 @@ auto activate_components = []( }; auto deactivate_components = []( - hardware_interface::ResourceManager & rm, + TestableResourceManager & rm, const std::vector & components = {"GenericSystem2dof"}) { set_components_state( @@ -674,7 +673,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 +681,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 +712,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 +743,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 +789,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 +899,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 +982,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); @@ -1081,7 +1080,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor) void TestGenericSystem::test_generic_system_with_mock_sensor_commands(const std::string & urdf) { - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1220,7 +1219,7 @@ TEST_F(TestGenericSystem, generic_system_2dof_sensor_mock_command_True) void TestGenericSystem::test_generic_system_with_mimic_joint(const std::string & urdf) { - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1319,7 +1318,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 +1428,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(); @@ -1657,7 +1656,7 @@ TEST_F(TestGenericSystem, sensor_with_initial_value) { auto urdf = ros2_control_test_assets::urdf_head + sensor_with_initial_value_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); @@ -1685,7 +1684,7 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) { auto urdf = ros2_control_test_assets::urdf_head + gpio_with_initial_value_ + ros2_control_test_assets::urdf_tail; - hardware_interface::ResourceManager rm(urdf); + TestableResourceManager rm(urdf); // Activate components to get all interfaces available activate_components(rm); diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 0230972d2d..8b56fda85b 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -98,7 +98,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 +106,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 +114,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 +122,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 +130,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 +139,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,24 +177,18 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } -TEST_F(ResourceManagerTest, when_missing_state_keys_expect_hw_initialization_fails) +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); } -} - -TEST_F(ResourceManagerTest, when_missing_command_keys_expect_hw_initialization_fails) -{ // 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 +196,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) @@ -249,7 +242,7 @@ TEST_F(ResourceManagerTest, can_load_urdf_later) 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 +354,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 +397,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 +431,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 +467,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 +539,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 +682,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 +895,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 +939,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 +993,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 +1027,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 +1056,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 +1078,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 +1116,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 +1149,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 +1186,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 +1222,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"}; @@ -1389,12 +1355,14 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces) rm.make_controller_reference_interfaces_unavailable("unknown_controller"), std::out_of_range); } -class TestResourceManagerReadWriteError : public ResourceManagerTest +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); @@ -1571,7 +1539,7 @@ class TestResourceManagerReadWriteError : public ResourceManagerTest } public: - std::shared_ptr rm; + std::shared_ptr rm; std::vector claimed_itfs; const rclcpp::Time time = rclcpp::Time(0); @@ -1582,31 +1550,31 @@ class TestResourceManagerReadWriteError : public ResourceManagerTest 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(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";