From a4196ff704c098b0f15755125640b85b57ac4680 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Fri, 22 Aug 2025 08:31:03 +0000 Subject: [PATCH 1/2] used new default publisher api in example 17 --- .../ros2_control/rrbot.ros2_control.xacro | 1 + .../ros2_control_demo_example_17/rrbot.hpp | 6 +++ example_17/hardware/rrbot.cpp | 46 +++++++++++++++++++ 3 files changed, 53 insertions(+) diff --git a/example_17/description/ros2_control/rrbot.ros2_control.xacro b/example_17/description/ros2_control/rrbot.ros2_control.xacro index f05f6a8a6..195d1e4e0 100644 --- a/example_17/description/ros2_control/rrbot.ros2_control.xacro +++ b/example_17/description/ros2_control/rrbot.ros2_control.xacro @@ -9,6 +9,7 @@ 0 3.0 100 + 10 diff --git a/example_17/hardware/include/ros2_control_demo_example_17/rrbot.hpp b/example_17/hardware/include/ros2_control_demo_example_17/rrbot.hpp index 1afcb7b01..beb0241c0 100644 --- a/example_17/hardware/include/ros2_control_demo_example_17/rrbot.hpp +++ b/example_17/hardware/include/ros2_control_demo_example_17/rrbot.hpp @@ -55,6 +55,12 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + hardware_interface::CallbackReturn configure_hardware_status_message( + control_msgs::msg::HardwareStatus & msg_template) override; + + hardware_interface::return_type update_hardware_status_message( + control_msgs::msg::HardwareStatus & msg) override; + private: // Parameters for the RRBot simulation double hw_start_sec_; diff --git a/example_17/hardware/rrbot.cpp b/example_17/hardware/rrbot.cpp index 61a6302a5..2992fa7fe 100644 --- a/example_17/hardware/rrbot.cpp +++ b/example_17/hardware/rrbot.cpp @@ -176,6 +176,52 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure return hardware_interface::CallbackReturn::SUCCESS; } +hardware_interface::CallbackReturn +RRBotSystemPositionOnlyHardware::configure_hardware_status_message( + control_msgs::msg::HardwareStatus & msg_template) +{ + RCLCPP_INFO(get_logger(), "Configuring hardware status message for RRBot."); + msg_template.hardware_id = get_hardware_info().name; + msg_template.hardware_device_states.resize(get_hardware_info().joints.size()); + + for (size_t i = 0; i < get_hardware_info().joints.size(); ++i) + { + auto & device_status = msg_template.hardware_device_states[i]; + const auto & joint = get_hardware_info().joints[i]; + device_status.device_id = joint.name; + device_status.hardware_status.resize(1); + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type RRBotSystemPositionOnlyHardware::update_hardware_status_message( + control_msgs::msg::HardwareStatus & msg) +{ + for (size_t i = 0; i < get_hardware_info().joints.size(); ++i) + { + auto & hardware_status = msg.hardware_device_states[i].hardware_status[0]; + double position = get_state(get_hardware_info().joints[i].name + "/position"); + if (std::abs(position) > 0.8) + { + hardware_status.health_status = control_msgs::msg::GenericHardwareState::HEALTH_WARNING; + } + else + { + hardware_status.health_status = control_msgs::msg::GenericHardwareState::HEALTH_OK; + } + hardware_status.operational_mode = control_msgs::msg::GenericHardwareState::MODE_AUTO; + hardware_status.power_state = control_msgs::msg::GenericHardwareState::POWER_ON; + hardware_status.state_details.clear(); + diagnostic_msgs::msg::KeyValue detail; + detail.key = "position_state"; + detail.value = std::to_string(position); + hardware_status.state_details.push_back(detail); + } + + return hardware_interface::return_type::OK; +} + hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { From fc5e1fe34049bb37553b3134196aff5c3c82ccdb Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Fri, 22 Aug 2025 20:43:25 +0000 Subject: [PATCH 2/2] updated example 17 documetnation --- example_17/doc/userdoc.rst | 157 +++++++++++++++++++++++++++++++++++-- 1 file changed, 151 insertions(+), 6 deletions(-) diff --git a/example_17/doc/userdoc.rst b/example_17/doc/userdoc.rst index f354f7a9a..a82037361 100644 --- a/example_17/doc/userdoc.rst +++ b/example_17/doc/userdoc.rst @@ -2,16 +2,17 @@ .. _ros2_control_demos_example_17_userdoc: -Example 17: RRBot with Hardware Component that publishes diagnostics -===================================================================== +Example 17: RRBot with Hardware Component that publishes diagnostics and status messages +======================================================================================== -This example shows how to publish diagnostics from a hardware component using ROS 2 features available within the ``ros2_control`` framework. +This example shows how to publish diagnostics and status messages from a hardware component using ROS 2 features available within the ``ros2_control`` framework. It is essentially the same as Example 1, but with a modified hardware interface plugin that demonstrates two methods for publishing status information: 1. Using the standard ``diagnostic_updater`` on the default node to publish to the ``/diagnostics`` topic. This is the recommended approach for hardware status reporting. 2. Using the Controller Manager's Executor to add a custom ROS 2 node for publishing to a separate, non-standard topic. +3. Using the framework managed default publisher, which published with a ``HardwareStatus`` message, this is the recommended way to approach standard hardware status reporting -See the :ref:`Implementation Details of the Diagnostic Publisher ` for more information. +See :ref:`Implementation Details of the Diagnostic Publisher ` and :ref:`Implementation Details of the Hardware Status Publisher ` for more information. For *example_17*, the hardware interface plugin is implemented having only one interface. @@ -33,6 +34,10 @@ This tutorial differs by including a hardware interface that publishes diagnosti 1. A **default node** is automatically provided to the hardware component. We use the standard ``diagnostic_updater`` library on this node to publish structured diagnostic data. 2. A **custom ROS 2 node** is created inside the hardware interface and added to the executor provided by the controller manager. This demonstrates a more manual approach useful for non-diagnostic topics. +as well as + +1. A **default publisher** is automatically created through steps detailed in :ref:`Implementation Details of the Hardware Status Publisher `. + The nodes and topics: - Default node (via ``diagnostic_updater``): @@ -44,6 +49,10 @@ The nodes and topics: - Publishes on the topic ``/rrbot_custom_status``. - Uses message type ``std_msgs/msg/String``. - Sends a message every 2 seconds. +- Default Publisher: + - Publishes on the topic ``/rrbot/hardware_status``. + - Uses message type ``control_msgs/msg/HardwareStatus``. + - Sends a message at rate specified by ``status_publish_rate`` parameter in ros2_control tag. To check that the nodes are running and diagnostics are published correctly: @@ -73,12 +82,19 @@ To check that the nodes are running and diagnostics are published correctly: ros2 topic info /rrbot_custom_status # Should show: std_msgs/msg/String + # Confirm message type of the default publisher topic + ros2 topic info /rrbot/hardware_status + # Should show: control_msgs/msg/HardwareStatus + # Echo the raw messages published by the default node ros2 topic echo /diagnostics # Echo the messages published by the custom node ros2 topic echo /rrbot_custom_status + # Echo the messages published by default publisher + ros2 topic echo /rrbot/hardware_status + .. group-tab:: Docker Enter the running container shell first: @@ -94,8 +110,10 @@ To check that the nodes are running and diagnostics are published correctly: ros2 node list ros2 topic info /diagnostics ros2 topic info /rrbot_custom_status + ros2 topic info /rrbot/hardware_status ros2 topic echo /diagnostics ros2 topic echo /rrbot_custom_status + ros2 topic echo /rrbot/hardware_status The echoed messages should look similar to: @@ -112,17 +130,144 @@ The echoed messages should look similar to: message: "Hardware is OK" hardware_id: "" values: [] - --- .. code-block:: shell # From /rrbot_custom_status (showing the custom node's output) data: RRBot 'RRBot' custom node is alive at 1751087597.146549 - --- + +.. code-block:: yaml + + # From /rrbot/hardware_status (showing the default publisher's output) + header: + stamp: + sec: 168700 + nanosec: 87768976 + frame_id: '' + hardware_id: RRBot + hardware_device_states: + - header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + device_id: joint1 + hardware_status: + - health_status: 1 + error_domain: [] + operational_mode: 2 + power_state: 3 + connectivity_status: 0 + manufacturer: '' + model: '' + firmware_version: '' + state_details: + - key: position_state + value: '0.000000' + canopen_states: [] + ethercat_states: [] + vda5050_states: [] + - header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + device_id: joint2 + hardware_status: + - health_status: 1 + error_domain: [] + operational_mode: 2 + power_state: 3 + connectivity_status: 0 + manufacturer: '' + model: '' + firmware_version: '' + state_details: + - key: position_state + value: '0.000000' + canopen_states: [] + ethercat_states: [] + vda5050_states: [] .. note:: The custom diagnostics node and its timer are created only if the executor is successfully passed to the hardware component. If you don't see the topic or node, ensure the hardware plugin is correctly implemented and that the controller manager is providing an executor. +.. _hardware_status_publisher_implementation: + +Implementation Details of the Hardware Status Publisher +------------------------------------------------------- +**1. Using the Framework-Managed Status Publisher (Recommended for HardwareStatus Messages)** + + The ``ros2_control`` framework provides a built-in, real-time safe mechanism for publishing standardized hardware status via the ``control_msgs/msg/HardwareStatus`` message. This is the simplest and most robust way to provide detailed status messages. It is enabled by implementing two virtual methods in your hardware component. + + a. **Override 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``) and populating static fields like ``device_id``. + + .. code-block:: cpp + + // In rrbot.hpp, add the override declaration: + hardware_interface::CallbackReturn configure_hardware_status_message( + control_msgs::msg::HardwareStatus & msg_template) override; + + // In rrbot.cpp + hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::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; + } + + b. **Override update_hardware_status_message**: This real-time safe method is called from the framework's internal timer. You override it to **fill in the dynamic values** of the pre-structured message, typically by copying your hardware's internal state variables (updated in your ``read()`` method) into the fields of the message. + + .. code-block:: cpp + + // In rrbot.hpp, add the override declaration: + hardware_interface::return_type update_hardware_status_message( + control_msgs::msg::HardwareStatus & msg) override; + + // In rrbot.cpp + hardware_interface::return_type RRBotSystemPositionOnlyHardware::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 + double position = get_state(get_hardware_info().joints[i].name + "/position"); + if (std::abs(position) > 2.5) // Arbitrary warning threshold + { + generic_status.health_status = control_msgs::msg::GenericState::HEALTH_WARNING; + } + else + { + generic_status.health_status = control_msgs::msg::GenericState::HEALTH_OK; + } + generic_status.operational_mode = control_msgs::msg::GenericState::MODE_AUTO; + generic_status.power_state = control_msgs::msg::GenericState::POWER_ON; + } + return hardware_interface::return_type::OK; + } + + c. **Enable in URDF**: To activate the publisher, add the ``status_publish_rate`` parameter to your ```` tag in the URDF. Setting it to 0.0 disabled the feature. + + .. code-block:: xml + + + + ros2_control_demo_example_17/RRBotSystemPositionOnlyHardware + 20.0 + + ... + + + This will create a publisher on the topic ``/rrbot/hardware_status``. .. _diagnostic_publisher_implementation: