Skip to content

Commit 7879acb

Browse files
soham2560soham2560
authored
Addition of Default Publisher for HardwareStatus Messages (#2476)
1 parent 1fcf10f commit 7879acb

2 files changed

Lines changed: 250 additions & 43 deletions

File tree

‎hardware_interface/doc/writing_new_hardware_component.rst‎

Lines changed: 110 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -52,54 +52,58 @@ The following is a step-by-step guide to create source files, basic tests, and c
5252

5353
A common requirement for a hardware component is to publish status or diagnostic information without interfering with the real-time control loop.
5454

55-
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.
55+
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.
5656

57-
**Method 1: Using the Framework-Managed Node (Recommended & Simplest)**
57+
**Method 1: Using the Framework-Managed Publisher (Recommended & Simplest for HardwareStatus Messages)**
5858

59-
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.
59+
Refer :ref:`Framework Managed Publisher <framework_managed_publisher>`
6060

61-
#. **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.
61+
**Method 2: Using the Framework-Managed Node (Recommended & Simplest for Custom Messages)**
6262

63-
.. code-block:: cpp
63+
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.
6464

65-
// Continuing inside on_configure()
66-
if (get_node())
67-
{
68-
my_publisher_ = get_node()->create_publisher<std_msgs::msg::String>("~/status", 10);
69-
70-
using namespace std::chrono_literals;
71-
my_timer_ = get_node()->create_wall_timer(1s, [this]() {
72-
std_msgs::msg::String msg;
73-
msg.data = "Hardware status update!";
74-
my_publisher_->publish(msg);
75-
});
76-
}
65+
#. **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.
7766

78-
**Method 2: Using the Executor from `HardwareComponentInterfaceParams`**
67+
.. code-block:: cpp
7968
80-
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.
69+
// Continuing inside on_configure()
70+
if (get_node())
71+
{
72+
my_publisher_ = get_node()->create_publisher<std_msgs::msg::String>("~/status", 10);
8173
82-
#. **Update ``on_init`` Signature**: First, your hardware interface must override the ``on_init`` version that takes ``HardwareComponentInterfaceParams``.
74+
using namespace std::chrono_literals;
75+
my_timer_ = get_node()->create_wall_timer(1s, [this]() {
76+
std_msgs::msg::String msg;
77+
msg.data = "Hardware status update!";
78+
my_publisher_->publish(msg);
79+
});
80+
}
8381
84-
.. code-block:: cpp
82+
**Method 3: Using the Executor from `HardwareComponentInterfaceParams`**
8583

86-
// In your <robot_hardware_interface_name>.hpp
87-
hardware_interface::CallbackReturn on_init(
88-
const hardware_interface::HardwareComponentInterfaceParams & params) override;
84+
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.
8985

90-
#. **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.
86+
#. **Update ``on_init`` Signature**: First, your hardware interface must override the ``on_init`` version that takes ``HardwareComponentInterfaceParams``.
9187

92-
.. code-block:: cpp
88+
.. code-block:: cpp
9389
94-
// In your <robot_hardware_interface_name>.cpp, inside on_init(params)
95-
if (auto locked_executor = params.executor.lock())
96-
{
97-
my_custom_node_ = std::make_shared<rclcpp::Node>("my_custom_node");
98-
locked_executor->add_node(my_custom_node_->get_node_base_interface());
99-
// ... create publishers/timers on my_custom_node_ ...
100-
}
90+
// In your <robot_hardware_interface_name>.hpp
91+
hardware_interface::CallbackReturn on_init(
92+
const hardware_interface::HardwareComponentInterfaceParams & params) override;
93+
94+
#. **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.
95+
96+
.. code-block:: cpp
97+
98+
// In your <robot_hardware_interface_name>.cpp, inside on_init(params)
99+
if (auto locked_executor = params.executor.lock())
100+
{
101+
my_custom_node_ = std::make_shared<rclcpp::Node>("my_custom_node");
102+
locked_executor->add_node(my_custom_node_->get_node_base_interface());
103+
// ... create publishers/timers on my_custom_node_ ...
104+
}
101105
102-
For a complete, working implementation that uses the framework-managed node to publish diagnostic messages, see the demo in example 17.
106+
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>`.
103107

104108
#. 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.
105109

@@ -150,6 +154,78 @@ The following is a step-by-step guide to create source files, basic tests, and c
150154

151155
#. Implement ``write`` method that commands the hardware based on the values stored in internal variables defined in ``export_command_interfaces``.
152156

157+
#. (optional) **Framework Managed Publisher**
158+
159+
.. _framework_managed_publisher:
160+
161+
Implement ``init_hardware_status_message`` and ``update_hardware_status_message`` methods to publish the framework-supported hardware status reporting through ``control_msgs/msg/HardwareStatus`` messages:
162+
163+
* **`init_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.
164+
165+
.. code-block:: cpp
166+
167+
// In your <robot_hardware_interface_name>.hpp
168+
hardware_interface::CallbackReturn init_hardware_status_message(
169+
control_msgs::msg::HardwareStatus & msg_template) override;
170+
171+
// In your <robot_hardware_interface_name>.cpp
172+
hardware_interface::CallbackReturn MyHardware::init_hardware_status_message(
173+
control_msgs::msg::HardwareStatus & msg)
174+
{
175+
msg.hardware_id = get_hardware_info().name;
176+
msg.hardware_device_states.resize(get_hardware_info().joints.size());
177+
178+
for (size_t i = 0; i < get_hardware_info().joints.size(); ++i)
179+
{
180+
msg.hardware_device_states[i].device_id = get_hardware_info().joints[i].name;
181+
// This example uses one generic status per joint
182+
msg.hardware_device_states[i].generic_hardware_status.resize(1);
183+
}
184+
return hardware_interface::CallbackReturn::SUCCESS;
185+
}
186+
187+
* **`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.
188+
189+
.. code-block:: cpp
190+
191+
// In your <robot_hardware_interface_name>.hpp
192+
hardware_interface::return_type update_hardware_status_message(
193+
control_msgs::msg::HardwareStatus & msg) override;
194+
195+
// In your <robot_hardware_interface_name>.cpp
196+
hardware_interface::return_type MyHardware::update_hardware_status_message(
197+
control_msgs::msg::HardwareStatus & msg)
198+
{
199+
for (size_t i = 0; i < get_hardware_info().joints.size(); ++i)
200+
{
201+
auto & generic_status = msg.hardware_device_states[i].generic_hardware_status;
202+
// Example: Map internal state to a standard status field
203+
if (std::abs(hw_positions_[i]) > joint_limits_[i].max_position)
204+
{
205+
generic_status.health_status = control_msgs::msg::GenericState::HEALTH_ERROR;
206+
}
207+
else
208+
{
209+
generic_status.health_status = control_msgs::msg::GenericState::HEALTH_OK;
210+
}
211+
}
212+
return hardware_interface::return_type::OK;
213+
}
214+
215+
* **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.
216+
217+
.. code-block:: xml
218+
219+
<ros2_control name="MyHardware" type="system">
220+
<hardware>
221+
<plugin>my_package/MyHardware</plugin>
222+
<param name="status_publish_rate">20.0</param>
223+
</hardware>
224+
...
225+
</ros2_control>
226+
227+
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>`.
228+
153229
#. IMPORTANT: At the end of your file after the namespace is closed, add the ``PLUGINLIB_EXPORT_CLASS`` macro.
154230

155231
For this you will need to include the ``"pluginlib/class_list_macros.hpp"`` header.

‎hardware_interface/include/hardware_interface/hardware_component_interface.hpp‎

Lines changed: 140 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424
#include <utility>
2525
#include <vector>
2626

27+
#include "control_msgs/msg/hardware_status.hpp"
2728
#include "hardware_interface/component_parser.hpp"
2829
#include "hardware_interface/handle.hpp"
2930
#include "hardware_interface/hardware_info.hpp"
@@ -44,6 +45,8 @@
4445
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
4546
#include "rclcpp_lifecycle/state.hpp"
4647
#include "realtime_tools/async_function_handler.hpp"
48+
#include "realtime_tools/realtime_publisher.hpp"
49+
#include "realtime_tools/realtime_thread_safe_box.hpp"
4750

4851
namespace hardware_interface
4952
{
@@ -101,9 +104,8 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
101104
*/
102105
[[deprecated(
103106
"Replaced by CallbackReturn init(const hardware_interface::HardwareComponentParams & "
104-
"params). Initialization is handled by the Framework.")]]
105-
CallbackReturn init(
106-
const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
107+
"params). Initialization is handled by the Framework.")]] CallbackReturn
108+
init(const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
107109
{
108110
hardware_interface::HardwareComponentParams params;
109111
params.hardware_info = hardware_info;
@@ -202,20 +204,145 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
202204
params.hardware_info.name.c_str());
203205
}
204206

207+
double publish_rate = 0.0;
208+
auto it = info_.hardware_parameters.find("status_publish_rate");
209+
if (it != info_.hardware_parameters.end())
210+
{
211+
try
212+
{
213+
publish_rate = hardware_interface::stod(it->second);
214+
}
215+
catch (const std::invalid_argument &)
216+
{
217+
RCLCPP_WARN(
218+
get_logger(), "Invalid 'status_publish_rate' parameter. Using default %.1f Hz.",
219+
publish_rate);
220+
}
221+
}
222+
223+
if (publish_rate == 0.0)
224+
{
225+
RCLCPP_INFO(
226+
get_logger(),
227+
"`status_publish_rate` is set to 0.0, hardware status publisher will not be created.");
228+
}
229+
else
230+
{
231+
control_msgs::msg::HardwareStatus status_msg_template;
232+
if (init_hardware_status_message(status_msg_template) != CallbackReturn::SUCCESS)
233+
{
234+
RCLCPP_ERROR(get_logger(), "User-defined 'init_hardware_status_message' failed.");
235+
return CallbackReturn::ERROR;
236+
}
237+
238+
if (!status_msg_template.hardware_device_states.empty())
239+
{
240+
if (!hardware_component_node_)
241+
{
242+
RCLCPP_WARN(
243+
get_logger(),
244+
"Hardware status message was configured, but no node is available for the publisher. "
245+
"Publisher will not be created.");
246+
}
247+
else
248+
{
249+
try
250+
{
251+
hardware_status_publisher_ =
252+
hardware_component_node_->create_publisher<control_msgs::msg::HardwareStatus>(
253+
"~/hardware_status", rclcpp::SystemDefaultsQoS());
254+
255+
hardware_status_timer_ = hardware_component_node_->create_wall_timer(
256+
std::chrono::duration<double>(1.0 / publish_rate),
257+
[this]()
258+
{
259+
std::optional<control_msgs::msg::HardwareStatus> msg_to_publish_opt;
260+
hardware_status_box_.get(msg_to_publish_opt);
261+
262+
if (msg_to_publish_opt.has_value() && hardware_status_publisher_)
263+
{
264+
control_msgs::msg::HardwareStatus & msg = msg_to_publish_opt.value();
265+
if (update_hardware_status_message(msg) != return_type::OK)
266+
{
267+
RCLCPP_WARN_THROTTLE(
268+
get_logger(), *clock_, 1000,
269+
"User's update_hardware_status_message() failed for '%s'.",
270+
info_.name.c_str());
271+
return;
272+
}
273+
msg.header.stamp = this->get_clock()->now();
274+
hardware_status_publisher_->publish(msg);
275+
}
276+
});
277+
hardware_status_box_.set(std::make_optional(status_msg_template));
278+
}
279+
catch (const std::exception & e)
280+
{
281+
RCLCPP_ERROR(
282+
get_logger(), "Exception during publisher/timer setup for hardware status: %s",
283+
e.what());
284+
return CallbackReturn::ERROR;
285+
}
286+
}
287+
}
288+
else
289+
{
290+
RCLCPP_WARN(
291+
get_logger(),
292+
"`status_publish_rate` was set to a non-zero value, but no hardware status message was "
293+
"configured. Publisher will not be created. Are you sure "
294+
"init_hardware_status_message() is set up properly?");
295+
}
296+
}
297+
205298
hardware_interface::HardwareComponentInterfaceParams interface_params;
206299
interface_params.hardware_info = info_;
207300
interface_params.executor = params.executor;
208301
return on_init(interface_params);
209302
};
210303

304+
/// User-overridable method to configure the structure of the HardwareStatus message.
305+
/**
306+
* To enable status publishing, override this method to pre-allocate the message structure
307+
* and fill in static information like device IDs and interface names. This method is called
308+
* once during the non-realtime `init()` phase. If the `hardware_device_states` vector is
309+
* left empty, publishing will be disabled.
310+
*
311+
* \param[out] msg_template A reference to a HardwareStatus message to be configured.
312+
* \returns CallbackReturn::SUCCESS if configured successfully, CallbackReturn::ERROR on failure.
313+
*/
314+
virtual CallbackReturn init_hardware_status_message(
315+
control_msgs::msg::HardwareStatus & /*msg_template*/)
316+
{
317+
// Default implementation does nothing, disabling the feature.
318+
return CallbackReturn::SUCCESS;
319+
}
320+
321+
/// User-overridable method to fill the hardware status message with real-time data.
322+
/**
323+
* This real-time safe method is called by the framework within the `trigger_read()` loop.
324+
* Override this method to populate the `value` fields of the pre-allocated message with the
325+
* latest hardware states that were updated in your `read()` method.
326+
*
327+
* \param[in,out] msg The pre-allocated message to be filled with the latest values.
328+
* \returns return_type::OK on success, return_type::ERROR on failure.
329+
*/
330+
virtual return_type update_hardware_status_message(control_msgs::msg::HardwareStatus & /*msg*/)
331+
{
332+
// Default implementation does nothing.
333+
return return_type::OK;
334+
}
335+
211336
/// Initialization of the hardware interface from data parsed from the robot's URDF.
212337
/**
213338
* \param[in] hardware_info structure with data from URDF.
214339
* \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
215340
* \returns CallbackReturn::ERROR if any error happens or data are missing.
216341
*/
217-
[[deprecated("Use on_init(const HardwareComponentInterfaceParams & params) instead.")]]
218-
virtual CallbackReturn on_init(const HardwareInfo & hardware_info)
342+
[[deprecated(
343+
"Use on_init(const HardwareComponentInterfaceParams & params) "
344+
"instead.")]] virtual CallbackReturn
345+
on_init(const HardwareInfo & hardware_info)
219346
{
220347
info_ = hardware_info;
221348
if (info_.type == "actuator")
@@ -273,8 +400,8 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
273400
*/
274401
[[deprecated(
275402
"Replaced by vector<StateInterface::ConstSharedPtr> on_export_state_interfaces() method. "
276-
"Exporting is handled by the Framework.")]]
277-
virtual std::vector<StateInterface> export_state_interfaces()
403+
"Exporting is handled by the Framework.")]] virtual std::vector<StateInterface>
404+
export_state_interfaces()
278405
{
279406
// return empty vector by default. For backward compatibility we try calling
280407
// export_state_interfaces() and only when empty vector is returned call
@@ -363,8 +490,8 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
363490
*/
364491
[[deprecated(
365492
"Replaced by vector<CommandInterface::SharedPtr> on_export_command_interfaces() method. "
366-
"Exporting is handled by the Framework.")]]
367-
virtual std::vector<CommandInterface> export_command_interfaces()
493+
"Exporting is handled by the Framework.")]] virtual std::vector<CommandInterface>
494+
export_command_interfaces()
368495
{
369496
// return empty vector by default. For backward compatibility we try calling
370497
// export_command_interfaces() and only when empty vector is returned call
@@ -837,6 +964,10 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
837964

838965
protected:
839966
pal_statistics::RegistrationsRAII stats_registrations_;
967+
std::shared_ptr<rclcpp::Publisher<control_msgs::msg::HardwareStatus>> hardware_status_publisher_;
968+
realtime_tools::RealtimeThreadSafeBox<std::optional<control_msgs::msg::HardwareStatus>>
969+
hardware_status_box_;
970+
rclcpp::TimerBase::SharedPtr hardware_status_timer_;
840971
};
841972

842973
} // namespace hardware_interface

0 commit comments

Comments
 (0)