fix lifecycle issue when reactivating

controller_manager/src/controller_manager.cpp

@@ -235,9 +235,6 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
       "future anymore. Use hardware_spawner instead.");
     resource_manager_->activate_all_components();
   }
-
-  // At this point all components are supposed to be stored in the resource storage's vector;
-  resource_manager_->allocate_threads_for_async_components();
 }
 
 void ControllerManager::init_services()

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -51,7 +51,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
   /// Default constructor for the Resource Manager.
   ResourceManager(
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr,
-    int update_rate = 100);
+    unsigned int update_rate = 100);
 
   /// Constructor for the Resource Manager.
   /**
@@ -396,14 +396,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    */
   bool state_interface_exists(const std::string & key) const;
 
-  /// Creates the background threads for all async components.
-  /**
-   * The threads get stored in the respective map based on the underlying component's type.
-   * Even though the callback is registered, and the thread is running by default, the read and write methods
-   * aren't called until the components get into active state.
-   */
-  void allocate_threads_for_async_components();
-
 private:
   void validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;
 
@@ -419,9 +411,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;
-
-  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_ = nullptr;
-  int cm_update_rate_;
 };
 
 }  // namespace hardware_interface

hardware_interface/src/resource_manager.cpp

@@ -76,13 +76,22 @@ class ResourceStorage
   static constexpr const char * system_interface_name = "hardware_interface::SystemInterface";
 
 public:
-  ResourceStorage()
+  // TODO(VX792): Change this when HW ifs get their own update rate,
+  // because the ResourceStorage really shouldn't know about the cm's parameters
+  ResourceStorage(
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr,
+    unsigned int update_rate = 100)
   : actuator_loader_(pkg_name, actuator_interface_name),
     sensor_loader_(pkg_name, sensor_interface_name),
-    system_loader_(pkg_name, system_interface_name)
+    system_loader_(pkg_name, system_interface_name),
+    clock_interface_(clock_interface),
+    cm_update_rate_(update_rate)
   {
   }
 
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
+  unsigned int cm_update_rate_;
+
   template <class HardwareT, class HardwareInterfaceT>
   void load_hardware(
     const HardwareInfo & hardware_info, pluginlib::ClassLoader<HardwareInterfaceT> & loader,
@@ -195,6 +204,36 @@ class ResourceStorage
             hardware.get_name().c_str(), interface.c_str());
         }
       }
+
+      if constexpr (std::is_same_v<hardware_interface::Actuator, HardwareT>)
+      {
+        if (hardware_info_map_[hardware.get_name()].is_async)
+        {
+          async_actuator_threads_.emplace(
+            std::piecewise_construct, std::forward_as_tuple(hardware.get_name()),
+            std::forward_as_tuple(hardware, cm_update_rate_, clock_interface_));
+        }
+      }
+
+      if constexpr (std::is_same_v<hardware_interface::System, HardwareT>)
+      {
+        if (hardware_info_map_[hardware.get_name()].is_async)
+        {
+          async_system_threads_.emplace(
+            std::piecewise_construct, std::forward_as_tuple(hardware.get_name()),
+            std::forward_as_tuple(hardware, cm_update_rate_, clock_interface_));
+        }
+      }
+
+      if constexpr (std::is_same_v<hardware_interface::Sensor, HardwareT>)
+      {
+        if (hardware_info_map_[hardware.get_name()].is_async)
+        {
+          async_sensor_threads_.emplace(
+            std::piecewise_construct, std::forward_as_tuple(hardware.get_name()),
+            std::forward_as_tuple(hardware, cm_update_rate_, clock_interface_));
+        }
+      }
     }
     return result;
   }
@@ -694,10 +733,8 @@ class ResourceStorage
 };
 
 ResourceManager::ResourceManager(
-  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, int update_rate)
-: resource_storage_(std::make_unique<ResourceStorage>()),
-  clock_interface_(clock_interface),
-  cm_update_rate_(update_rate)
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, unsigned int update_rate)
+: resource_storage_(std::make_unique<ResourceStorage>(clock_interface, update_rate))
 {
 }
 
@@ -706,7 +743,7 @@ ResourceManager::~ResourceManager() = default;
 ResourceManager::ResourceManager(
   const std::string & urdf, bool validate_interfaces, bool activate_all,
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
-: resource_storage_(std::make_unique<ResourceStorage>()), clock_interface_(clock_interface)
+: resource_storage_(std::make_unique<ResourceStorage>(clock_interface))
 {
   load_urdf(urdf, validate_interfaces);
 
@@ -1298,37 +1335,6 @@ bool ResourceManager::state_interface_exists(const std::string & key) const
 
 // END: "used only in tests and locally"
 
-void ResourceManager::allocate_threads_for_async_components()
-{
-  for (auto & component : resource_storage_->async_actuators_)
-  {
-    if (resource_storage_->hardware_info_map_[component.get_name()].is_async)
-    {
-      resource_storage_->async_actuator_threads_.emplace(
-        std::piecewise_construct, std::forward_as_tuple(component.get_name()),
-        std::forward_as_tuple(component, cm_update_rate_, clock_interface_));
-    }
-  }
-  for (auto & component : resource_storage_->async_sensors_)
-  {
-    if (resource_storage_->hardware_info_map_[component.get_name()].is_async)
-    {
-      resource_storage_->async_sensor_threads_.emplace(
-        std::piecewise_construct, std::forward_as_tuple(component.get_name()),
-        std::forward_as_tuple(component, cm_update_rate_, clock_interface_));
-    }
-  }
-  for (auto & component : resource_storage_->async_systems_)
-  {
-    if (resource_storage_->hardware_info_map_[component.get_name()].is_async)
-    {
-      resource_storage_->async_system_threads_.emplace(
-        std::piecewise_construct, std::forward_as_tuple(component.get_name()),
-        std::forward_as_tuple(component, cm_update_rate_, clock_interface_));
-    }
-  }
-}
-
 // BEGIN: private methods
 
 void ResourceManager::validate_storage(

joint_limits_interface/test/joint_limits_urdf_test.cpp

@@ -14,11 +14,12 @@
 
 /// \author Adolfo Rodriguez Tsouroukdissian
 
+#include <joint_limits_interface/joint_limits_urdf.hpp>
+
 #include <string>
 
 #include <gtest/gtest.h>
 
-#include <joint_limits_interface/joint_limits_urdf.hpp>
 
 class JointLimitsUrdfTest : public ::testing::Test
 {