From 26585af2095d67e592c757e65f056e0b1f4995a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 1 Feb 2024 17:03:37 +0100 Subject: [PATCH] Don't throw exception if something goes wrong with loading and initialization of the hardware components. --- controller_manager/src/controller_manager.cpp | 23 +- .../test_controller_manager_urdf_passing.cpp | 10 +- .../hardware_interface/resource_manager.hpp | 11 +- hardware_interface/src/resource_manager.cpp | 191 +++++++---- .../mock_components/test_generic_system.cpp | 5 +- .../test/test_components/test_actuator.cpp | 5 + .../test/test_components/test_sensor.cpp | 2 +- .../test/test_components/test_system.cpp | 16 + .../test/test_resource_manager.cpp | 91 ++++-- .../test/test_resource_manager.hpp | 5 +- .../ros2_control_test_assets/descriptions.hpp | 300 +++++++++++++++++- 11 files changed, 536 insertions(+), 123 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2b390101f9..82ee99bc4c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -286,7 +286,13 @@ ControllerManager::ControllerManager( "[Deprecated] Passing the robot description parameter directly to the control_manager node " "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead."); init_resource_manager(robot_description_); - init_services(); + if (resource_manager_->are_components_initialized()) + { + RCLCPP_FATAL( + get_logger(), + "You have to restart the framework when using robot description from parameter!"); + init_services(); + } } diagnostics_updater_.setHardwareID("ros2_control"); @@ -313,7 +319,7 @@ ControllerManager::ControllerManager( RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } - if (resource_manager_->is_urdf_already_loaded()) + if (resource_manager_->are_components_initialized()) { init_services(); } @@ -342,7 +348,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & RCLCPP_DEBUG( get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); robot_description_ = robot_description.data; - if (resource_manager_->is_urdf_already_loaded()) + if (resource_manager_->are_components_initialized()) { RCLCPP_WARN( get_logger(), @@ -351,7 +357,10 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & return; } init_resource_manager(robot_description_); - init_services(); + if (resource_manager_->are_components_initialized()) + { + init_services(); + } } void ControllerManager::init_resource_manager(const std::string & robot_description) @@ -360,9 +369,9 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript { 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..."); + "Could not load and initialize hardware. Please check previous output for more details. " + "After you have corrected your URDF, try to publish robot description again."); + return; } // Get all components and if they are not defined in parameters activate them automatically diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 2ea291d2a8..602078c292 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -63,29 +63,29 @@ class TestControllerManagerWithTestableCM TEST_P(TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called) { - ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); } TEST_P(TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback) { - ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); // mimic callback auto msg = std_msgs::msg::String(); msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); - ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); + ASSERT_TRUE(cm_->resource_manager_->are_components_initialized()); } TEST_P( TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_invalid_urdf_passed) { - ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); // 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()); + ASSERT_TRUE(cm_->resource_manager_->are_components_initialized()); } INSTANTIATE_TEST_SUITE_P( diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index b7b7a414bb..65605ffb74 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -69,8 +69,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * activated. Currently used only in tests. */ explicit ResourceManager( - const std::string & urdf, bool validate_interfaces = true, bool activate_all = false, - unsigned int update_rate = 100, + const std::string & urdf, bool activate_all = false, unsigned int update_rate = 100, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); ResourceManager(const ResourceManager &) = delete; @@ -90,9 +89,7 @@ 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_and_initialize_components( - const std::string & urdf, bool validate_interfaces = true, - bool load_and_initialize_components = true); + bool load_and_initialize_components(const std::string & urdf); /** * @brief if the resource manager load_and_initialize_components(...) function has been called @@ -102,7 +99,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * @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; + bool are_components_initialized() const; /// Claim a state interface given its key. /** @@ -424,7 +421,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 components_are_loaded_and_initialized_ = false; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 6e6f09632c..b19cddb73b 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -90,17 +90,29 @@ class ResourceStorage } template - void load_hardware( + bool load_hardware( const HardwareInfo & hardware_info, pluginlib::ClassLoader & loader, std::vector & container) { RCUTILS_LOG_INFO_NAMED( "resource_manager", "Loading hardware '%s' ", hardware_info.name.c_str()); // hardware_plugin_name has to match class name in plugin xml description - auto interface = std::unique_ptr( - loader.createUnmanagedInstance(hardware_info.hardware_plugin_name)); - HardwareT hardware(std::move(interface)); - container.emplace_back(std::move(hardware)); + try + { + auto interface = std::unique_ptr( + loader.createUnmanagedInstance(hardware_info.hardware_plugin_name)); + HardwareT hardware(std::move(interface)); + container.emplace_back(std::move(hardware)); + } + catch (const std::exception & ex) + { + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", + "The following exception was raised when creating instance of '%s' " + "from plugin '%s' component! %s", + hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str(), ex.what()); + return false; + } // initialize static data about hardware component to reduce later calls HardwareComponentInfo component_info; component_info.name = hardware_info.name; @@ -111,6 +123,8 @@ class ResourceStorage hardware_info_map_.insert(std::make_pair(component_info.name, component_info)); hardware_used_by_controllers_.insert( std::make_pair(component_info.name, std::vector())); + + return true; } template @@ -499,24 +513,15 @@ class ResourceStorage } } - void check_for_duplicates(const HardwareInfo & hardware_info) - { - // Check for identical names - if (hardware_info_map_.find(hardware_info.name) != hardware_info_map_.end()) - { - throw std::runtime_error( - "Hardware name " + hardware_info.name + - " is duplicated. Please provide a unique 'name' in the URDF."); - } - } - // TODO(destogl): Propagate "false" up, if happens in initialize_hardware - void load_and_initialize_actuator(const HardwareInfo & hardware_info) + bool load_and_initialize_actuator(const HardwareInfo & hardware_info) { auto load_and_init_actuators = [&](auto & container) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, actuator_loader_, container); + if (!load_hardware(hardware_info, actuator_loader_, container)) + { + return false; + } if (initialize_hardware(hardware_info, container.back())) { import_state_interfaces(container.back()); @@ -524,29 +529,33 @@ class ResourceStorage } else { - RCUTILS_LOG_WARN_NAMED( + RCUTILS_LOG_ERROR_NAMED( "resource_manager", "Actuator hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + return false; } + return true; }; if (hardware_info.is_async) { - load_and_init_actuators(async_actuators_); + return load_and_init_actuators(async_actuators_); } else { - load_and_init_actuators(actuators_); + return load_and_init_actuators(actuators_); } } - void load_and_initialize_sensor(const HardwareInfo & hardware_info) + bool load_and_initialize_sensor(const HardwareInfo & hardware_info) { auto load_and_init_sensors = [&](auto & container) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, sensor_loader_, container); + if (!load_hardware(hardware_info, sensor_loader_, container)) + { + return false; + } if (initialize_hardware(hardware_info, container.back())) { import_state_interfaces(container.back()); @@ -557,25 +566,29 @@ class ResourceStorage "resource_manager", "Sensor hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + return false; } + return true; }; if (hardware_info.is_async) { - load_and_init_sensors(async_sensors_); + return load_and_init_sensors(async_sensors_); } else { - load_and_init_sensors(sensors_); + return load_and_init_sensors(sensors_); } } - void load_and_initialize_system(const HardwareInfo & hardware_info) + bool load_and_initialize_system(const HardwareInfo & hardware_info) { auto load_and_init_systems = [&](auto & container) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, system_loader_, container); + if (!load_hardware(hardware_info, system_loader_, container)) + { + return false; + } if (initialize_hardware(hardware_info, container.back())) { import_state_interfaces(container.back()); @@ -583,20 +596,22 @@ class ResourceStorage } else { - RCUTILS_LOG_WARN_NAMED( + RCUTILS_LOG_ERROR_NAMED( "resource_manager", "System hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + return false; } + return true; }; if (hardware_info.is_async) { - load_and_init_systems(async_systems_); + return load_and_init_systems(async_systems_); } else { - load_and_init_systems(systems_); + return load_and_init_systems(systems_); } } @@ -689,6 +704,26 @@ class ResourceStorage } } + void clear() + { + actuators_.clear(); + sensors_.clear(); + systems_.clear(); + + async_actuators_.clear(); + async_sensors_.clear(); + async_systems_.clear(); + + hardware_info_map_.clear(); + state_interface_map_.clear(); + command_interface_map_.clear(); + + available_state_interfaces_.clear(); + available_command_interfaces_.clear(); + + claimed_command_interface_map_.clear(); + } + // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; @@ -740,11 +775,11 @@ ResourceManager::ResourceManager( ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager( - const std::string & urdf, bool validate_interfaces, bool activate_all, unsigned int update_rate, + const std::string & urdf, bool activate_all, unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) : resource_storage_(std::make_unique(update_rate, clock_interface)) { - load_and_initialize_components(urdf, validate_interfaces); + load_and_initialize_components(urdf); if (activate_all) { @@ -758,55 +793,83 @@ ResourceManager::ResourceManager( } // CM API: Called in "callback/slow"-thread -bool ResourceManager::load_and_initialize_components( - const std::string & urdf, bool validate_interfaces, bool load_and_initialize_components) +bool ResourceManager::load_and_initialize_components(const std::string & urdf) { - bool result = true; + components_are_loaded_and_initialized_ = true; + + const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); 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) { - for (const auto & individual_hardware_info : hardware_info) + // Check for identical names + if ( + resource_storage_->hardware_info_map_.find(individual_hardware_info.name) != + resource_storage_->hardware_info_map_.end()) { - if (individual_hardware_info.type == actuator_type) + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", + "Hardware name %s is duplicated. Please provide a unique 'name' " + "in the URDF.", + individual_hardware_info.name.c_str()); + components_are_loaded_and_initialized_ = false; + break; + } + + if (individual_hardware_info.type == actuator_type) + { + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + if (!resource_storage_->load_and_initialize_actuator(individual_hardware_info)) { - std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); - resource_storage_->load_and_initialize_actuator(individual_hardware_info); + components_are_loaded_and_initialized_ = false; + break; } - if (individual_hardware_info.type == sensor_type) + } + if (individual_hardware_info.type == sensor_type) + { + std::lock_guard guard(resource_interfaces_lock_); + if (!resource_storage_->load_and_initialize_sensor(individual_hardware_info)) { - std::lock_guard guard(resource_interfaces_lock_); - resource_storage_->load_and_initialize_sensor(individual_hardware_info); + components_are_loaded_and_initialized_ = false; + break; } - if (individual_hardware_info.type == system_type) + } + if (individual_hardware_info.type == system_type) + { + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + if (!resource_storage_->load_and_initialize_system(individual_hardware_info)) { - std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); - resource_storage_->load_and_initialize_system(individual_hardware_info); + components_are_loaded_and_initialized_ = false; + break; } } } - // throw on missing state and command interfaces, not specified keys are being ignored - if (validate_interfaces) + if (components_are_loaded_and_initialized_ && 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()); + } + else + { + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + resource_storage_->clear(); + // cleanup everything + components_are_loaded_and_initialized_ = false; } - 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; + return components_are_loaded_and_initialized_; } -bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded_; } +bool ResourceManager::are_components_initialized() const +{ + return components_are_loaded_and_initialized_; +} // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) @@ -1497,12 +1560,12 @@ bool ResourceManager::validate_storage( message += "missing state interfaces:\n"; for (const auto & missing_key : missing_state_keys) { - message += "' " + missing_key + " '" + "\t"; + message += " " + missing_key + "\t"; } message += "\nmissing command interfaces:\n"; for (const auto & missing_key : missing_command_keys) { - message += "' " + missing_key + " '" + "\t"; + message += " " + missing_key + "\t"; } RCUTILS_LOG_ERROR_NAMED( diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 0641cfda57..a6b578d6c5 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -630,9 +630,8 @@ class TestableResourceManager : public hardware_interface::ResourceManager 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) + explicit TestableResourceManager(const std::string & urdf, bool activate_all = false) + : hardware_interface::ResourceManager(urdf, activate_all) { } }; diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 41f27e201b..bb54ea9443 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -32,6 +32,11 @@ class TestActuator : public ActuatorInterface return CallbackReturn::ERROR; } + if (info_.joints[0].state_interfaces[1].name == "does_not_exist") + { + return CallbackReturn::ERROR; + } + /* * a hardware can optional prove for incorrect info here. * diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 4811244138..2c414aebb4 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -30,7 +30,7 @@ class TestSensor : public SensorInterface return CallbackReturn::ERROR; } // can only give feedback state for velocity - if (info_.sensors[0].state_interfaces.size() != 1) + if (info_.sensors[0].state_interfaces.size() == 2) { return CallbackReturn::ERROR; } diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index bc7a75df6f..8fb3098822 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -27,6 +27,22 @@ using hardware_interface::SystemInterface; class TestSystem : public SystemInterface { + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + // Simulating initialization error + if (info_.joints[0].state_interfaces[1].name == "does_not_exist") + { + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; + } + std::vector export_state_interfaces() override { std::vector state_interfaces; diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index d534c2488a..730410ddd7 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -106,17 +106,21 @@ TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation) // 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_and_initialize_components( - ros2_control_test_assets::minimal_unitilizable_robot_urdf, true)); + EXPECT_FALSE( + rm.load_and_initialize_components(ros2_control_test_assets::minimal_unitilizable_robot_urdf)); } -TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) +void test_load_and_initialized_components_failure(const std::string & urdf) { - // 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_and_initialize_components( - ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + ASSERT_FALSE(rm.load_and_initialize_components(urdf)); + + ASSERT_FALSE(rm.are_components_initialized()); + + // resource manage should also not have any components + EXPECT_EQ(rm.actuator_components_size(), 0); + EXPECT_EQ(rm.sensor_components_size(), 0); + EXPECT_EQ(rm.system_components_size(), 0); // test actuator EXPECT_FALSE(rm.state_interface_exists("joint1/position")); @@ -140,7 +144,15 @@ TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) EXPECT_FALSE(rm.command_interface_exists("joint3/max_acceleration")); } -TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) +TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) +{ + // 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 + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_unitilizable_robot_urdf); +} + +TEST_F(ResourceManagerTest, initialization_with_urdf_and_manual_validation) { // we validate the results manually TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); @@ -164,22 +176,15 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } -TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) +TEST_F(ResourceManagerTest, expect_validation_failure_if_not_all_interfaces_are_exported) { // missing state keys - { - 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()); - } + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf); + // missing command keys - { - 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()); - } + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_missing_command_keys_urdf); } TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) @@ -204,29 +209,53 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) TEST_F(ResourceManagerTest, no_load_and_initialize_components_function_called) { TestableResourceManager rm; - ASSERT_FALSE(rm.is_urdf_already_loaded()); + ASSERT_FALSE(rm.are_components_initialized()); } -TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_invalid) +TEST_F( + ResourceManagerTest, expect_load_and_initialize_to_fail_when_a_hw_component_plugin_does_not_exist) { - TestableResourceManager rm; - ASSERT_FALSE(rm.load_and_initialize_components( - ros2_control_test_assets::minimal_robot_missing_state_keys_urdf)); - ASSERT_FALSE(rm.is_urdf_already_loaded()); + // Actuator + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_not_existing_actuator_plugin); + + // Sensor + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_not_existing_sensors_plugin); + + // System + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_not_existing_system_plugin); +} + +TEST_F( + ResourceManagerTest, expect_load_and_initialize_to_fail_when_a_hw_component_initialization_fails) +{ + // Actuator + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_actuator_initialization_error); + + // Sensor + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_sensor_initialization_error); + + // System + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_system_initialization_error); } 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()); + ASSERT_TRUE(rm.are_components_initialized()); } TEST_F(ResourceManagerTest, can_load_and_initialize_components_later) { TestableResourceManager rm; - ASSERT_FALSE(rm.is_urdf_already_loaded()); + ASSERT_FALSE(rm.are_components_initialized()); rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf); - ASSERT_TRUE(rm.is_urdf_already_loaded()); + ASSERT_TRUE(rm.are_components_initialized()); } TEST_F(ResourceManagerTest, resource_claiming) @@ -345,7 +374,7 @@ class ExternalComponent : public hardware_interface::ActuatorInterface TEST_F(ResourceManagerTest, post_initialization_add_components) { // we validate the results manually - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available activate_components(rm); diff --git a/hardware_interface_testing/test/test_resource_manager.hpp b/hardware_interface_testing/test/test_resource_manager.hpp index c9bd17490c..72cf9e941b 100644 --- a/hardware_interface_testing/test/test_resource_manager.hpp +++ b/hardware_interface_testing/test/test_resource_manager.hpp @@ -52,9 +52,8 @@ class TestableResourceManager : public hardware_interface::ResourceManager 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) + explicit TestableResourceManager(const std::string & urdf, bool activate_all = false) + : hardware_interface::ResourceManager(urdf, activate_all) { } }; diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index a2f6195c67..713453bf62 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -225,7 +225,194 @@ const auto unitilizable_hardware_resources = )"; -const auto hardware_resources_missing_state_keys = +const auto hardware_resources_not_existing_actuator_plugin = + R"( + + + test_actuator23 + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + +const auto hardware_resources_not_existing_sensor_plugin = + R"( + + + test_actuator + + + + + + + + + + + test_sensor23 + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; +const auto hardware_resources_not_existing_system_plugin = + R"( + + + test_actuator + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system23 + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + +const auto hardware_resources_actuator_initializaion_error = + R"( + + + test_actuator + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + +)"; + +const auto hardware_resources_sensor_initializaion_error = R"( @@ -248,6 +435,47 @@ const auto hardware_resources_missing_state_keys = + + + test_system + 2 + 2 + + + + + + + + + + + + +)"; + +const auto hardware_resources_system_initializaion_error = + R"( + + + test_actuator + + + + + + + + + + test_sensor + 2 + 2 + + + + + test_system @@ -267,6 +495,52 @@ const auto hardware_resources_missing_state_keys = )"; +const auto hardware_resources_missing_state_keys = + R"( + + + test_actuator + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + +)"; + const auto hardware_resources_missing_command_keys = R"( @@ -298,13 +572,15 @@ const auto hardware_resources_missing_command_keys = - + + + )"; @@ -458,6 +734,26 @@ const auto minimal_robot_urdf = const auto minimal_unitilizable_robot_urdf = std::string(urdf_head) + std::string(unitilizable_hardware_resources) + std::string(urdf_tail); +const auto minimal_robot_not_existing_actuator_plugin = + std::string(urdf_head) + std::string(hardware_resources_not_existing_actuator_plugin) + + std::string(urdf_tail); +const auto minimal_robot_not_existing_sensors_plugin = + std::string(urdf_head) + std::string(hardware_resources_not_existing_sensor_plugin) + + std::string(urdf_tail); +const auto minimal_robot_not_existing_system_plugin = + std::string(urdf_head) + std::string(hardware_resources_not_existing_system_plugin) + + std::string(urdf_tail); + +const auto minimal_robot_actuator_initialization_error = + std::string(urdf_head) + std::string(hardware_resources_not_existing_actuator_plugin) + + std::string(urdf_tail); +const auto minimal_robot_sensor_initialization_error = + std::string(urdf_head) + std::string(hardware_resources_sensor_initializaion_error) + + std::string(urdf_tail); +const auto minimal_robot_system_initialization_error = + std::string(urdf_head) + std::string(hardware_resources_system_initializaion_error) + + std::string(urdf_tail); + const auto minimal_robot_missing_state_keys_urdf = std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) + std::string(urdf_tail);