Further Struct based Method changes to support propagation in gz_ros2_control (backport #2340) (#2407)

Author: mergify[bot]

controller_manager/src/controller_manager.cpp

@@ -621,7 +621,13 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
   {
     resource_manager_->import_joint_limiters(robot_description_);
   }
-  if (!resource_manager_->load_and_initialize_components(robot_description, update_rate_))
+  hardware_interface::ResourceManagerParams params;
+  params.robot_description = robot_description;
+  params.clock = trigger_clock_;
+  params.logger = this->get_logger();
+  params.executor = executor_;
+  params.update_rate = static_cast<unsigned int>(params_->update_rate);
+  if (!resource_manager_->load_and_initialize_components(params))
   {
     RCLCPP_WARN(
       get_logger(),

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -446,6 +446,57 @@ class ResourceManager
    * \param[in] system pointer to the system interface.
    * \param[in] hardware_info hardware info
    */
+  void import_component(
+    std::unique_ptr<SystemInterface> system, const HardwareComponentParams & params);
+
+  /// Import a hardware component which is not listed in the URDF
+  /**
+   * Components which are initialized outside a URDF can be added post initialization.
+   * Nevertheless, there should still be `HardwareInfo` available for this component,
+   * either parsed from a URDF string (easiest) or filled manually.
+   *
+   * \note this might invalidate existing state and command interfaces and should thus
+   * not be called when a controller is running.
+   * \note given that no hardware_info is available, the component has to be configured
+   * externally and prior to the call to import.
+   * \param[in] actuator pointer to the actuator interface.
+   * \param[in] params Struct of type HardwareComponentParams containing the hardware info
+   * and other parameters for the component.
+   */
+  void import_component(
+    std::unique_ptr<ActuatorInterface> actuator, const HardwareComponentParams & params);
+
+  /// Import a hardware component which is not listed in the URDF
+  /**
+   * Components which are initialized outside a URDF can be added post initialization.
+   * Nevertheless, there should still be `HardwareInfo` available for this component,
+   * either parsed from a URDF string (easiest) or filled manually.
+   *
+   * \note this might invalidate existing state and command interfaces and should thus
+   * not be called when a controller is running.
+   * \note given that no hardware_info is available, the component has to be configured
+   * externally and prior to the call to import.
+   * \param[in] sensor pointer to the sensor interface.
+   * \param[in] params Struct of type HardwareComponentParams containing the hardware info
+   * and other parameters for the component.
+   */
+  void import_component(
+    std::unique_ptr<SensorInterface> sensor, const HardwareComponentParams & params);
+
+  /// Import a hardware component which is not listed in the URDF
+  /**
+   * Components which are initialized outside a URDF can be added post initialization.
+   * Nevertheless, there should still be `HardwareInfo` available for this component,
+   * either parsed from a URDF string (easiest) or filled manually.
+   *
+   * \note this might invalidate existing state and command interfaces and should thus
+   * not be called when a controller is running.
+   * \note given that no hardware_info is available, the component has to be configured
+   * externally and prior to the call to import.
+   * \param[in] system pointer to the system interface.
+   * \param[in] params Struct of type HardwareComponentParams containing the hardware info
+   * and other parameters for the component.
+   */
   void import_component(
     std::unique_ptr<SystemInterface> system, const HardwareInfo & hardware_info);
 

hardware_interface/src/resource_manager.cpp

@@ -1347,6 +1347,7 @@ class ResourceStorage
   // Logger and Clock interfaces
   rclcpp::Clock::SharedPtr rm_clock_;
   rclcpp::Logger rm_logger_;
+  rclcpp::Executor::WeakPtr executor_;
 
   std::vector<Actuator> actuators_;
   std::vector<Sensor> sensors_;
@@ -1469,28 +1470,17 @@ bool ResourceManager::shutdown_components()
 // CM API: Called in "callback/slow"-thread
 bool ResourceManager::load_and_initialize_components(
   const std::string & urdf, const unsigned int update_rate)
-{
-  hardware_interface::ResourceManagerParams params;
-  params.robot_description = urdf;
-  params.update_rate = update_rate;
-  return load_and_initialize_components(params);
-}
-
-bool ResourceManager::load_and_initialize_components(
-  const hardware_interface::ResourceManagerParams & params)
 {
   components_are_loaded_and_initialized_ = true;
 
-  resource_storage_->robot_description_ = params.robot_description;
-  resource_storage_->cm_update_rate_ = params.update_rate;
+  resource_storage_->robot_description_ = urdf;
+  resource_storage_->cm_update_rate_ = update_rate;
 
-  auto hardware_info =
-    hardware_interface::parse_control_resources_from_urdf(params.robot_description);
+  auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
   // Set the update rate for all hardware components
   for (auto & hw : hardware_info)
   {
-    hw.rw_rate =
-      (hw.rw_rate == 0 || hw.rw_rate > params.update_rate) ? params.update_rate : hw.rw_rate;
+    hw.rw_rate = (hw.rw_rate == 0 || hw.rw_rate > update_rate) ? update_rate : hw.rw_rate;
   }
 
   const std::string system_type = "system";
@@ -1515,9 +1505,9 @@ bool ResourceManager::load_and_initialize_components(
     }
     hardware_interface::HardwareComponentParams interface_params;
     interface_params.hardware_info = individual_hardware_info;
-    interface_params.executor = params.executor;
-    interface_params.clock = params.clock;
-    interface_params.logger = params.logger;
+    interface_params.executor = resource_storage_->executor_;
+    interface_params.clock = resource_storage_->rm_clock_;
+    interface_params.logger = resource_storage_->rm_logger_;
 
     if (individual_hardware_info.type == actuator_type)
     {
@@ -1566,6 +1556,17 @@ bool ResourceManager::load_and_initialize_components(
   return components_are_loaded_and_initialized_;
 }
 
+bool ResourceManager::load_and_initialize_components(
+  const hardware_interface::ResourceManagerParams & params)
+{
+  resource_storage_->rm_clock_ = params.clock;
+  resource_storage_->rm_logger_ = params.logger;
+  resource_storage_->robot_description_ = params.robot_description;
+  resource_storage_->cm_update_rate_ = params.update_rate;
+  resource_storage_->executor_ = params.executor;
+  return load_and_initialize_components(params.robot_description, params.update_rate);
+}
+
 void ResourceManager::import_joint_limiters(const std::string & urdf)
 {
   std::lock_guard<std::recursive_mutex> guard(joint_limiters_lock_);
@@ -1894,29 +1895,53 @@ std::string ResourceManager::get_command_interface_data_type(const std::string &
 
 void ResourceManager::import_component(
   std::unique_ptr<ActuatorInterface> actuator, const HardwareInfo & hardware_info)
+{
+  HardwareComponentParams params;
+  params.hardware_info = hardware_info;
+  import_component(std::move(actuator), params);
+}
+
+void ResourceManager::import_component(
+  std::unique_ptr<SensorInterface> sensor, const HardwareInfo & hardware_info)
+{
+  HardwareComponentParams params;
+  params.hardware_info = hardware_info;
+  import_component(std::move(sensor), params);
+}
+
+void ResourceManager::import_component(
+  std::unique_ptr<SystemInterface> system, const HardwareInfo & hardware_info)
+{
+  HardwareComponentParams params;
+  params.hardware_info = hardware_info;
+  import_component(std::move(system), params);
+}
+
+void ResourceManager::import_component(
+  std::unique_ptr<ActuatorInterface> actuator, const HardwareComponentParams & params)
 {
   std::lock_guard<std::recursive_mutex> guard(resources_lock_);
-  resource_storage_->initialize_actuator(std::move(actuator), hardware_info);
+  resource_storage_->initialize_actuator(std::move(actuator), params);
   read_write_status.failed_hardware_names.reserve(
     resource_storage_->actuators_.size() + resource_storage_->sensors_.size() +
     resource_storage_->systems_.size());
 }
 
 void ResourceManager::import_component(
-  std::unique_ptr<SensorInterface> sensor, const HardwareInfo & hardware_info)
+  std::unique_ptr<SensorInterface> sensor, const HardwareComponentParams & params)
 {
   std::lock_guard<std::recursive_mutex> guard(resources_lock_);
-  resource_storage_->initialize_sensor(std::move(sensor), hardware_info);
+  resource_storage_->initialize_sensor(std::move(sensor), params);
   read_write_status.failed_hardware_names.reserve(
     resource_storage_->actuators_.size() + resource_storage_->sensors_.size() +
     resource_storage_->systems_.size());
 }
 
 void ResourceManager::import_component(
-  std::unique_ptr<SystemInterface> system, const HardwareInfo & hardware_info)
+  std::unique_ptr<SystemInterface> system, const HardwareComponentParams & params)
 {
   std::lock_guard<std::recursive_mutex> guard(resources_lock_);
-  resource_storage_->initialize_system(std::move(system), hardware_info);
+  resource_storage_->initialize_system(std::move(system), params);
   read_write_status.failed_hardware_names.reserve(
     resource_storage_->actuators_.size() + resource_storage_->sensors_.size() +
     resource_storage_->systems_.size());