From 510ba7309921339a3b4e40835efc5a7fcddb225f Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
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
---
 .../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<std::recursive_mutex> 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<std::recursive_mutex> 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);
+      }
     }
   }