diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6c16a552f3..28fb970126 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -337,7 +337,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & void ControllerManager::init_resource_manager(const std::string & robot_description) { - if (!resource_manager_->load_urdf(robot_description)) + if (!resource_manager_->load_and_initialize_components(robot_description)) { RCLCPP_WARN( get_logger(), diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index b5adb257ab..d8a9785052 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -33,9 +33,13 @@ class TestableControllerManager : public controller_manager::ControllerManager { friend TestControllerManagerWithTestableCM; - FRIEND_TEST(TestControllerManagerWithTestableCM, initial_no_load_urdf_called); - FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); - FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed); + FRIEND_TEST( + TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called); + FRIEND_TEST( + TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback); + FRIEND_TEST( + TestControllerManagerWithTestableCM, + load_and_initialize_components_called_after_invalid_urdf_passed); FRIEND_TEST( TestControllerManagerWithTestableCM, when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description); @@ -106,19 +110,21 @@ class TestControllerManagerWithTestableCM std::shared_ptr test_controller_; }; -TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) +TEST_P(TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called) { ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); } -TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback) +TEST_P(TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback) { ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); pass_robot_description_to_cm_and_rm(); ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); } -TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed) +TEST_P( + TestControllerManagerWithTestableCM, + load_and_initialize_components_called_after_invalid_urdf_passed) { ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); pass_robot_description_to_cm_and_rm( diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 24d9264850..b7b7a414bb 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -90,17 +90,17 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * initialize the components present in the parsed URDF. Defaults to true. * \returns false if URDF validation has failed. */ - bool load_urdf( + bool load_and_initialize_components( const std::string & urdf, bool validate_interfaces = true, bool load_and_initialize_components = true); /** - * @brief if the resource manager load_urdf(...) function has been called this returns true. - * We want to permit to load the urdf later on but we currently don't want to permit multiple - * calls to load_urdf (reloading/loading different urdf). + * @brief if the resource manager load_and_initialize_components(...) function has been called + * this returns true. We want to permit to load the urdf later on but we currently don't want to + * permit multiple calls to load_and_initialize_components (reloading/loading different urdf). * - * @return true if resource manager's load_urdf() has been already called. - * @return false if resource manager's load_urdf() has not been yet called. + * @return true if resource manager's load_and_initialize_components() has been already called. + * @return false if resource manager's load_and_initialize_components() has not been yet called. */ bool is_urdf_already_loaded() const; @@ -424,7 +424,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; - bool is_urdf_loaded__ = false; + bool is_urdf_loaded_ = false; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 1e125e6128..ccb531f6f3 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -744,7 +744,7 @@ ResourceManager::ResourceManager( rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) : resource_storage_(std::make_unique(update_rate, clock_interface)) { - load_urdf(urdf, validate_interfaces); + load_and_initialize_components(urdf, validate_interfaces); if (activate_all) { @@ -758,17 +758,18 @@ ResourceManager::ResourceManager( } // CM API: Called in "callback/slow"-thread -bool ResourceManager::load_urdf( +bool ResourceManager::load_and_initialize_components( const std::string & urdf, bool validate_interfaces, bool load_and_initialize_components) { bool result = true; - is_urdf_loaded__ = true; const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + is_urdf_loaded_ = true; + if (load_and_initialize_components) { for (const auto & individual_hardware_info : hardware_info) @@ -805,7 +806,7 @@ bool ResourceManager::load_urdf( return result; } -bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } +bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded_; } // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 897b97ca54..4e5a21a427 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -98,23 +98,25 @@ TEST_F(ResourceManagerTest, initialization_with_urdf) TEST_F(ResourceManagerTest, post_initialization_with_urdf) { TestableResourceManager rm; - ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf)); + ASSERT_NO_THROW(rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf)); } TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation) { - // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a - // runtime exception is thrown + // If the the hardware can not be initialized and load_and_initialize_components tried to validate + // the interfaces a runtime exception is thrown TestableResourceManager rm; - EXPECT_FALSE(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true)); + EXPECT_FALSE(rm.load_and_initialize_components( + ros2_control_test_assets::minimal_unitilizable_robot_urdf, true)); } TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) { - // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces, - // the interface should not show up + // If the the hardware can not be initialized and load_and_initialize_components didn't try to + // validate the interfaces, the interface should not show up TestableResourceManager rm; - EXPECT_TRUE(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + EXPECT_TRUE(rm.load_and_initialize_components( + ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); // test actuator EXPECT_FALSE(rm.state_interface_exists("joint1/position")); @@ -167,13 +169,15 @@ TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) // missing state keys { auto rm = TestableResourceManager(); - EXPECT_FALSE(rm.load_urdf(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf)); + EXPECT_FALSE(rm.load_and_initialize_components( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf)); ASSERT_TRUE(rm.is_urdf_already_loaded()); } // missing command keys { auto rm = TestableResourceManager(); - EXPECT_FALSE(rm.load_urdf(ros2_control_test_assets::minimal_robot_missing_command_keys_urdf)); + EXPECT_FALSE(rm.load_and_initialize_components( + ros2_control_test_assets::minimal_robot_missing_command_keys_urdf)); ASSERT_TRUE(rm.is_urdf_already_loaded()); } } @@ -197,23 +201,23 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) } } -TEST_F(ResourceManagerTest, no_load_urdf_function_called) +TEST_F(ResourceManagerTest, no_load_and_initialize_components_function_called) { TestableResourceManager rm; ASSERT_FALSE(rm.is_urdf_already_loaded()); } -TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_valid) +TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_valid) { TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); ASSERT_TRUE(rm.is_urdf_already_loaded()); } -TEST_F(ResourceManagerTest, can_load_urdf_later) +TEST_F(ResourceManagerTest, can_load_and_initialize_components_later) { TestableResourceManager rm; ASSERT_FALSE(rm.is_urdf_already_loaded()); - rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf); + rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf); ASSERT_TRUE(rm.is_urdf_already_loaded()); }