Period calculation in controller_manager incorrect?

[fmauch]

I do have the impression that the period calculation introduced in #1140 actually causes some problems. This is maybe related to #1574 but I am not completely sure, hence a new issue.

When implementing a controller I realized that the periods reported to the controller were way too large (roughly factor 2), while a debug statement printing those periods was printed in the correct frequency. To debug this further I added a couple of logging statements to the controller_manager:

https://github.com/fmauch/ros2_control/tree/debug_periods

Some example output:

2024-09-30 15:59:08.975 Setting next_update_cycle_time to 1727704748.975775s for the controller : scaled_joint_trajectory_controller             
2024-09-30 15:59:08.975 scaled_joint_trajectory_controller: time                     measured_period     next_update_cycle_time   actual period  
2024-09-30 15:59:08.975 scaled_joint_trajectory_controller: 1727704748.9757745266    0.00200859          1727704748.9757745266    0.00200859     
2024-09-30 15:59:08.977 update time: 1727704748.977764                                                                                           
2024-09-30 15:59:08.979 update time: 1727704748.979762                                                                                           
2024-09-30 15:59:08.979 scaled_joint_trajectory_controller: 1727704748.9797618389    0.00200254          1727704748.9777829647    0.00398133     
2024-09-30 15:59:08.981 update time: 1727704748.981752                                                                                           
2024-09-30 15:59:08.981 scaled_joint_trajectory_controller: 1727704748.9817519188    0.00199638          1727704748.9797856808    0.00396257     
2024-09-30 15:59:08.983 update time: 1727704748.983768                                                                                           
2024-09-30 15:59:08.983 scaled_joint_trajectory_controller: 1727704748.9837677479    0.0020053           1727704748.9817819595    0.00399119     
2024-09-30 15:59:08.985 update time: 1727704748.985751                                                                                           
2024-09-30 15:59:08.985 scaled_joint_trajectory_controller: 1727704748.9857513905    0.00199351          1727704748.9837872982    0.00395743     
2024-09-30 15:59:08.987 update time: 1727704748.987751                                                                                           
2024-09-30 15:59:08.987 scaled_joint_trajectory_controller: 1727704748.9877514839    0.00200196          1727704748.9857807159    0.00397259

Similar output can straightforward be produced using

ros2 launch ros2_control_demo_example_3 rrbot_system_multi_interface.launch.py | grep controller_manager

What I realized: At some point there is a control cycle where the controller doesn't receive an update (update time: ) is printed at the beginning of CM's update method, while the other logs are from the inner loop over the controllers.

So, a skipped controller update due to a cm cycle time under the desired period due to some system jitter results in an all-time wrong period.

What basically happens with a cm rate == controller rate == 5 Hz with some jitter:

CM updates	controller update	next expected update	actual period
0.0	yes	0.2	-
0.21	yes	0.4	0.21
0.39	no	0.4	-
0.6	yes	0.6	0.4
0.8	yes	0.8	0.4
1.0	yes	1.2	0.4

Expected behavior
I would expect that the period times normalize again after such an incident.

One way to achieve this could be to increment the next cycletime as such

*loaded_controller.next_update_cycle_time = time + controller_period;

but I think this wouldn't do any controller justice for which the controller update times would not exactly match the controller_manager rate.

E.g.: Controller manager running with 10 Hz, controller running with 3 Hz

Correctly:

CM updates	controller update	next update
0.0	yes	0.3333
0.1	no
0.2	no
0.3	no
0.4	yes	0.6667
0.5	no
0.6	no
0.7	yes	0.99999
0.8	no
0.9	no
1.0	yes	1.333
1.1	no

With the proposed approach:

CM updates	controller update	next update
0.0	yes	0.3333
0.1	no
0.2	no
0.3	no
0.4	yes	0.7333
0.5	no
0.6	no
0.7	no
0.8	yes	1.1333
0.9	no
1.0	no
1.1	no