Skip to content

Commit 3d57fc8

Browse files
saikishorsaikishor
authored
[Feature] Choose different read and write rate for the hardware components (#1570)
1 parent dbee650 commit 3d57fc8

File tree

18 files changed

+721
-243
lines changed

18 files changed

+721
-243
lines changed

‎doc/release_notes.rst

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -113,6 +113,44 @@ hardware_interface
113113
114114
* Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 <https://github.com/ros-controls/ros2_control/pull/1488>`_)
115115
* Access to logger and clock through ``get_logger`` and ``get_clock`` methods in ResourceManager and HardwareComponents ``Actuator``, ``Sensor`` and ``System`` (`#1585 <https://github.com/ros-controls/ros2_control/pull/1585>`_)
116+
* The ``ros2_control`` tag now supports parsing read/write rate ``rw_rate`` for the each hardware component parsed through the URDF (`#1570 <https://github.com/ros-controls/ros2_control/pull/1570>`_)
117+
118+
.. code:: xml
119+
120+
<ros2_control name="RRBotSystemMutipleGPIOs" type="system" rw_rate="500">
121+
<hardware>
122+
<plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
123+
<param name="example_param_hw_start_duration_sec">2.0</param>
124+
<param name="example_param_hw_stop_duration_sec">3.0</param>
125+
<param name="example_param_hw_slowdown">2.0</param>
126+
</hardware>
127+
<joint name="joint1">
128+
<command_interface name="position"/>
129+
<command_interface name="velocity"/>
130+
<state_interface name="position"/>
131+
</joint>
132+
<joint name="joint2">
133+
<command_interface name="position"/>
134+
<state_interface name="position"/>
135+
</joint>
136+
</ros2_control>
137+
<ros2_control name="MultimodalGripper" type="actuator" rw_rate="200">
138+
<hardware>
139+
<plugin>ros2_control_demo_hardware/MultimodalGripper</plugin>
140+
</hardware>
141+
<joint name="parallel_fingers">
142+
<command_interface name="position">
143+
<param name="min">0</param>
144+
<param name="max">100</param>
145+
</command_interface>
146+
<state_interface name="position"/>
147+
</joint>
148+
<gpio name="suction">
149+
<command_interface name="suction"/>
150+
<state_interface name="suction"/>
151+
</gpio>
152+
</ros2_control>
153+
116154
* Added ``get_hardware_info`` method to the hardware components interface to access the ``HardwareInfo`` instead of accessing the variable ``info_`` directly (`#1643 <https://github.com/ros-controls/ros2_control/pull/1643>`_)
117155
* With (`#1683 <https://github.com/ros-controls/ros2_control/pull/1683>`_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 <https://github.com/ros-controls/ros2_control/pull/1240>`_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles.
118156
* With (`#1421 <https://github.com/ros-controls/ros2_control/pull/1421>`_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file.

‎hardware_interface/doc/different_update_rates_userdoc.rst

Lines changed: 81 additions & 160 deletions
Original file line numberDiff line numberDiff line change
@@ -5,165 +5,86 @@
55
Different update rates for Hardware Components
66
===============================================================================
77

8-
In the following sections you can find some advice which will help you to implement Hardware
9-
Components with update rates different from the main control loop.
10-
11-
By counting loops
12-
-------------------------------------------------------------------------------
13-
14-
Current implementation of
15-
`ros2_control main node <https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_manager/src/ros2_control_node.cpp>`_
16-
has one update rate that controls the rate of the
17-
`read(...) <https://github.com/ros-controls/ros2_control/blob/0bdcd414c7ab8091f3e1b8d9b73a91c778388e82/hardware_interface/include/hardware_interface/system_interface.hpp#L175>`_
18-
and `write(...) <https://github.com/ros-controls/ros2_control/blob/fe462926416d527d1da163bc3eabd02ee1de9be9/hardware_interface/include/hardware_interface/system_interface.hpp#L178>`_
19-
calls in `hardware_interface(s) <https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/system_interface.hpp>`_.
20-
To achieve different update rates for your hardware component you can use the following steps:
21-
22-
.. _step-1:
23-
24-
1. Add parameters of main control loop update rate and desired update rate for your hardware component
25-
26-
.. code:: xml
27-
28-
<?xml version="1.0"?>
29-
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
30-
31-
<xacro:macro name="system_interface" params="name main_loop_update_rate desired_hw_update_rate">
32-
33-
<ros2_control name="${name}" type="system">
34-
<hardware>
35-
<plugin>my_system_interface/MySystemHardware</plugin>
36-
<param name="main_loop_update_rate">${main_loop_update_rate}</param>
37-
<param name="desired_hw_update_rate">${desired_hw_update_rate}</param>
38-
</hardware>
39-
...
40-
</ros2_control>
41-
42-
</xacro:macro>
43-
44-
</robot>
45-
46-
.. _step-2:
47-
48-
2. In you ``on_init()`` specific implementation fetch the desired parameters
49-
50-
.. code:: cpp
51-
52-
namespace my_system_interface
53-
{
54-
hardware_interface::CallbackReturn MySystemHardware::on_init(
55-
const hardware_interface::HardwareInfo & info)
56-
{
57-
if (
58-
hardware_interface::SystemInterface::on_init(info) !=
59-
hardware_interface::CallbackReturn::SUCCESS)
60-
{
61-
return hardware_interface::CallbackReturn::ERROR;
62-
}
63-
64-
// declaration in *.hpp file --> unsigned int main_loop_update_rate_, desired_hw_update_rate_ = 100 ;
65-
main_loop_update_rate_ = stoi(info_.hardware_parameters["main_loop_update_rate"]);
66-
desired_hw_update_rate_ = stoi(info_.hardware_parameters["desired_hw_update_rate"]);
67-
68-
...
69-
}
70-
...
71-
} // my_system_interface
72-
73-
3. In your ``on_activate`` specific implementation reset internal loop counter
74-
75-
.. code:: cpp
76-
77-
hardware_interface::CallbackReturn MySystemHardware::on_activate(
78-
const rclcpp_lifecycle::State & /*previous_state*/)
79-
{
80-
// declaration in *.hpp file --> unsigned int update_loop_counter_ ;
81-
update_loop_counter_ = 0;
82-
...
83-
}
84-
85-
4. In your ``read(const rclcpp::Time & time, const rclcpp::Duration & period)``
86-
and/or ``write(const rclcpp::Time & time, const rclcpp::Duration & period)``
87-
specific implementations decide if you should interfere with your hardware
88-
89-
.. code:: cpp
90-
91-
hardware_interface::return_type MySystemHardware::read(const rclcpp::Time & time, const rclcpp::Duration & period)
92-
{
93-
94-
bool hardware_go = main_loop_update_rate_ == 0 ||
95-
desired_hw_update_rate_ == 0 ||
96-
((update_loop_counter_ % desired_hw_update_rate_) == 0);
97-
98-
if (hardware_go){
99-
// hardware comms and operations
100-
...
101-
}
102-
...
103-
104-
// update counter
105-
++update_loop_counter_;
106-
update_loop_counter_ %= main_loop_update_rate_;
107-
}
108-
109-
By measuring elapsed time
110-
-------------------------------------------------------------------------------
111-
112-
Another way to decide if hardware communication should be executed in the
113-
``read(const rclcpp::Time & time, const rclcpp::Duration & period)`` and/or
114-
``write(const rclcpp::Time & time, const rclcpp::Duration & period)``
115-
implementations is to measure elapsed time since last pass:
116-
117-
1. In your ``on_activate`` specific implementation reset the flags that indicate
118-
that this is the first pass of the ``read`` and ``write`` methods
119-
120-
.. code:: cpp
121-
122-
hardware_interface::CallbackReturn MySystemHardware::on_activate(
123-
const rclcpp_lifecycle::State & /*previous_state*/)
124-
{
125-
// declaration in *.hpp file --> bool first_read_pass_, first_write_pass_ = true ;
126-
first_read_pass_ = first_write_pass_ = true;
127-
...
128-
}
129-
130-
2. In your ``read(const rclcpp::Time & time, const rclcpp::Duration & period)``
131-
and/or ``write(const rclcpp::Time & time, const rclcpp::Duration & period)``
132-
specific implementations decide if you should interfere with your hardware
133-
134-
.. code:: cpp
135-
136-
hardware_interface::return_type MySystemHardware::read(const rclcpp::Time & time, const rclcpp::Duration & period)
137-
{
138-
if (first_read_pass_ || (time - last_read_time_ ) > desired_hw_update_period_)
139-
{
140-
first_read_pass_ = false;
141-
// declaration in *.hpp file --> rclcpp::Time last_read_time_ ;
142-
last_read_time_ = time;
143-
// hardware comms and operations
144-
...
145-
}
146-
...
147-
}
148-
149-
hardware_interface::return_type MySystemHardware::write(const rclcpp::Time & time, const rclcpp::Duration & period)
150-
{
151-
if (first_write_pass_ || (time - last_write_time_ ) > desired_hw_update_period_)
152-
{
153-
first_write_pass_ = false;
154-
// declaration in *.hpp file --> rclcpp::Time last_write_time_ ;
155-
last_write_time_ = time;
156-
// hardware comms and operations
157-
...
158-
}
159-
...
160-
}
8+
The ``ros2_control`` framework allows to run different hardware components at different update rates. This is useful when some of the hardware components needs to run at a different frequency than the traditional control loop frequency which is same as the one of the ``controller_manager``. It is very typical to have different components with different frequencies in a robotic system with different sensors or different components using different communication protocols.
9+
This is useful when you have a hardware component that needs to run at a higher rate than the rest of the components. For example, you might have a sensor that needs to be read at 1000Hz, while the rest of the components can run at 500Hz, given that the control loop frequency of the ``controller_manager`` is higher than 1000Hz. The read/write rate can be defined easily by adding the parameter ``rw_rate`` to the ``ros2_control`` tag of the hardware component.
10+
11+
Examples
12+
*****************************
13+
The following examples show how to use the different hardware interface types with different update frequencies in a ``ros2_control`` URDF.
14+
They can be combined together within the different hardware component types (system, actuator, sensor) (:ref:`see detailed documentation <overview_hardware_components>`) as follows
15+
16+
For a RRBot with Multimodal gripper and external sensor running at different rates:
17+
18+
.. code-block:: xml
19+
20+
<ros2_control name="RRBotSystemMutipleGPIOs" type="system" rw_rate="500">
21+
<hardware>
22+
<plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
23+
<param name="example_param_hw_start_duration_sec">2.0</param>
24+
<param name="example_param_hw_stop_duration_sec">3.0</param>
25+
<param name="example_param_hw_slowdown">2.0</param>
26+
</hardware>
27+
<joint name="joint1">
28+
<command_interface name="position">
29+
<param name="min">-1</param>
30+
<param name="max">1</param>
31+
</command_interface>
32+
<state_interface name="position"/>
33+
</joint>
34+
<joint name="joint2">
35+
<command_interface name="position">
36+
<param name="min">-1</param>
37+
<param name="max">1</param>
38+
</command_interface>
39+
<state_interface name="position"/>
40+
</joint>
41+
<gpio name="flange_digital_IOs">
42+
<command_interface name="digital_output1"/>
43+
<state_interface name="digital_output1"/> <!-- Needed to know current state of the output -->
44+
<command_interface name="digital_output2"/>
45+
<state_interface name="digital_output2"/>
46+
<state_interface name="digital_input1"/>
47+
<state_interface name="digital_input2"/>
48+
</gpio>
49+
</ros2_control>
50+
<ros2_control name="MultimodalGripper" type="actuator" rw_rate="200">
51+
<hardware>
52+
<plugin>ros2_control_demo_hardware/MultimodalGripper</plugin>
53+
</hardware>
54+
<joint name="parallel_fingers">
55+
<command_interface name="position">
56+
<param name="min">0</param>
57+
<param name="max">100</param>
58+
</command_interface>
59+
<state_interface name="position"/>
60+
</joint>
61+
<gpio name="suction">
62+
<command_interface name="suction"/>
63+
<state_interface name="suction"/> <!-- Needed to know current state of the output -->
64+
</gpio>
65+
</ros2_control>
66+
<ros2_control name="RRBotForceTorqueSensor2D" type="sensor" rw_rate="250">
67+
<hardware>
68+
<plugin>ros2_control_demo_hardware/ForceTorqueSensor2DHardware</plugin>
69+
<param name="example_param_read_for_sec">0.43</param>
70+
</hardware>
71+
<sensor name="tcp_fts_sensor">
72+
<state_interface name="fx"/>
73+
<state_interface name="tz"/>
74+
<param name="frame_id">kuka_tcp</param>
75+
<param name="fx_range">100</param>
76+
<param name="tz_range">100</param>
77+
</sensor>
78+
<sensor name="temp_feedback">
79+
<state_interface name="temperature"/>
80+
</sensor>
81+
<gpio name="calibration">
82+
<command_interface name="calibration_matrix_nr"/>
83+
<state_interface name="calibration_matrix_nr"/>
84+
</gpio>
85+
</ros2_control>
86+
87+
In the above example, the system hardware component that controls the joints of the RRBot is running at 500 Hz, the multimodal gripper is running at 200 Hz and the force torque sensor is running at 250 Hz.
16188

16289
.. note::
163-
164-
The approach to get the desired update period value from the URDF and assign it to the variable
165-
``desired_hw_update_period_`` is the same as in the previous section (|step-1|_ and |step-2|_) but
166-
with a different parameter name.
167-
168-
.. |step-1| replace:: step 1
169-
.. |step-2| replace:: step 2
90+
In the above example, the ``rw_rate`` parameter is set to 500 Hz, 200 Hz and 250 Hz for the system, actuator and sensor hardware components respectively. This parameter is optional and if not set, the default value of 0 will be used which means that the hardware component will run at the same rate as the ``controller_manager``. However, if the specified rate is higher than the ``controller_manager`` rate, the hardware component will then run at the rate of the ``controller_manager``.

‎hardware_interface/include/hardware_interface/actuator.hpp

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -95,15 +95,28 @@ class Actuator final
9595
HARDWARE_INTERFACE_PUBLIC
9696
const rclcpp_lifecycle::State & get_lifecycle_state() const;
9797

98+
HARDWARE_INTERFACE_PUBLIC
99+
const rclcpp::Time & get_last_read_time() const;
100+
101+
HARDWARE_INTERFACE_PUBLIC
102+
const rclcpp::Time & get_last_write_time() const;
103+
98104
HARDWARE_INTERFACE_PUBLIC
99105
return_type read(const rclcpp::Time & time, const rclcpp::Duration & period);
100106

101107
HARDWARE_INTERFACE_PUBLIC
102108
return_type write(const rclcpp::Time & time, const rclcpp::Duration & period);
103109

110+
HARDWARE_INTERFACE_PUBLIC
111+
std::recursive_mutex & get_mutex();
112+
104113
private:
105114
std::unique_ptr<ActuatorInterface> impl_;
106115
mutable std::recursive_mutex actuators_mutex_;
116+
// Last read cycle time
117+
rclcpp::Time last_read_cycle_time_;
118+
// Last write cycle time
119+
rclcpp::Time last_write_cycle_time_;
107120
};
108121

109122
} // namespace hardware_interface

‎hardware_interface/include/hardware_interface/hardware_component_info.hpp

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include <string>
2323
#include <vector>
2424

25+
#include <rclcpp/time.hpp>
2526
#include "rclcpp_lifecycle/state.hpp"
2627

2728
namespace hardware_interface
@@ -47,6 +48,9 @@ struct HardwareComponentInfo
4748
/// Component is async
4849
bool is_async;
4950

51+
//// read/write rate
52+
unsigned int rw_rate;
53+
5054
/// Component current state.
5155
rclcpp_lifecycle::State state;
5256

‎hardware_interface/include/hardware_interface/hardware_info.hpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -174,6 +174,8 @@ struct HardwareInfo
174174
std::string type;
175175
/// Hardware group to which the hardware belongs.
176176
std::string group;
177+
/// Component's read and write rates in Hz.
178+
unsigned int rw_rate;
177179
/// Component is async
178180
bool is_async;
179181
/// Name of the pluginlib plugin of the hardware that will be loaded.

‎hardware_interface/include/hardware_interface/sensor.hpp

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,15 +82,23 @@ class Sensor final
8282
HARDWARE_INTERFACE_PUBLIC
8383
const rclcpp_lifecycle::State & get_lifecycle_state() const;
8484

85+
HARDWARE_INTERFACE_PUBLIC
86+
const rclcpp::Time & get_last_read_time() const;
87+
8588
HARDWARE_INTERFACE_PUBLIC
8689
return_type read(const rclcpp::Time & time, const rclcpp::Duration & period);
8790

8891
HARDWARE_INTERFACE_PUBLIC
8992
return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; }
9093

94+
HARDWARE_INTERFACE_PUBLIC
95+
std::recursive_mutex & get_mutex();
96+
9197
private:
9298
std::unique_ptr<SensorInterface> impl_;
9399
mutable std::recursive_mutex sensors_mutex_;
100+
// Last read cycle time
101+
rclcpp::Time last_read_cycle_time_;
94102
};
95103

96104
} // namespace hardware_interface

0 commit comments

Comments
 (0)