From a31acf81a2d77d84b81cd8354801fe880dab6e63 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 29 Jan 2024 19:16:09 +0000 Subject: [PATCH] __ -> _ load_urdf() -> load_and_initialize_components() --- controller_manager/src/controller_manager.cpp | 39 ++++++--------- .../test_controller_manager_urdf_passing.cpp | 20 +++++--- .../hardware_interface/resource_manager.hpp | 14 +++--- hardware_interface/src/resource_manager.cpp | 11 ++-- .../test/test_resource_manager.cpp | 50 ++++++++++--------- 5 files changed, 69 insertions(+), 65 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ab57c5a196..2b390101f9 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -341,36 +341,29 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & RCLCPP_INFO(get_logger(), "Received robot description from topic."); RCLCPP_DEBUG( get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); - // TODO(Manuel): errors should probably be caught since we don't want controller_manager node - // to die if a non valid urdf is passed. However, should maybe be fine tuned. - try - { - robot_description_ = robot_description.data; - if (resource_manager_->is_urdf_already_loaded()) - { - RCLCPP_WARN( - get_logger(), - "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " - "description file."); - return; - } - init_resource_manager(robot_description_); - init_services(); - } - catch (std::runtime_error & e) + robot_description_ = robot_description.data; + if (resource_manager_->is_urdf_already_loaded()) { - RCLCPP_ERROR_STREAM( + RCLCPP_WARN( get_logger(), - "The published robot description file (urdf) seems not to be genuine. The following error " - "was caught:" - << e.what()); + "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " + "description file."); + return; } + init_resource_manager(robot_description_); + init_services(); } void ControllerManager::init_resource_manager(const std::string & robot_description) { - // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong... - resource_manager_->load_urdf(robot_description); + if (!resource_manager_->load_and_initialize_components(robot_description)) + { + RCLCPP_WARN( + get_logger(), + "URDF validation went wrong check the previous output. This might only mean that interfaces " + "defined in URDF and exported by the hardware do not match. Therefore continue initializing " + "controller manager..."); + } // Get all components and if they are not defined in parameters activate them automatically auto components_to_activate = resource_manager_->get_components_status(); diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 5c2ebd14f6..2ea291d2a8 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -29,11 +29,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, load_urdf_called_after_callback); - FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + 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); public: TestableControllerManager( @@ -59,12 +61,12 @@ class TestControllerManagerWithTestableCM } }; -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()); // mimic callback @@ -74,7 +76,9 @@ TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback) 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()); // mimic callback diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 743e548a4c..751ccf1606 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -89,17 +89,17 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] load_and_initialize_components boolean argument indicating whether to load and * initialize the components present in the parsed URDF. Defaults to true. */ - void 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; @@ -423,7 +423,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 2e8ccc7b1f..1c9723a8ff 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -742,7 +742,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) { @@ -756,15 +756,18 @@ ResourceManager::ResourceManager( } // CM API: Called in "callback/slow"-thread -void ResourceManager::load_urdf( +bool ResourceManager::load_and_initialize_components( const std::string & urdf, bool validate_interfaces, bool load_and_initialize_components) { - is_urdf_loaded__ = true; + bool result = 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) @@ -799,7 +802,7 @@ void ResourceManager::load_urdf( resource_storage_->systems_.size()); } -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 4bb9e0c5fe..39c1813cf8 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -98,25 +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; - ASSERT_THROW( - rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true), - std::runtime_error); + 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_NO_THROW(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")); @@ -168,15 +168,17 @@ TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) { // missing state keys { - EXPECT_THROW( - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), - std::exception); + auto rm = TestableResourceManager(); + 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 { - EXPECT_THROW( - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_missing_command_keys_urdf), - std::exception); + auto rm = TestableResourceManager(); + EXPECT_FALSE(rm.load_and_initialize_components( + ros2_control_test_assets::minimal_robot_missing_command_keys_urdf)); + ASSERT_TRUE(rm.is_urdf_already_loaded()); } } @@ -199,31 +201,33 @@ 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_invalid) +TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_valid) { TestableResourceManager rm; - EXPECT_THROW( - rm.load_urdf(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), std::exception); - ASSERT_TRUE(rm.is_urdf_already_loaded()); + ASSERT_FALSE( + rm.load_and_initialize_components( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), + std::exception); + 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()); }