Addition of Default Publisher for HardwareStatus Messages

hardware_interface/doc/writing_new_hardware_component.rst

@@ -52,54 +52,58 @@ The following is a step-by-step guide to create source files, basic tests, and c
 
          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.
+         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 three primary ways to achieve this.
 
-         **Method 1: Using the Framework-Managed Node (Recommended & Simplest)**
+         **Method 1: Using the Framework-Managed Publisher (Recommended & Simplest for HardwareStatus Messages)**
 
-         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.
+            Refer :ref:`Framework Managed Publisher <framework_managed_publisher>`
 
-         #. **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.
+         **Method 2: Using the Framework-Managed Node (Recommended & Simplest for Custom Messages)**
 
-            .. code-block:: cpp
+            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.
 
-               // 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);
-                  });
-               }
+            #. **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.
 
-         **Method 2: Using the Executor from `HardwareComponentInterfaceParams`**
+               .. code-block:: cpp
 
-         For more advanced use cases where you need direct control over node creation, the ``on_init`` method can be configured to receive a ``HardwareComponentInterfaceParams`` struct. This struct contains a ``weak_ptr`` to the ``ControllerManager``'s executor.
+                  // Continuing inside on_configure()
+                  if (get_node())
+                  {
+                     my_publisher_ = get_node()->create_publisher<std_msgs::msg::String>("~/status", 10);
 
-         #. **Update ``on_init`` Signature**: First, your hardware interface must override the ``on_init`` version that takes ``HardwareComponentInterfaceParams``.
+                     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);
+                     });
+                  }
 
-            .. code-block:: cpp
+         **Method 3: Using the Executor from `HardwareComponentInterfaceParams`**
 
-               // In your <robot_hardware_interface_name>.hpp
-               hardware_interface::CallbackReturn on_init(
-               const hardware_interface::HardwareComponentInterfaceParams & params) override;
+            For more advanced use cases where you need direct control over node creation, the ``on_init`` method can be configured to receive a ``HardwareComponentInterfaceParams`` struct. This struct contains a ``weak_ptr`` to the ``ControllerManager``'s executor.
 
-         #. **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.
+            #. **Update ``on_init`` Signature**: First, your hardware interface must override the ``on_init`` version that takes ``HardwareComponentInterfaceParams``.
 
-            .. code-block:: cpp
+               .. 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_ ...
-               }
+                  // In your <robot_hardware_interface_name>.hpp
+                  hardware_interface::CallbackReturn on_init(
+                  const hardware_interface::HardwareComponentInterfaceParams & 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.
+            For a complete, working implementation that uses the framework-managed node to publish diagnostic messages, see the demo in :ref:`Example 17 <ros2_control_demos_example_17_userdoc>`.
 
    #. 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.
 
@@ -150,6 +154,78 @@ The following is a step-by-step guide to create source files, basic tests, and c
 
    #.  Implement ``write`` method that commands the hardware based on the values stored in internal variables defined in ``export_command_interfaces``.
 
+   #. (optional) **Framework Managed Publisher**
+
+      .. _framework_managed_publisher:
+
+      Implement ``configure_hardware_status_message`` and ``update_hardware_status_message`` methods to publish the framework-supported hardware status reporting through ``control_msgs/msg/HardwareStatus`` messages:
+
+      *   **`configure_hardware_status_message`**: This non-realtime method is called once during initialization. You must override it to define the **static structure** of your status message. This includes setting the ``hardware_id``, resizing the ``hardware_device_states`` vector, and for each device, resizing its specific status vectors (e.g., ``generic_hardware_status``, ``canopen_states``) and populating static fields like ``device_id`` and interface ``name``. Pre-allocating the message structure here is crucial for real-time safety.
+
+         .. code-block:: cpp
+
+            // In your <robot_hardware_interface_name>.hpp
+            hardware_interface::CallbackReturn configure_hardware_status_message(
+            control_msgs::msg::HardwareStatus & msg_template) override;
+
+            // In your <robot_hardware_interface_name>.cpp
+            hardware_interface::CallbackReturn MyHardware::configure_hardware_status_message(
+            control_msgs::msg::HardwareStatus & msg)
+            {
+               msg.hardware_id = get_hardware_info().name;
+               msg.hardware_device_states.resize(get_hardware_info().joints.size());
+
+               for (size_t i = 0; i < get_hardware_info().joints.size(); ++i)
+               {
+                  msg.hardware_device_states[i].device_id = get_hardware_info().joints[i].name;
+                  // This example uses one generic status per joint
+                  msg.hardware_device_states[i].generic_hardware_status.resize(1);
+               }
+               return hardware_interface::CallbackReturn::SUCCESS;
+            }
+
+      *   **`update_hardware_status_message`**: This real-time safe method is called from the framework's timer callback. You must override it to **fill in the dynamic values** of the pre-structured message. This typically involves copying your internal state variables (updated in your `read()` method) into the fields of the message. This method must be fast and non-allocating.
+
+         .. code-block:: cpp
+
+            // In your <robot_hardware_interface_name>.hpp
+            hardware_interface::return_type update_hardware_status_message(
+            control_msgs::msg::HardwareStatus & msg) override;
+
+            // In your <robot_hardware_interface_name>.cpp
+            hardware_interface::return_type MyHardware::update_hardware_status_message(
+            control_msgs::msg::HardwareStatus & msg)
+            {
+               for (size_t i = 0; i < get_hardware_info().joints.size(); ++i)
+               {
+                  auto & generic_status = msg.hardware_device_states[i].generic_hardware_status;
+                  // Example: Map internal state to a standard status field
+                  if (std::abs(hw_positions_[i]) > joint_limits_[i].max_position)
+                  {
+                     generic_status.health_status = control_msgs::msg::GenericState::HEALTH_ERROR;
+                  }
+                  else
+                  {
+                     generic_status.health_status = control_msgs::msg::GenericState::HEALTH_OK;
+                  }
+               }
+               return hardware_interface::return_type::OK;
+            }
+
+      *   **Enable in URDF**: Finally, to activate the publisher, add the ``status_publish_rate`` parameter to your ``<hardware>`` tag in the URDF. Setting it to 0.0 disables the feature.
+
+         .. code-block:: xml
+
+            <ros2_control name="MyHardware" type="system">
+            <hardware>
+               <plugin>my_package/MyHardware</plugin>
+               <param name="status_publish_rate">20.0</param>
+            </hardware>
+            ...
+            </ros2_control>
+
+      For a complete, working implementation that uses the framework-managed node to publish diagnostic messages, see the demo in :ref:`Example 17 <ros2_control_demos_example_17_userdoc>`.
+
    #.  IMPORTANT: At the end of your file after the namespace is closed, add the ``PLUGINLIB_EXPORT_CLASS`` macro.
 
       For this you will need to include the ``"pluginlib/class_list_macros.hpp"`` header.

hardware_interface/include/hardware_interface/hardware_component_interface.hpp

@@ -24,6 +24,7 @@
 #include <utility>
 #include <vector>
 
+#include "control_msgs/msg/hardware_status.hpp"
 #include "hardware_interface/component_parser.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
@@ -43,6 +44,8 @@
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/async_function_handler.hpp"
+#include "realtime_tools/realtime_publisher.hpp"
+#include "realtime_tools/realtime_thread_safe_box.hpp"
 
 namespace hardware_interface
 {
@@ -176,12 +179,135 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
         params.hardware_info.name.c_str());
     }
 
+    double publish_rate = 0.0;
+    auto it = info_.hardware_parameters.find("status_publish_rate");
+    if (it != info_.hardware_parameters.end())
+    {
+      try
+      {
+        publish_rate = hardware_interface::stod(it->second);
+      }
+      catch (const std::invalid_argument &)
+      {
+        RCLCPP_WARN(
+          get_logger(), "Invalid 'status_publish_rate' parameter. Using default %.1f Hz.",
+          publish_rate);
+      }
+    }
+
+    if (publish_rate == 0.0)
+    {
+      RCLCPP_INFO(
+        get_logger(),
+        "`status_publish_rate` is set to 0.0, hardware status publisher will not be created.");
+    }
+    else
+    {
+      control_msgs::msg::HardwareStatus status_msg_template;
+      if (configure_hardware_status_message(status_msg_template) != CallbackReturn::SUCCESS)
+      {
+        RCLCPP_ERROR(get_logger(), "User-defined 'configure_hardware_status_message' failed.");
+        return CallbackReturn::ERROR;
+      }
+
+      if (!status_msg_template.hardware_device_states.empty())
+      {
+        if (!hardware_component_node_)
+        {
+          RCLCPP_WARN(
+            get_logger(),
+            "Hardware status message was configured, but no node is available for the publisher. "
+            "Publisher will not be created.");
+        }
+        else
+        {
+          try
+          {
+            hardware_status_publisher_ =
+              hardware_component_node_->create_publisher<control_msgs::msg::HardwareStatus>(
+                "~/hardware_status", rclcpp::SystemDefaultsQoS());
+
+            hardware_status_timer_ = hardware_component_node_->create_wall_timer(
+              std::chrono::duration<double>(1.0 / publish_rate),
+              [this]()
+              {
+                std::optional<control_msgs::msg::HardwareStatus> msg_to_publish_opt;
+                hardware_status_box_.get(msg_to_publish_opt);
+
+                if (msg_to_publish_opt.has_value() && hardware_status_publisher_)
+                {
+                  control_msgs::msg::HardwareStatus & msg = msg_to_publish_opt.value();
+                  if (update_hardware_status_message(msg) != return_type::OK)
+                  {
+                    RCLCPP_WARN_THROTTLE(
+                      get_logger(), *clock_, 1000,
+                      "User's update_hardware_status_message() failed for '%s'.",
+                      info_.name.c_str());
+                    return;
+                  }
+                  msg.header.stamp = this->get_clock()->now();
+                  hardware_status_publisher_->publish(msg);
+                }
+              });
+            hardware_status_box_.set(std::make_optional(status_msg_template));
+          }
+          catch (const std::exception & e)
+          {
+            RCLCPP_ERROR(
+              get_logger(), "Exception during publisher/timer setup for hardware status: %s",
+              e.what());
+            return CallbackReturn::ERROR;
+          }
+        }
+      }
+      else
+      {
+        RCLCPP_WARN(
+          get_logger(),
+          "`status_publish_rate` was set to a non-zero value, but no hardware status message was "
+          "configured. Publisher will not be created. Are you sure "
+          "configure_hardware_status_message() is set up properly?");
+      }
+    }
+
     hardware_interface::HardwareComponentInterfaceParams interface_params;
     interface_params.hardware_info = info_;
     interface_params.executor = params.executor;
     return on_init(interface_params);
   };
 
+  /// User-overridable method to configure the structure of the HardwareStatus message.
+  /**
+   * To enable status publishing, override this method to pre-allocate the message structure
+   * and fill in static information like device IDs and interface names. This method is called
+   * once during the non-realtime `init()` phase. If the `hardware_device_states` vector is
+   * left empty, publishing will be disabled.
+   *
+   * \param[out] msg_template A reference to a HardwareStatus message to be configured.
+   * \returns CallbackReturn::SUCCESS if configured successfully, CallbackReturn::ERROR on failure.
+   */
+  virtual CallbackReturn configure_hardware_status_message(
+    control_msgs::msg::HardwareStatus & /*msg_template*/)
+  {
+    // Default implementation does nothing, disabling the feature.
+    return CallbackReturn::SUCCESS;
+  }
+
+  /// User-overridable method to fill the hardware status message with real-time data.
+  /**
+   * This real-time safe method is called by the framework within the `trigger_read()` loop.
+   * Override this method to populate the `value` fields of the pre-allocated message with the
+   * latest hardware states that were updated in your `read()` method.
+   *
+   * \param[in,out] msg The pre-allocated message to be filled with the latest values.
+   * \returns return_type::OK on success, return_type::ERROR on failure.
+   */
+  virtual return_type update_hardware_status_message(control_msgs::msg::HardwareStatus & /*msg*/)
+  {
+    // Default implementation does nothing.
+    return return_type::OK;
+  }
+
   /// Initialization of the hardware interface from data parsed from the robot's URDF.
   /**
    * \param[in] hardware_info structure with data from URDF.
@@ -798,6 +924,10 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
 
 protected:
   pal_statistics::RegistrationsRAII stats_registrations_;
+  std::shared_ptr<rclcpp::Publisher<control_msgs::msg::HardwareStatus>> hardware_status_publisher_;
+  realtime_tools::RealtimeThreadSafeBox<std::optional<control_msgs::msg::HardwareStatus>>
+    hardware_status_box_;
+  rclcpp::TimerBase::SharedPtr hardware_status_timer_;
 };
 
 }  // namespace hardware_interface