Skip to content

[Feature] Choose different read and write rate for the hardware components #1570

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 28 commits into from
Dec 4, 2024
Merged

[Feature] Choose different read and write rate for the hardware components #1570

Show file tree
Hide file tree
Changes from 25 commits
Commits
Show all changes
28 commits
Select commit Hold shift + click to select a range
b9416a1
added the initial changes for read_rate and write_rate
saikishor Jun 13, 2024
bbfb1e5
add last read and write cycle time to control the rate
saikishor Jun 16, 2024
ef24cdd
add read write rate parsing from the ros2_control tag in component pa…
saikishor Jun 16, 2024
50e3257
fix the component info using the rw_rate in the HardwareInfo
saikishor Jun 16, 2024
e6baee7
Add tests to the parsed rw_rate
saikishor Jun 16, 2024
15fc2a4
change the rw_rate between system and sensor interface for tests
saikishor Jun 16, 2024
328f59a
for hardware read and write period switch to rclcpp::Duration
saikishor Jun 16, 2024
eed5e3b
update the state of the system test component similar to actuator tes…
saikishor Jun 16, 2024
0a868ac
Add a test for a different components update rate and verifying with …
saikishor Jun 16, 2024
13fefac
Add more checks and clean a part of the testing code
saikishor Jun 16, 2024
081ead3
Add the test case of the deactivated case
saikishor Jun 16, 2024
f8b76e2
parameterize the tests for flexibility
saikishor Jun 16, 2024
c8b3550
Test also for the unconfigured and finalized lifecycle states
saikishor Jun 16, 2024
a8a19c2
Add testing for the component parser of the read and write rate
saikishor Jun 16, 2024
6815e42
Update documentation and the release notes
saikishor Jun 17, 2024
eb357e1
Parse the actual period to the read and write methods
saikishor Jun 17, 2024
19fc9bb
Use the current time to trigger the components based on their update …
saikishor Aug 6, 2024
b97f806
Update tests to reflect the change w.r.t reality
saikishor Aug 7, 2024
cc64820
update the logic to use the clock to initialize the last read and wri…
saikishor Aug 7, 2024
3b46b09
Fix the test_component_interface for missing valid node_clock_interface
saikishor Aug 7, 2024
c4014f0
use the internal mutex to skip when occupied by other nonRT things
saikishor Aug 27, 2024
b46f581
add missing copying of read and write time in the copy constructor
saikishor Aug 27, 2024
7437630
added the corner case of the simulation
saikishor Aug 27, 2024
bc47f7d
Fix test_component_interfaces for newly added tests
saikishor Nov 25, 2024
e273e06
Merge branch 'master' into add/hw_components/update_rate
bmagyar Nov 28, 2024
98aeea8
Merge branch 'master' into add/hw_components/update_rate
saikishor Dec 3, 2024
ef64595
Move to single rw_rate variable instead of read and write separate rates
saikishor Dec 4, 2024
68ddc9c
Merge branch 'master' into add/hw_components/update_rate
saikishor Dec 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
38 changes: 38 additions & 0 deletions doc/release_notes.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,44 @@ hardware_interface
* 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>`_)
* 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>`_)
* 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>`_)

.. code:: xml
<ros2_control name="RRBotSystemMutipleGPIOs" type="system" rw_rate="500">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
<param name="example_param_hw_start_duration_sec">2.0</param>
<param name="example_param_hw_stop_duration_sec">3.0</param>
<param name="example_param_hw_slowdown">2.0</param>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<state_interface name="position"/>
</joint>
</ros2_control>
<ros2_control name="MultimodalGripper" type="actuator" rw_rate="200">
<hardware>
<plugin>ros2_control_demo_hardware/MultimodalGripper</plugin>
</hardware>
<joint name="parallel_fingers">
<command_interface name="position">
<param name="min">0</param>
<param name="max">100</param>
</command_interface>
<state_interface name="position"/>
</joint>
<gpio name="suction">
<command_interface name="suction"/>
<state_interface name="suction"/>
</gpio>
</ros2_control>
* 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>`_)
* 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.
* 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.
Expand Down
241 changes: 81 additions & 160 deletions hardware_interface/doc/different_update_rates_userdoc.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,165 +5,86 @@
Different update rates for Hardware Components
===============================================================================

In the following sections you can find some advice which will help you to implement Hardware
Components with update rates different from the main control loop.

By counting loops
-------------------------------------------------------------------------------

Current implementation of
`ros2_control main node <https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/controller_manager/src/ros2_control_node.cpp>`_
has one update rate that controls the rate of the
`read(...) <https://github.com/ros-controls/ros2_control/blob/0bdcd414c7ab8091f3e1b8d9b73a91c778388e82/hardware_interface/include/hardware_interface/system_interface.hpp#L175>`_
and `write(...) <https://github.com/ros-controls/ros2_control/blob/fe462926416d527d1da163bc3eabd02ee1de9be9/hardware_interface/include/hardware_interface/system_interface.hpp#L178>`_
calls in `hardware_interface(s) <https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/system_interface.hpp>`_.
To achieve different update rates for your hardware component you can use the following steps:

.. _step-1:

1. Add parameters of main control loop update rate and desired update rate for your hardware component

.. code:: xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="system_interface" params="name main_loop_update_rate desired_hw_update_rate">
<ros2_control name="${name}" type="system">
<hardware>
<plugin>my_system_interface/MySystemHardware</plugin>
<param name="main_loop_update_rate">${main_loop_update_rate}</param>
<param name="desired_hw_update_rate">${desired_hw_update_rate}</param>
</hardware>
...
</ros2_control>
</xacro:macro>
</robot>
.. _step-2:

2. In you ``on_init()`` specific implementation fetch the desired parameters

.. code:: cpp
namespace my_system_interface
{
hardware_interface::CallbackReturn MySystemHardware::on_init(
const hardware_interface::HardwareInfo & info)
{
if (
hardware_interface::SystemInterface::on_init(info) !=
hardware_interface::CallbackReturn::SUCCESS)
{
return hardware_interface::CallbackReturn::ERROR;
}
// declaration in *.hpp file --> unsigned int main_loop_update_rate_, desired_hw_update_rate_ = 100 ;
main_loop_update_rate_ = stoi(info_.hardware_parameters["main_loop_update_rate"]);
desired_hw_update_rate_ = stoi(info_.hardware_parameters["desired_hw_update_rate"]);
...
}
...
} // my_system_interface
3. In your ``on_activate`` specific implementation reset internal loop counter

.. code:: cpp
hardware_interface::CallbackReturn MySystemHardware::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
// declaration in *.hpp file --> unsigned int update_loop_counter_ ;
update_loop_counter_ = 0;
...
}
4. In your ``read(const rclcpp::Time & time, const rclcpp::Duration & period)``
and/or ``write(const rclcpp::Time & time, const rclcpp::Duration & period)``
specific implementations decide if you should interfere with your hardware

.. code:: cpp
hardware_interface::return_type MySystemHardware::read(const rclcpp::Time & time, const rclcpp::Duration & period)
{
bool hardware_go = main_loop_update_rate_ == 0 ||
desired_hw_update_rate_ == 0 ||
((update_loop_counter_ % desired_hw_update_rate_) == 0);
if (hardware_go){
// hardware comms and operations
...
}
...
// update counter
++update_loop_counter_;
update_loop_counter_ %= main_loop_update_rate_;
}
By measuring elapsed time
-------------------------------------------------------------------------------

Another way to decide if hardware communication should be executed in the
``read(const rclcpp::Time & time, const rclcpp::Duration & period)`` and/or
``write(const rclcpp::Time & time, const rclcpp::Duration & period)``
implementations is to measure elapsed time since last pass:

1. In your ``on_activate`` specific implementation reset the flags that indicate
that this is the first pass of the ``read`` and ``write`` methods

.. code:: cpp
hardware_interface::CallbackReturn MySystemHardware::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
// declaration in *.hpp file --> bool first_read_pass_, first_write_pass_ = true ;
first_read_pass_ = first_write_pass_ = true;
...
}
2. In your ``read(const rclcpp::Time & time, const rclcpp::Duration & period)``
and/or ``write(const rclcpp::Time & time, const rclcpp::Duration & period)``
specific implementations decide if you should interfere with your hardware

.. code:: cpp
hardware_interface::return_type MySystemHardware::read(const rclcpp::Time & time, const rclcpp::Duration & period)
{
if (first_read_pass_ || (time - last_read_time_ ) > desired_hw_update_period_)
{
first_read_pass_ = false;
// declaration in *.hpp file --> rclcpp::Time last_read_time_ ;
last_read_time_ = time;
// hardware comms and operations
...
}
...
}
hardware_interface::return_type MySystemHardware::write(const rclcpp::Time & time, const rclcpp::Duration & period)
{
if (first_write_pass_ || (time - last_write_time_ ) > desired_hw_update_period_)
{
first_write_pass_ = false;
// declaration in *.hpp file --> rclcpp::Time last_write_time_ ;
last_write_time_ = time;
// hardware comms and operations
...
}
...
}
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.
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.

Examples
*****************************
The following examples show how to use the different hardware interface types with different update frequencies in a ``ros2_control`` URDF.
They can be combined together within the different hardware component types (system, actuator, sensor) (:ref:`see detailed documentation <overview_hardware_components>`) as follows

For a RRBot with Multimodal gripper and external sensor running at different rates:

.. code-block:: xml
<ros2_control name="RRBotSystemMutipleGPIOs" type="system" rw_rate="500">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
<param name="example_param_hw_start_duration_sec">2.0</param>
<param name="example_param_hw_stop_duration_sec">3.0</param>
<param name="example_param_hw_slowdown">2.0</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<gpio name="flange_digital_IOs">
<command_interface name="digital_output1"/>
<state_interface name="digital_output1"/> <!-- Needed to know current state of the output -->
<command_interface name="digital_output2"/>
<state_interface name="digital_output2"/>
<state_interface name="digital_input1"/>
<state_interface name="digital_input2"/>
</gpio>
</ros2_control>
<ros2_control name="MultimodalGripper" type="actuator" rw_rate="200">
<hardware>
<plugin>ros2_control_demo_hardware/MultimodalGripper</plugin>
</hardware>
<joint name="parallel_fingers">
<command_interface name="position">
<param name="min">0</param>
<param name="max">100</param>
</command_interface>
<state_interface name="position"/>
</joint>
<gpio name="suction">
<command_interface name="suction"/>
<state_interface name="suction"/> <!-- Needed to know current state of the output -->
</gpio>
</ros2_control>
<ros2_control name="RRBotForceTorqueSensor2D" type="sensor" rw_rate="250">
<hardware>
<plugin>ros2_control_demo_hardware/ForceTorqueSensor2DHardware</plugin>
<param name="example_param_read_for_sec">0.43</param>
</hardware>
<sensor name="tcp_fts_sensor">
<state_interface name="fx"/>
<state_interface name="tz"/>
<param name="frame_id">kuka_tcp</param>
<param name="fx_range">100</param>
<param name="tz_range">100</param>
</sensor>
<sensor name="temp_feedback">
<state_interface name="temperature"/>
</sensor>
<gpio name="calibration">
<command_interface name="calibration_matrix_nr"/>
<state_interface name="calibration_matrix_nr"/>
</gpio>
</ros2_control>
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.

.. note::

The approach to get the desired update period value from the URDF and assign it to the variable
``desired_hw_update_period_`` is the same as in the previous section (|step-1|_ and |step-2|_) but
with a different parameter name.

.. |step-1| replace:: step 1
.. |step-2| replace:: step 2
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``.
13 changes: 13 additions & 0 deletions hardware_interface/include/hardware_interface/actuator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,15 +95,28 @@ class Actuator final
HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & get_lifecycle_state() const;

HARDWARE_INTERFACE_PUBLIC
const rclcpp::Time & get_last_read_time() const;

HARDWARE_INTERFACE_PUBLIC
const rclcpp::Time & get_last_write_time() const;

HARDWARE_INTERFACE_PUBLIC
return_type read(const rclcpp::Time & time, const rclcpp::Duration & period);

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

HARDWARE_INTERFACE_PUBLIC
std::recursive_mutex & get_mutex();

private:
std::unique_ptr<ActuatorInterface> impl_;
mutable std::recursive_mutex actuators_mutex_;
// Last read cycle time
rclcpp::Time last_read_cycle_time_;
// Last write cycle time
rclcpp::Time last_write_cycle_time_;
};

} // namespace hardware_interface
Expand Down
7 changes: 7 additions & 0 deletions hardware_interface/include/hardware_interface/hardware_component_info.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include <string>
#include <vector>

#include <rclcpp/time.hpp>
#include "rclcpp_lifecycle/state.hpp"

namespace hardware_interface
Expand All @@ -47,6 +48,12 @@ struct HardwareComponentInfo
/// Component is async
bool is_async;

//// read rate
unsigned int read_rate;

//// write rate
unsigned int write_rate;

/// Component current state.
rclcpp_lifecycle::State state;

Expand Down
2 changes: 2 additions & 0 deletions hardware_interface/include/hardware_interface/hardware_info.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -174,6 +174,8 @@ struct HardwareInfo
std::string type;
/// Hardware group to which the hardware belongs.
std::string group;
/// Component's read and write rates in Hz.
unsigned int rw_rate;
/// Component is async
bool is_async;
/// Name of the pluginlib plugin of the hardware that will be loaded.
Expand Down
8 changes: 8 additions & 0 deletions hardware_interface/include/hardware_interface/sensor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,15 +82,23 @@ class Sensor final
HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & get_lifecycle_state() const;

HARDWARE_INTERFACE_PUBLIC
const rclcpp::Time & get_last_read_time() const;

HARDWARE_INTERFACE_PUBLIC
return_type read(const rclcpp::Time & time, const rclcpp::Duration & period);

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

HARDWARE_INTERFACE_PUBLIC
std::recursive_mutex & get_mutex();

private:
std::unique_ptr<SensorInterface> impl_;
mutable std::recursive_mutex sensors_mutex_;
// Last read cycle time
rclcpp::Time last_read_cycle_time_;
};

} // namespace hardware_interface
Expand Down
Loading