From c4c060f72d76fb7d5433df37b82c16d855a6a89d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jan 2024 22:03:09 +0100 Subject: [PATCH 1/4] check the components size instead of the variable (fixes #1299) --- hardware_interface/src/resource_manager.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 0cbb620c76..469c576f5d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -795,7 +795,12 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac resource_storage_->systems_.size()); } -bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } +bool ResourceManager::is_urdf_already_loaded() const +{ + return ( + !resource_storage_->actuators_.empty() || !resource_storage_->sensors_.empty() || + !resource_storage_->systems_.empty()); +} // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) From a43d04c0c6cc48fde6f96ba1ec21d003d6bded63 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jan 2024 22:07:46 +0100 Subject: [PATCH 2/4] remove is_urdf_loaded variable --- .../include/hardware_interface/resource_manager.hpp | 2 -- hardware_interface/src/resource_manager.cpp | 1 - 2 files changed, 3 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 146b903f46..6bde1ca5d6 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -418,8 +418,6 @@ 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; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 469c576f5d..bca0617d8f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -758,7 +758,6 @@ ResourceManager::ResourceManager( // CM API: Called in "callback/slow"-thread void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { - is_urdf_loaded__ = true; const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; From 4224d372da605f01cfc44ffb3d32208d5fb87f6c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Jan 2024 10:12:21 +0100 Subject: [PATCH 3/4] Revert "remove is_urdf_loaded variable" This reverts commit a43d04c0c6cc48fde6f96ba1ec21d003d6bded63. --- .../include/hardware_interface/resource_manager.hpp | 2 ++ hardware_interface/src/resource_manager.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 6bde1ca5d6..146b903f46 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -418,6 +418,8 @@ 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; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index bca0617d8f..469c576f5d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -758,6 +758,7 @@ ResourceManager::ResourceManager( // CM API: Called in "callback/slow"-thread void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { + is_urdf_loaded__ = true; const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; From 89ec65dbff4b4c25dc34b3ffd3d9585936e97389 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Jan 2024 10:26:35 +0100 Subject: [PATCH 4/4] add new load_and_initialize_components default argument to the load_urdf method --- .../hardware_interface/resource_manager.hpp | 6 ++- hardware_interface/src/resource_manager.cpp | 41 +++++++++---------- 2 files changed, 25 insertions(+), 22 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 469c576f5d..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); + } } } @@ -795,12 +799,7 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac resource_storage_->systems_.size()); } -bool ResourceManager::is_urdf_already_loaded() const -{ - return ( - !resource_storage_->actuators_.empty() || !resource_storage_->sensors_.empty() || - !resource_storage_->systems_.empty()); -} +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)