Shift to Struct based Method and Constructors, with Executor passed from CM to on_init() (ros-controls#2323)

Author: soham2560

controller_manager/src/controller_manager.cpp

@@ -410,8 +410,14 @@ ControllerManager::ControllerManager(
   robot_description_(urdf)
 {
   initialize_parameters();
-  resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(
-    urdf, trigger_clock_, this->get_logger(), activate_all_hw_components, params_->update_rate);
+  hardware_interface::ResourceManagerParams params;
+  params.robot_description = urdf;
+  params.clock = trigger_clock_;
+  params.logger = this->get_logger();
+  params.activate_all = activate_all_hw_components;
+  params.update_rate = static_cast<unsigned int>(params_->update_rate);
+  params.executor = executor_;
+  resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(params, true);
   init_controller_manager();
 }
 

hardware_interface/include/hardware_interface/actuator.hpp

@@ -52,6 +52,9 @@ class Actuator final
   const rclcpp_lifecycle::State & initialize(
     const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock);
 
+  const rclcpp_lifecycle::State & initialize(
+    const hardware_interface::HardwareComponentParams & params);
+
   const rclcpp_lifecycle::State & configure();
 
   const rclcpp_lifecycle::State & cleanup();

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -28,6 +28,8 @@
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/introspection.hpp"
+#include "hardware_interface/types/hardware_component_interface_params.hpp"
+#include "hardware_interface/types/hardware_component_params.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "hardware_interface/types/trigger_type.hpp"
@@ -124,9 +126,32 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
   CallbackReturn init(
     const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
   {
-    actuator_clock_ = clock;
-    actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name);
-    info_ = hardware_info;
+    hardware_interface::HardwareComponentParams params;
+    params.hardware_info = hardware_info;
+    params.clock = clock;
+    params.logger = logger;
+    return init(params);
+  };
+
+  /// Initialization of the hardware interface from data parsed from the robot's URDF and also the
+  /// clock and logger interfaces.
+  /**
+   * \param[in] params  A struct of type HardwareComponentParams containing all necessary
+   * parameters for initializing this specific hardware component,
+   * including its HardwareInfo, a dedicated logger, a clock, and a
+   * weak_ptr to the executor.
+   * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks
+   * such as `spin()`.
+   * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
+   * \returns CallbackReturn::ERROR if any error happens or data are missing.
+   */
+  CallbackReturn init(const hardware_interface::HardwareComponentParams & params)
+  {
+    actuator_clock_ = params.clock;
+    auto logger_copy = params.logger;
+    actuator_logger_ =
+      logger_copy.get_child("hardware_component.actuator." + params.hardware_info.name);
+    info_ = params.hardware_info;
     if (info_.is_async)
     {
       RCLCPP_INFO_STREAM(
@@ -158,7 +183,10 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
         info_.thread_priority);
       async_handler_->start_thread();
     }
-    return on_init(hardware_info);
+    hardware_interface::HardwareComponentInterfaceParams interface_params;
+    interface_params.hardware_info = info_;
+    interface_params.executor = params.executor;
+    return on_init(interface_params);
   };
 
   /// Initialization of the hardware interface from data parsed from the robot's URDF.
@@ -175,6 +203,22 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
     return CallbackReturn::SUCCESS;
   };
 
+  /// Initialization of the hardware interface from data parsed from the robot's URDF.
+  /**
+   * \param[in] params  A struct of type hardware_interface::HardwareComponentInterfaceParams
+   * containing all necessary parameters for initializing this specific hardware component,
+   * specifically its HardwareInfo, and a weak_ptr to the executor.
+   * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks
+   * such as `spin()`.
+   * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
+   * \returns CallbackReturn::ERROR if any error happens or data are missing.
+   */
+  virtual CallbackReturn on_init(
+    const hardware_interface::HardwareComponentInterfaceParams & params)
+  {
+    return on_init(params.hardware_info);
+  };
+
   /// Exports all state interfaces for this hardware interface.
   /**
    * Old way of exporting the StateInterfaces. If a empty vector is returned then

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -32,7 +32,9 @@
 #include "hardware_interface/system.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "hardware_interface/types/resource_manager_params.hpp"
 #include "rclcpp/duration.hpp"
+#include "rclcpp/executor.hpp"
 #include "rclcpp/node_interfaces/node_logging_interface.hpp"
 #include "rclcpp/time.hpp"
 
@@ -98,6 +100,19 @@ class ResourceManager
     const std::string & urdf, rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger,
     bool activate_all = false, const unsigned int update_rate = 100);
 
+  /// Constructor for the Resource Manager.
+  /**
+   * The implementation uses the ResourceManagerParams to load the specified urdf and initializes
+   * the hardware components listed within as well as populate their respective state and command
+   * interfaces.
+   *
+   * \param[in] params ResourceManagerParams containing the parameters for the ResourceManager.
+   * \param[in] load boolean argument indicating if the components should be loaded and
+   * initialized. If false, the ResourceManager will not load any components and will only
+   * initialize the ResourceManager with the given parameters.
+   */
+  explicit ResourceManager(const hardware_interface::ResourceManagerParams & params, bool load);
+
   ResourceManager(const ResourceManager &) = delete;
 
   virtual ~ResourceManager();
@@ -122,6 +137,19 @@ class ResourceManager
   virtual bool load_and_initialize_components(
     const std::string & urdf, const unsigned int update_rate = 100);
 
+  /// Load resources from on a given URDF.
+  /**
+   * The resource manager can be post-initialized with a given URDF.
+   * This is mainly used in conjunction with the default constructor
+   * in which the URDF might not be present at first initialization.
+   *
+   * \param[in] urdf string containing the URDF.
+   * \param[in] update_rate update rate of  the main control loop, i.e., of the controller manager.
+   * \returns false if URDF validation has failed.
+   */
+  virtual bool load_and_initialize_components(
+    const hardware_interface::ResourceManagerParams & params);
+
   /**
    * @brief Import joint limiters from the URDF.
    * @param urdf string containing the URDF.
@@ -553,6 +581,13 @@ class ResourceManager
 
   void release_command_interface(const std::string & key);
 
+  // Note this was added in #2323 and is a temporary addition to be backwards compatible with the
+  // original constructors. This is planned to be removed in a future PR along with the
+  // aforementioned constructors.
+  hardware_interface::ResourceManagerParams constructParams(
+    rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger, const std::string & urdf = std::string(),
+    bool activate_all = false, unsigned int update_rate = 100);
+
   std::unordered_map<std::string, bool> claimed_command_interface_map_;
 
   std::unique_ptr<ResourceStorage> resource_storage_;

hardware_interface/include/hardware_interface/sensor.hpp

@@ -52,6 +52,9 @@ class Sensor final
   const rclcpp_lifecycle::State & initialize(
     const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock);
 
+  const rclcpp_lifecycle::State & initialize(
+    const hardware_interface::HardwareComponentParams & params);
+
   const rclcpp_lifecycle::State & configure();
 
   const rclcpp_lifecycle::State & cleanup();

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -28,6 +28,8 @@
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/introspection.hpp"
+#include "hardware_interface/types/hardware_component_interface_params.hpp"
+#include "hardware_interface/types/hardware_component_params.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
@@ -108,9 +110,32 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   CallbackReturn init(
     const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
   {
-    sensor_clock_ = clock;
-    sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name);
-    info_ = hardware_info;
+    hardware_interface::HardwareComponentParams params;
+    params.hardware_info = hardware_info;
+    params.clock = clock;
+    params.logger = logger;
+    return init(params);
+  };
+
+  /// Initialization of the hardware interface from data parsed from the robot's URDF and also the
+  /// clock and logger interfaces.
+  /**
+   * \param[in] params  A struct of type HardwareComponentParams containing all necessary
+   * parameters for initializing this specific hardware component,
+   * including its HardwareInfo, a dedicated logger, a clock, and a
+   * weak_ptr to the executor.
+   * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks
+   * such as `spin()`.
+   * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
+   * \returns CallbackReturn::ERROR if any error happens or data are missing.
+   */
+  CallbackReturn init(const hardware_interface::HardwareComponentParams & params)
+  {
+    sensor_clock_ = params.clock;
+    auto logger_copy = params.logger;
+    sensor_logger_ =
+      logger_copy.get_child("hardware_component.sensor." + params.hardware_info.name);
+    info_ = params.hardware_info;
     if (info_.is_async)
     {
       RCLCPP_INFO_STREAM(
@@ -121,7 +146,10 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
         info_.thread_priority);
       read_async_handler_->start_thread();
     }
-    return on_init(hardware_info);
+    hardware_interface::HardwareComponentInterfaceParams interface_params;
+    interface_params.hardware_info = info_;
+    interface_params.executor = params.executor;
+    return on_init(interface_params);
   };
 
   /// Initialization of the hardware interface from data parsed from the robot's URDF.
@@ -138,6 +166,22 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     return CallbackReturn::SUCCESS;
   };
 
+  /// Initialization of the hardware interface from data parsed from the robot's URDF.
+  /**
+   * \param[in] params  A struct of type hardware_interface::HardwareComponentInterfaceParams
+   * containing all necessary parameters for initializing this specific hardware component,
+   * specifically its HardwareInfo, and a weak_ptr to the executor.
+   * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks
+   * such as `spin()`.
+   * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
+   * \returns CallbackReturn::ERROR if any error happens or data are missing.
+   */
+  virtual CallbackReturn on_init(
+    const hardware_interface::HardwareComponentInterfaceParams & params)
+  {
+    return on_init(params.hardware_info);
+  };
+
   /// Exports all state interfaces for this hardware interface.
   /**
    * Old way of exporting the StateInterfaces. If a empty vector is returned then

hardware_interface/include/hardware_interface/system.hpp

@@ -52,6 +52,9 @@ class System final
   const rclcpp_lifecycle::State & initialize(
     const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock);
 
+  const rclcpp_lifecycle::State & initialize(
+    const hardware_interface::HardwareComponentParams & params);
+
   const rclcpp_lifecycle::State & configure();
 
   const rclcpp_lifecycle::State & cleanup();

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -28,6 +28,8 @@
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/introspection.hpp"
+#include "hardware_interface/types/hardware_component_interface_params.hpp"
+#include "hardware_interface/types/hardware_component_params.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
@@ -128,9 +130,32 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   CallbackReturn init(
     const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
   {
-    system_clock_ = clock;
-    system_logger_ = logger.get_child("hardware_component.system." + hardware_info.name);
-    info_ = hardware_info;
+    hardware_interface::HardwareComponentParams params;
+    params.hardware_info = hardware_info;
+    params.clock = clock;
+    params.logger = logger;
+    return init(params);
+  };
+
+  /// Initialization of the hardware interface from data parsed from the robot's URDF and also the
+  /// clock and logger interfaces.
+  /**
+   * \param[in] params  A struct of type HardwareComponentParams containing all necessary
+   * parameters for initializing this specific hardware component,
+   * including its HardwareInfo, a dedicated logger, a clock, and a
+   * weak_ptr to the executor.
+   * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks
+   * such as `spin()`.
+   * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
+   * \returns CallbackReturn::ERROR if any error happens or data are missing.
+   */
+  CallbackReturn init(const hardware_interface::HardwareComponentParams & params)
+  {
+    system_clock_ = params.clock;
+    auto logger_copy = params.logger;
+    system_logger_ =
+      logger_copy.get_child("hardware_component.system." + params.hardware_info.name);
+    info_ = params.hardware_info;
     if (info_.is_async)
     {
       RCLCPP_INFO_STREAM(
@@ -162,7 +187,10 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
         info_.thread_priority);
       async_handler_->start_thread();
     }
-    return on_init(hardware_info);
+    hardware_interface::HardwareComponentInterfaceParams interface_params;
+    interface_params.hardware_info = info_;
+    interface_params.executor = params.executor;
+    return on_init(interface_params);
   };
 
   /// Initialization of the hardware interface from data parsed from the robot's URDF.
@@ -182,6 +210,22 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
     return CallbackReturn::SUCCESS;
   };
 
+  /// Initialization of the hardware interface from data parsed from the robot's URDF.
+  /**
+   * \param[in] params  A struct of type hardware_interface::HardwareComponentInterfaceParams
+   * containing all necessary parameters for initializing this specific hardware component,
+   * specifically its HardwareInfo, and a weak_ptr to the executor.
+   * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks
+   * such as `spin()`.
+   * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
+   * \returns CallbackReturn::ERROR if any error happens or data are missing.
+   */
+  virtual CallbackReturn on_init(
+    const hardware_interface::HardwareComponentInterfaceParams & params)
+  {
+    return on_init(params.hardware_info);
+  };
+
   /// Exports all state interfaces for this hardware interface.
   /**
    * Old way of exporting the StateInterfaces. If a empty vector is returned then