Addition of a Default Node for Hardware Component (#2348)

Author: soham2560

hardware_interface/doc/writing_new_hardware_component.rst

@@ -48,6 +48,59 @@ The following is a step-by-step guide to create source files, basic tests, and c
       In the first line usually the parents ``on_init`` is called to process standard values, like name. This is done using: ``hardware_interface::(Actuator|Sensor|System)Interface::on_init(info)``.
       If all required parameters are set and valid and everything works fine return ``CallbackReturn::SUCCESS`` or ``return CallbackReturn::ERROR`` otherwise.
 
+      #. **(Optional) Adding Publisher, Services, etc.**
+
+         A common requirement for a hardware component is to publish status or diagnostic information without interfering with the real-time control loop.
+
+         This allows you to add any standard ROS 2 component (publishers, subscribers, services, timers) to your hardware interface without compromising real-time performance. There are two primary ways to achieve this.
+
+         **Method 1: Using the Framework-Managed Node (Recommended & Simplest)**
+
+         The framework internally creates a dedicated ROS 2 node for each hardware component. Your hardware plugin can then get a handle to this node and use it.
+
+         #. **Access and using the Default Node**: You can get a ``shared_ptr`` to the node by calling the ``get_node()`` method and use it just like any other ``rclcpp::Node::SharedPtr`` to create publishers, timers, etc.
+
+            .. code-block:: cpp
+
+               // Continuing inside on_configure()
+               if (get_node())
+               {
+                  my_publisher_ = get_node()->create_publisher<std_msgs::msg::String>("~/status", 10);
+
+                  using namespace std::chrono_literals;
+                  my_timer_ = get_node()->create_wall_timer(1s, [this]() {
+                     std_msgs::msg::String msg;
+                     msg.data = "Hardware status update!";
+                     my_publisher_->publish(msg);
+                  });
+               }
+
+         **Method 2: Using the Executor from `HardwareComponentParams`**
+
+         For more advanced use cases where you need direct control over node creation, the ``on_init`` method can be configured to receive a ``HardwareComponentParams`` struct. This struct contains a ``weak_ptr`` to the ``ControllerManager``'s executor.
+
+         #. **Update ``on_init`` Signature**: First, your hardware interface must override the ``on_init`` version that takes ``HardwareComponentParams``.
+
+            .. code-block:: cpp
+
+               // In your <robot_hardware_interface_name>.hpp
+               hardware_interface::CallbackReturn on_init(
+               const hardware_interface::HardwareComponentParams & params) override;
+
+         #. **Lock and Use the Executor**: Inside ``on_init``, you must safely "lock" the ``weak_ptr`` to get a usable ``shared_ptr``. You can then create your own node and add it to the executor.
+
+            .. code-block:: cpp
+
+               // In your <robot_hardware_interface_name>.cpp, inside on_init(params)
+               if (auto locked_executor = params.executor.lock())
+               {
+                  my_custom_node_ = std::make_shared<rclcpp::Node>("my_custom_node");
+                  locked_executor->add_node(my_custom_node_->get_node_base_interface());
+                  // ... create publishers/timers on my_custom_node_ ...
+               }
+
+         For a complete, working implementation that uses the framework-managed node to publish diagnostic messages, see the demo in example 17.
+
    #. Write the ``on_configure`` method where you usually setup the communication to the hardware and set everything up so that the hardware can be activated.
 
    #. Implement ``on_cleanup`` method, which does the opposite of ``on_configure``.

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -186,6 +186,26 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
         info_.thread_priority);
       async_handler_->start_thread();
     }
+
+    if (auto locked_executor = params.executor.lock())
+    {
+      std::string node_name = params.hardware_info.name;
+      std::transform(
+        node_name.begin(), node_name.end(), node_name.begin(),
+        [](unsigned char c) { return std::tolower(c); });
+      std::replace(node_name.begin(), node_name.end(), '/', '_');
+      hardware_component_node_ = std::make_shared<rclcpp::Node>(node_name);
+      locked_executor->add_node(hardware_component_node_->get_node_base_interface());
+    }
+    else
+    {
+      RCLCPP_WARN(
+        params.logger,
+        "Executor is not available during hardware component initialization for '%s'. Skipping "
+        "node creation!",
+        params.hardware_info.name.c_str());
+    }
+
     hardware_interface::HardwareComponentInterfaceParams interface_params;
     interface_params.hardware_info = info_;
     interface_params.executor = params.executor;
@@ -646,6 +666,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
    */
   rclcpp::Clock::SharedPtr get_clock() const { return actuator_clock_; }
 
+  /// Get the default node of the ActuatorInterface.
+  /**
+   * \return node of the ActuatorInterface.
+   */
+  rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; }
+
   /// Get the hardware info of the ActuatorInterface.
   /**
    * \return hardware info of the ActuatorInterface.
@@ -702,6 +728,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
 private:
   rclcpp::Clock::SharedPtr actuator_clock_;
   rclcpp::Logger actuator_logger_;
+  rclcpp::Node::SharedPtr hardware_component_node_ = nullptr;
   // interface names to Handle accessed through getters/setters
   std::unordered_map<std::string, StateInterface::SharedPtr> actuator_states_;
   std::unordered_map<std::string, CommandInterface::SharedPtr> actuator_commands_;

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -149,6 +149,26 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
         info_.thread_priority);
       read_async_handler_->start_thread();
     }
+
+    if (auto locked_executor = params.executor.lock())
+    {
+      std::string node_name = params.hardware_info.name;
+      std::transform(
+        node_name.begin(), node_name.end(), node_name.begin(),
+        [](unsigned char c) { return std::tolower(c); });
+      std::replace(node_name.begin(), node_name.end(), '/', '_');
+      hardware_component_node_ = std::make_shared<rclcpp::Node>(node_name);
+      locked_executor->add_node(hardware_component_node_->get_node_base_interface());
+    }
+    else
+    {
+      RCLCPP_WARN(
+        params.logger,
+        "Executor is not available during hardware component initialization for '%s'. Skipping "
+        "node creation!",
+        params.hardware_info.name.c_str());
+    }
+
     hardware_interface::HardwareComponentInterfaceParams interface_params;
     interface_params.hardware_info = info_;
     interface_params.executor = params.executor;
@@ -419,6 +439,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   rclcpp::Clock::SharedPtr get_clock() const { return sensor_clock_; }
 
+  /// Get the default node of the ActuatorInterface.
+  /**
+   * \return node of the ActuatorInterface.
+   */
+  rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; }
+
   /// Get the hardware info of the SensorInterface.
   /**
    * \return hardware info of the SensorInterface.
@@ -459,6 +485,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
 private:
   rclcpp::Clock::SharedPtr sensor_clock_;
   rclcpp::Logger sensor_logger_;
+  rclcpp::Node::SharedPtr hardware_component_node_ = nullptr;
   // interface names to Handle accessed through getters/setters
   std::unordered_map<std::string, StateInterface::SharedPtr> sensor_states_map_;
 

hardware_interface/include/hardware_interface/system_interface.hpp

@@ -190,6 +190,26 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
         info_.thread_priority);
       async_handler_->start_thread();
     }
+
+    if (auto locked_executor = params.executor.lock())
+    {
+      std::string node_name = params.hardware_info.name;
+      std::transform(
+        node_name.begin(), node_name.end(), node_name.begin(),
+        [](unsigned char c) { return std::tolower(c); });
+      std::replace(node_name.begin(), node_name.end(), '/', '_');
+      hardware_component_node_ = std::make_shared<rclcpp::Node>(node_name);
+      locked_executor->add_node(hardware_component_node_->get_node_base_interface());
+    }
+    else
+    {
+      RCLCPP_WARN(
+        params.logger,
+        "Executor is not available during hardware component initialization for '%s'. Skipping "
+        "node creation!",
+        params.hardware_info.name.c_str());
+    }
+
     hardware_interface::HardwareComponentInterfaceParams interface_params;
     interface_params.hardware_info = info_;
     interface_params.executor = params.executor;
@@ -677,6 +697,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
    */
   rclcpp::Clock::SharedPtr get_clock() const { return system_clock_; }
 
+  /// Get the default node of the ActuatorInterface.
+  /**
+   * \return node of the ActuatorInterface.
+   */
+  rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; }
+
   /// Get the hardware info of the SystemInterface.
   /**
    * \return hardware info of the SystemInterface.
@@ -743,6 +769,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
 private:
   rclcpp::Clock::SharedPtr system_clock_;
   rclcpp::Logger system_logger_;
+  rclcpp::Node::SharedPtr hardware_component_node_ = nullptr;
   // interface names to Handle accessed through getters/setters
   std::unordered_map<std::string, StateInterface::SharedPtr> system_states_;
   std::unordered_map<std::string, CommandInterface::SharedPtr> system_commands_;

hardware_interface_testing/test/test_resource_manager.cpp

@@ -1342,6 +1342,49 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces)
     rm.make_controller_reference_interfaces_unavailable("unknown_controller"), std::out_of_range);
 }
 
+class MockExecutor : public rclcpp::executors::SingleThreadedExecutor
+{
+public:
+  explicit MockExecutor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions())
+  : rclcpp::executors::SingleThreadedExecutor(options)
+  {
+  }
+
+  void add_node(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) override
+  {
+    rclcpp::executors::SingleThreadedExecutor::add_node(node_ptr, notify);
+    added_node_names.push_back(node_ptr->get_name());
+  }
+
+  std::vector<std::string> added_node_names;
+};
+
+TEST_F(ResourceManagerTest, hardware_nodes_are_added_to_mock_executor_on_load)
+{
+  auto mock_executor = std::make_shared<MockExecutor>();
+
+  hardware_interface::ResourceManagerParams params;
+  params.robot_description = ros2_control_test_assets::minimal_robot_urdf;
+  params.clock = node_.get_clock();
+  params.logger = node_.get_logger();
+  params.executor = mock_executor;
+  TestableResourceManager rm(params);
+  auto to_lower = [](const std::string & s)
+  {
+    std::string result = s;
+    std::transform(
+      result.begin(), result.end(), result.begin(),
+      [](unsigned char c) { return std::tolower(c); });
+    return result;
+  };
+  EXPECT_THAT(
+    mock_executor->added_node_names,
+    testing::UnorderedElementsAre(
+      to_lower(TEST_ACTUATOR_HARDWARE_NAME), to_lower(TEST_SENSOR_HARDWARE_NAME),
+      to_lower(TEST_SYSTEM_HARDWARE_NAME)));
+}
+
 class ResourceManagerTestReadWriteError : public ResourceManagerTest
 {
 public:

hardware_interface_testing/test/test_resource_manager.hpp

@@ -64,6 +64,11 @@ class TestableResourceManager : public hardware_interface::ResourceManager
       urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, 100)
   {
   }
+
+  explicit TestableResourceManager(const hardware_interface::ResourceManagerParams & params)
+  : hardware_interface::ResourceManager(params, true)
+  {
+  }
 };
 
 std::vector<hardware_interface::return_type> set_components_state(