Skip to content

Commit 208f63f

Browse files
authored
Addition of a Default Node for Hardware Component (#2348) (#2413)
1 parent ff453e3 commit 208f63f

File tree

6 files changed

+182
-0
lines changed

6 files changed

+182
-0
lines changed

hardware_interface/doc/writing_new_hardware_component.rst

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,59 @@ The following is a step-by-step guide to create source files, basic tests, and c
4848
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)``.
4949
If all required parameters are set and valid and everything works fine return ``CallbackReturn::SUCCESS`` or ``return CallbackReturn::ERROR`` otherwise.
5050

51+
#. **(Optional) Adding Publisher, Services, etc.**
52+
53+
A common requirement for a hardware component is to publish status or diagnostic information without interfering with the real-time control loop.
54+
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.
56+
57+
**Method 1: Using the Framework-Managed Node (Recommended & Simplest)**
58+
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.
60+
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.
62+
63+
.. code-block:: cpp
64+
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+
}
77+
78+
**Method 2: Using the Executor from `HardwareComponentParams`**
79+
80+
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.
81+
82+
#. **Update ``on_init`` Signature**: First, your hardware interface must override the ``on_init`` version that takes ``HardwareComponentParams``.
83+
84+
.. code-block:: cpp
85+
86+
// In your <robot_hardware_interface_name>.hpp
87+
hardware_interface::CallbackReturn on_init(
88+
const hardware_interface::HardwareComponentParams & params) override;
89+
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.
91+
92+
.. code-block:: cpp
93+
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+
}
101+
102+
For a complete, working implementation that uses the framework-managed node to publish diagnostic messages, see the demo in example 17.
103+
51104
#. 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.
52105

53106
#. Implement ``on_cleanup`` method, which does the opposite of ``on_configure``.

hardware_interface/include/hardware_interface/actuator_interface.hpp

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -203,6 +203,26 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
203203
info_.thread_priority);
204204
async_handler_->start_thread();
205205
}
206+
207+
if (auto locked_executor = params.executor.lock())
208+
{
209+
std::string node_name = params.hardware_info.name;
210+
std::transform(
211+
node_name.begin(), node_name.end(), node_name.begin(),
212+
[](unsigned char c) { return std::tolower(c); });
213+
std::replace(node_name.begin(), node_name.end(), '/', '_');
214+
hardware_component_node_ = std::make_shared<rclcpp::Node>(node_name);
215+
locked_executor->add_node(hardware_component_node_->get_node_base_interface());
216+
}
217+
else
218+
{
219+
RCLCPP_WARN(
220+
params.logger,
221+
"Executor is not available during hardware component initialization for '%s'. Skipping "
222+
"node creation!",
223+
params.hardware_info.name.c_str());
224+
}
225+
206226
hardware_interface::HardwareComponentInterfaceParams interface_params;
207227
interface_params.hardware_info = info_;
208228
interface_params.executor = params.executor;
@@ -663,6 +683,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
663683
*/
664684
rclcpp::Clock::SharedPtr get_clock() const { return actuator_clock_; }
665685

686+
/// Get the default node of the ActuatorInterface.
687+
/**
688+
* \return node of the ActuatorInterface.
689+
*/
690+
rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; }
691+
666692
/// Get the hardware info of the ActuatorInterface.
667693
/**
668694
* \return hardware info of the ActuatorInterface.
@@ -719,6 +745,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
719745
private:
720746
rclcpp::Clock::SharedPtr actuator_clock_;
721747
rclcpp::Logger actuator_logger_;
748+
rclcpp::Node::SharedPtr hardware_component_node_ = nullptr;
722749
// interface names to Handle accessed through getters/setters
723750
std::unordered_map<std::string, StateInterface::SharedPtr> actuator_states_;
724751
std::unordered_map<std::string, CommandInterface::SharedPtr> actuator_commands_;

hardware_interface/include/hardware_interface/sensor_interface.hpp

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -166,6 +166,26 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
166166
info_.thread_priority);
167167
read_async_handler_->start_thread();
168168
}
169+
170+
if (auto locked_executor = params.executor.lock())
171+
{
172+
std::string node_name = params.hardware_info.name;
173+
std::transform(
174+
node_name.begin(), node_name.end(), node_name.begin(),
175+
[](unsigned char c) { return std::tolower(c); });
176+
std::replace(node_name.begin(), node_name.end(), '/', '_');
177+
hardware_component_node_ = std::make_shared<rclcpp::Node>(node_name);
178+
locked_executor->add_node(hardware_component_node_->get_node_base_interface());
179+
}
180+
else
181+
{
182+
RCLCPP_WARN(
183+
params.logger,
184+
"Executor is not available during hardware component initialization for '%s'. Skipping "
185+
"node creation!",
186+
params.hardware_info.name.c_str());
187+
}
188+
169189
hardware_interface::HardwareComponentInterfaceParams interface_params;
170190
interface_params.hardware_info = info_;
171191
interface_params.executor = params.executor;
@@ -436,6 +456,12 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
436456
*/
437457
rclcpp::Clock::SharedPtr get_clock() const { return sensor_clock_; }
438458

459+
/// Get the default node of the ActuatorInterface.
460+
/**
461+
* \return node of the ActuatorInterface.
462+
*/
463+
rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; }
464+
439465
/// Get the hardware info of the SensorInterface.
440466
/**
441467
* \return hardware info of the SensorInterface.
@@ -476,6 +502,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
476502
private:
477503
rclcpp::Clock::SharedPtr sensor_clock_;
478504
rclcpp::Logger sensor_logger_;
505+
rclcpp::Node::SharedPtr hardware_component_node_ = nullptr;
479506
// interface names to Handle accessed through getters/setters
480507
std::unordered_map<std::string, StateInterface::SharedPtr> sensor_states_map_;
481508

hardware_interface/include/hardware_interface/system_interface.hpp

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -207,6 +207,26 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
207207
info_.thread_priority);
208208
async_handler_->start_thread();
209209
}
210+
211+
if (auto locked_executor = params.executor.lock())
212+
{
213+
std::string node_name = params.hardware_info.name;
214+
std::transform(
215+
node_name.begin(), node_name.end(), node_name.begin(),
216+
[](unsigned char c) { return std::tolower(c); });
217+
std::replace(node_name.begin(), node_name.end(), '/', '_');
218+
hardware_component_node_ = std::make_shared<rclcpp::Node>(node_name);
219+
locked_executor->add_node(hardware_component_node_->get_node_base_interface());
220+
}
221+
else
222+
{
223+
RCLCPP_WARN(
224+
params.logger,
225+
"Executor is not available during hardware component initialization for '%s'. Skipping "
226+
"node creation!",
227+
params.hardware_info.name.c_str());
228+
}
229+
210230
hardware_interface::HardwareComponentInterfaceParams interface_params;
211231
interface_params.hardware_info = info_;
212232
interface_params.executor = params.executor;
@@ -694,6 +714,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
694714
*/
695715
rclcpp::Clock::SharedPtr get_clock() const { return system_clock_; }
696716

717+
/// Get the default node of the ActuatorInterface.
718+
/**
719+
* \return node of the ActuatorInterface.
720+
*/
721+
rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; }
722+
697723
/// Get the hardware info of the SystemInterface.
698724
/**
699725
* \return hardware info of the SystemInterface.
@@ -760,6 +786,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
760786
private:
761787
rclcpp::Clock::SharedPtr system_clock_;
762788
rclcpp::Logger system_logger_;
789+
rclcpp::Node::SharedPtr hardware_component_node_ = nullptr;
763790
// interface names to Handle accessed through getters/setters
764791
std::unordered_map<std::string, StateInterface::SharedPtr> system_states_;
765792
std::unordered_map<std::string, CommandInterface::SharedPtr> system_commands_;

hardware_interface_testing/test/test_resource_manager.cpp

Lines changed: 43 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1342,6 +1342,49 @@ TEST_F(ResourceManagerTest, managing_controllers_reference_interfaces)
13421342
rm.make_controller_reference_interfaces_unavailable("unknown_controller"), std::out_of_range);
13431343
}
13441344

1345+
class MockExecutor : public rclcpp::executors::SingleThreadedExecutor
1346+
{
1347+
public:
1348+
explicit MockExecutor(const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions())
1349+
: rclcpp::executors::SingleThreadedExecutor(options)
1350+
{
1351+
}
1352+
1353+
void add_node(
1354+
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, bool notify) override
1355+
{
1356+
rclcpp::executors::SingleThreadedExecutor::add_node(node_ptr, notify);
1357+
added_node_names.push_back(node_ptr->get_name());
1358+
}
1359+
1360+
std::vector<std::string> added_node_names;
1361+
};
1362+
1363+
TEST_F(ResourceManagerTest, hardware_nodes_are_added_to_mock_executor_on_load)
1364+
{
1365+
auto mock_executor = std::make_shared<MockExecutor>();
1366+
1367+
hardware_interface::ResourceManagerParams params;
1368+
params.robot_description = ros2_control_test_assets::minimal_robot_urdf;
1369+
params.clock = node_.get_clock();
1370+
params.logger = node_.get_logger();
1371+
params.executor = mock_executor;
1372+
TestableResourceManager rm(params);
1373+
auto to_lower = [](const std::string & s)
1374+
{
1375+
std::string result = s;
1376+
std::transform(
1377+
result.begin(), result.end(), result.begin(),
1378+
[](unsigned char c) { return std::tolower(c); });
1379+
return result;
1380+
};
1381+
EXPECT_THAT(
1382+
mock_executor->added_node_names,
1383+
testing::UnorderedElementsAre(
1384+
to_lower(TEST_ACTUATOR_HARDWARE_NAME), to_lower(TEST_SENSOR_HARDWARE_NAME),
1385+
to_lower(TEST_SYSTEM_HARDWARE_NAME)));
1386+
}
1387+
13451388
class ResourceManagerTestReadWriteError : public ResourceManagerTest
13461389
{
13471390
public:

hardware_interface_testing/test/test_resource_manager.hpp

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,11 @@ class TestableResourceManager : public hardware_interface::ResourceManager
6464
urdf, node.get_node_clock_interface(), node.get_node_logging_interface(), activate_all, 100)
6565
{
6666
}
67+
68+
explicit TestableResourceManager(const hardware_interface::ResourceManagerParams & params)
69+
: hardware_interface::ResourceManager(params, true)
70+
{
71+
}
6772
};
6873

6974
std::vector<hardware_interface::return_type> set_components_state(

0 commit comments

Comments
 (0)