[RM] Fix crash for missing urdf in resource manager (ros-controls#1301)

* check the components size instead of the variable (fixes ros-controls#1299) * add new load_and_initialize_components default argument to the load_urdf method
saikishor · Jan 18, 2024 · b1f61e7 · b1f61e7
1 parent a3bf280
commit b1f61e7
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 18 deletions.
diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp
@@ -80,8 +80,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
@@ -676,32 +676,36 @@ 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";
   const std::string sensor_type = "sensor";
   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::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
-      std::lock_guard<std::recursive_mutex> guard_claimed(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::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
-      std::lock_guard<std::recursive_mutex> guard_claimed(claimed_command_interfaces_lock_);
-      resource_storage_->load_and_initialize_system(individual_hardware_info);
+      if (individual_hardware_info.type == actuator_type)
+      {
+        std::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
+        std::lock_guard<std::recursive_mutex> guard_claimed(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::lock_guard<std::recursive_mutex> guard(resource_interfaces_lock_);
+        std::lock_guard<std::recursive_mutex> guard_claimed(claimed_command_interfaces_lock_);
+        resource_storage_->load_and_initialize_system(individual_hardware_info);
+      }
     }
   }