rw_rate with multiple hardware components leads to skipped cycles #2089
Description
Describe the bug
We currently have a robot system with two hardware components (a UR16e and a kuka KR50), one running at 500Hz and one running at 250Hz. The controller manager update_rate is set to 500 and the slower interface uses the rw_rate attribute:
<ros2_control name="${name}" type="system" rw_rate="250">
[...]
</ros2_control>
With this setup, the slower hardware is not able to move smoothly and "stutters". Using the tracing in kuka_rsi_driver, we can clearly see that the read- and write-functions frequently "miss a step" (see screenshots), i.e. only get called after 3 cycles instead of every 2.
Expected behavior
The read() and write() functions of the slower hardware interface should get called in every other control cycle consistently.
Screenshots
Through tracing we obtained the periods between successive read() and write() calls in our hardware interface. We expect these functions to be called every ~4ms, but we can clearly see that the period often times jumps to ~6ms:
In detail:
Environment (please complete the following information):
- OS: Ubuntu 24.04.1 LTS
- Version: Jazzy
- ros2_control is built from sources
Additional context
It seems like the calculation in resource_manager is the cause of the problem. It currently only ensures that the read/write functions are not called too frequently by checking
actual_period.seconds() * read_rate >= 0.99
I suspect that jitter in the faster hardware interface execution time leads to this deciding to skip a cycle frequently. I think a better approach would be to also consider the "timing error" resulting from skipping the call in a given cycle:
(actual_period.seconds() + 1. / resource_storage_->cm_update_rate_) * read_rate
I am going to check if this fixes the problem for our setup and try creating a PR if successful.