From 21c43a10ad19541faf408130ea03e8b1e05019d3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 18 Dec 2023 14:53:25 +0100 Subject: [PATCH] Added functionality and tests for getting robot description from the topic. --- controller_manager/CMakeLists.txt | 1 + .../controller_manager/controller_manager.hpp | 3 +- controller_manager/src/controller_manager.cpp | 65 +++++--- .../test_controller_manager_urdf_passing.cpp | 156 +++++++++++++++--- .../test/test_spawner_unspawner.cpp | 80 +++++++++ .../hardware_interface/resource_manager.hpp | 5 +- hardware_interface/src/resource_manager.cpp | 34 ++-- .../test/test_resource_manager.cpp | 20 +-- 8 files changed, 287 insertions(+), 77 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 376cef3362d..0d1b88a55ab 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -150,6 +150,7 @@ if(BUILD_TESTING) ) target_link_libraries(test_controller_manager_urdf_passing controller_manager + test_controller ros2_control_test_assets::ros2_control_test_assets ) ament_target_dependencies(test_controller_manager_urdf_passing diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 79a98614638..cc1fdaf3a45 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -322,7 +322,7 @@ class ControllerManager : public rclcpp::Node std::vector get_controller_names(); std::pair split_command_interface( const std::string & command_interface); - void subscribe_to_robot_description_topic(); + void init_controller_manager(); /** * Clear request lists used when switching controllers. The lists are shared between "callback" @@ -536,6 +536,7 @@ class ControllerManager : public rclcpp::Node std::string robot_description_; rclcpp::Subscription::SharedPtr robot_description_subscription_; + rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; struct SwitchParams { diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e74c3ded795..f0d02375b8e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -267,18 +267,7 @@ ControllerManager::ControllerManager( std::make_shared>( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { - if (!get_parameter("update_rate", update_rate_)) - { - RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); - } - - subscribe_to_robot_description_topic(); - - diagnostics_updater_.setHardwareID("ros2_control"); - diagnostics_updater_.add( - "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); - - init_services(); + init_controller_manager(); } ControllerManager::ControllerManager( @@ -295,21 +284,17 @@ ControllerManager::ControllerManager( std::make_shared>( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { + init_controller_manager(); +} + +void ControllerManager::init_controller_manager() +{ + // Get parameters needed for RT "update" loop to work if (!get_parameter("update_rate", update_rate_)) { RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } - subscribe_to_robot_description_topic(); - - diagnostics_updater_.setHardwareID("ros2_control"); - diagnostics_updater_.add( - "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); - init_services(); -} - -void ControllerManager::subscribe_to_robot_description_topic() -{ // set QoS to transient local to get messages that have already been published // (if robot state publisher starts before controller manager) robot_description_subscription_ = create_subscription( @@ -318,6 +303,19 @@ void ControllerManager::subscribe_to_robot_description_topic() RCLCPP_INFO( get_logger(), "Subscribing to '%s' topic for robot description.", robot_description_subscription_->get_topic_name()); + + // Setup diagnostics + diagnostics_updater_.setHardwareID("ros2_control"); + diagnostics_updater_.add( + "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); + + robot_description_notification_timer_ = create_wall_timer( + std::chrono::seconds(1), + [&]() + { + RCLCPP_WARN( + get_logger(), "Waiting for data on '~/robot_description' topic to finish initialization"); + }); } void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) @@ -339,8 +337,14 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & 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_urdf(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(); @@ -396,6 +400,10 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript { resource_manager_->set_component_state(component, active_state); } + + // Init CM services first after the URDF is loaded an components are set + init_services(); + robot_description_notification_timer_->cancel(); } void ControllerManager::init_services() @@ -752,6 +760,15 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & deactivate_controllers, int strictness, bool activate_asap, const rclcpp::Duration & timeout) { + if (!resource_manager_->is_urdf_already_loaded()) + { + RCLCPP_ERROR( + get_logger(), + "Resource Manager is not initialized yet! Please provide robot description on " + "'~/robot_description' topic before trying to switch controllers."); + return controller_interface::return_type::ERROR; + } + switch_params_ = SwitchParams(); if (!deactivate_request_.empty() || !activate_request_.empty()) diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index e10e5b603f1..88b92533a1b 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -37,7 +37,14 @@ class TestableControllerManager : public controller_manager::ControllerManager FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed); FRIEND_TEST( - TestControllerManagerWithTestableCM, when_starting_controller_without_hw_expect_error); + TestControllerManagerWithTestableCM, + when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description); + FRIEND_TEST( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_with_robot_description); + FRIEND_TEST( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_after_some_time); public: TestableControllerManager( @@ -58,6 +65,45 @@ class TestControllerManagerWithTestableCM public: // create cm with no urdf TestControllerManagerWithTestableCM() : ControllerManagerFixture("") {} + + void prepare_controller() + { + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + test_controller_ = std::make_shared(); + cm_->add_controller(test_controller_, CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME); + ASSERT_NE(test_controller_, nullptr); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller_->get_state().id()); + } + + void configure_and_try_switch_that_returns_error() + { + // configure controller + cm_->configure_controller(CONTROLLER_NAME); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + + switch_test_controllers( + strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::ready, + controller_interface::return_type::ERROR); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + } + + void try_successful_switch() + { + switch_test_controllers( + strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout, + controller_interface::return_type::OK); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_->get_state().id()); + } + + std::shared_ptr test_controller_; }; TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) @@ -80,30 +126,92 @@ TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_ ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); } -TEST_P(TestControllerManagerWithTestableCM, when_starting_controller_without_hw_expect_error) +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description) { - ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); - auto controller_if = cm_->load_controller(CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME); - ASSERT_NE(controller_if, nullptr); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); - cm_->configure_controller(CONTROLLER_NAME); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); - - // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers - cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); - - switch_test_controllers( - strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout, - controller_interface::return_type::ERROR); - - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); - - // mimic callback - // auto msg = std_msgs::msg::String(); - // msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf; - // cm_->robot_description_callback(msg); - // ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); + prepare_controller(); + + // setup state interfaces to claim from controllers + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller_->set_state_interface_configuration(state_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); + + try_successful_switch(); +} + +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_with_robot_description) +{ + prepare_controller(); + + // setup command interfaces to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller_->set_command_interface_configuration(cmd_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); + + try_successful_switch(); +} + +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_after_some_time) +{ + prepare_controller(); + + // setup state interfaces to claim from controllers + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller_->set_state_interface_configuration(state_itfs_cfg); + + // setup command interfaces to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller_->set_command_interface_configuration(cmd_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); + + try_successful_switch(); } INSTANTIATE_TEST_SUITE_P( diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 1ecc3164b85..353f6738ab9 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -31,6 +31,7 @@ using ::testing::Return; using namespace std::chrono_literals; class TestLoadController : public ControllerManagerFixture { +public: void SetUp() override { ControllerManagerFixture::SetUp(); @@ -274,3 +275,82 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } + +class TestLoadControllerWithoutRobotDescription +: public ControllerManagerFixture +{ +public: + TestLoadControllerWithoutRobotDescription() + : ControllerManagerFixture("") + { + } + + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(10), + [&]() + { + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); + }); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + + rclcpp::TimerBase::SharedPtr robot_description_sending_timer_; + +protected: + rclcpp::TimerBase::SharedPtr update_timer_; + + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +TEST_F(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spawner_times_out) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 1) + << "could not spawn controller because not robot description and not services for controller " + "manager are active"; +} + +TEST_F( + TestLoadControllerWithoutRobotDescription, + controller_starting_with_later_load_of_robot_description) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + // Delay sending robot description + robot_description_sending_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(1500), [&]() { pass_robot_description_to_cm_and_rm(); }); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 1) + << "could not activate control because not robot description"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + { + auto ctrl_1 = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); + ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } +} diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index cac4c3fbde8..f446948182c 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -86,8 +86,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] urdf string containing the URDF. * \param[in] validate_interfaces boolean argument indicating whether the exported * interfaces ought to be validated. Defaults to true. + * \returns false if URDF validation has failed. */ - void load_urdf(const std::string & urdf, bool validate_interfaces = true); + bool load_urdf(const std::string & urdf, bool validate_interfaces = true); /** * @brief if the resource manager load_urdf(...) function has been called this returns true. @@ -404,7 +405,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager bool state_interface_exists(const std::string & key) const; private: - void validate_storage(const std::vector & hardware_info) const; + bool validate_storage(const std::vector & hardware_info) const; void release_command_interface(const std::string & key); diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 784610b676e..9203cde0835 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -60,7 +60,7 @@ auto trigger_and_print_hardware_state_transition = } else { - RCUTILS_LOG_INFO_NAMED( + RCUTILS_LOG_WARN_NAMED( "resource_manager", "Failed to '%s' hardware '%s'", transition_name.c_str(), hardware_name.c_str()); } @@ -433,6 +433,7 @@ class ResourceStorage template void import_state_interfaces(HardwareT & hardware) { + // TODO(destogl): add here exception catch block - this can otherwise crash the whole system auto interfaces = hardware.export_state_interfaces(); std::vector interface_names; interface_names.reserve(interfaces.size()); @@ -450,6 +451,7 @@ class ResourceStorage template void import_command_interfaces(HardwareT & hardware) { + // TODO(destogl): add here exception catch block - this can otherwise crash the whole system auto interfaces = hardware.export_command_interfaces(); hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); } @@ -551,7 +553,7 @@ class ResourceStorage } else { - RCUTILS_LOG_WARN_NAMED( + RCUTILS_LOG_ERROR_NAMED( "resource_manager", "Sensor hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); @@ -756,8 +758,10 @@ ResourceManager::ResourceManager( } // CM API: Called in "callback/slow"-thread -void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) +bool ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { + bool result = true; + is_urdf_loaded__ = true; const std::string system_type = "system"; const std::string sensor_type = "sensor"; @@ -786,13 +790,15 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac // throw on missing state and command interfaces, not specified keys are being ignored if (validate_interfaces) { - validate_storage(hardware_info); + result = validate_storage(hardware_info); } std::lock_guard guard(resources_lock_); read_write_status.failed_hardware_names.reserve( resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + resource_storage_->systems_.size()); + + return result; } bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } @@ -1426,7 +1432,7 @@ bool ResourceManager::state_interface_exists(const std::string & key) const // BEGIN: private methods -void ResourceManager::validate_storage( +bool ResourceManager::validate_storage( const std::vector & hardware_info) const { std::vector missing_state_keys = {}; @@ -1482,25 +1488,29 @@ void ResourceManager::validate_storage( if (!missing_state_keys.empty() || !missing_command_keys.empty()) { - std::string err_msg = "Wrong state or command interface configuration.\n"; - err_msg += "missing state interfaces:\n"; + std::string message = "Wrong state or command interface configuration.\n"; + message += "missing state interfaces:\n"; for (const auto & missing_key : missing_state_keys) { - err_msg += "' " + missing_key + " '" + "\t"; + message += "' " + missing_key + " '" + "\t"; } - err_msg += "\nmissing command interfaces:\n"; + message += "\nmissing command interfaces:\n"; for (const auto & missing_key : missing_command_keys) { - err_msg += "' " + missing_key + " '" + "\t"; + message += "' " + missing_key + " '" + "\t"; } RCUTILS_LOG_WARN_NAMED( "resource_manager", "Discrepancy between robot description file (urdf) and actually exported HW interfaces." - "If this is not intentional - fix it! If it is, Controller Manager will work correctly!" + "If this is not intentional - fix it! " "Details: %s", - err_msg.c_str()); + message.c_str()); + + return false; } + + return true; } // END: private methods diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 7b39935e753..6f3c6a77bc5 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -129,15 +129,15 @@ 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_urdf(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_urdf(ros2_control_test_assets::minimal_robot_missing_command_keys_urdf)); + ASSERT_TRUE(rm.is_urdf_already_loaded()); } } @@ -166,14 +166,6 @@ TEST_F(ResourceManagerTest, no_load_urdf_function_called) ASSERT_FALSE(rm.is_urdf_already_loaded()); } -TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_invalid) -{ - 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()); -} - TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_valid) { TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf);