From d8cb7b9d9855697ac06416d70719008391d74c7b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Jan 2024 19:33:58 +0100 Subject: [PATCH] [RM] Fix crash for missing urdf in resource manager (#1301) * check the components size instead of the variable (fixes #1299) * add new load_and_initialize_components default argument to the load_urdf method (cherry picked from commit 510ba7309921339a3b4e40835efc5a7fcddb225f) --- .../hardware_interface/resource_manager.hpp | 6 +++- hardware_interface/src/resource_manager.cpp | 34 +++++++++++-------- 2 files changed, 24 insertions(+), 16 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 146b903f46..743e548a4c 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -86,8 +86,12 @@ 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. + * \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(const std::string & urdf, bool validate_interfaces = true); + void load_urdf( + 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. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 0cbb620c76..2e8ccc7b1f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -756,7 +756,8 @@ ResourceManager::ResourceManager( } // CM API: Called in "callback/slow"-thread -void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) +void ResourceManager::load_urdf( + const std::string & urdf, bool validate_interfaces, bool load_and_initialize_components) { is_urdf_loaded__ = true; const std::string system_type = "system"; @@ -764,22 +765,25 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac const std::string actuator_type = "actuator"; const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); - for (const auto & individual_hardware_info : hardware_info) + if (load_and_initialize_components) { - if (individual_hardware_info.type == actuator_type) + for (const auto & individual_hardware_info : hardware_info) { - std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); - resource_storage_->load_and_initialize_actuator(individual_hardware_info); - } - if (individual_hardware_info.type == sensor_type) - { - std::lock_guard guard(resource_interfaces_lock_); - resource_storage_->load_and_initialize_sensor(individual_hardware_info); - } - if (individual_hardware_info.type == system_type) - { - std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); - resource_storage_->load_and_initialize_system(individual_hardware_info); + if (individual_hardware_info.type == actuator_type) + { + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + resource_storage_->load_and_initialize_actuator(individual_hardware_info); + } + if (individual_hardware_info.type == sensor_type) + { + std::lock_guard guard(resource_interfaces_lock_); + resource_storage_->load_and_initialize_sensor(individual_hardware_info); + } + if (individual_hardware_info.type == system_type) + { + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + resource_storage_->load_and_initialize_system(individual_hardware_info); + } } }