period parameter in controller::update(time, period) is incorrect

[Nate711]

controller_interface_base.hpp says that controller_actual_period is "The measured time taken by the last control loop iteration" but in some experiments on my Raspberry Pi 5 robot I've found that is not the case. I think the issue is that if the controller manager is "lagging behind" then this calculation is wrong: const auto controller_actual_period = (time - *loaded_controller.next_update_cycle_time) + controller_period;

https://github.com/ros-controls/ros2_control/blob/d61abcdf593330839d698c32b3c7745f9515d06a/controller_manager/src/controller_manager.cpp#L2179C62-L2179C86

If you look at this data from a simple controller I'm running, you can see that the actual_period is quite different than the period argument to update()

FORMAT:               time     period      actual_period
[ros2_control_node-1] 0.997678 0.00606166  
[ros2_control_node-1] 1.00599 0.0103756 0.008312
[ros2_control_node-1] 1.0067 0.00810721 0.00071
[ros2_control_node-1] 1.01289 0.00722331 0.00619
[ros2_control_node-1] 1.01445 0.00683336 0.00156
[ros2_control_node-1] 1.01985 0.00823644 0.0054
[ros2_control_node-1] 1.02168 0.00605724 0.00183
[ros2_control_node-1] 1.02747 0.00785675 0.00579
[ros2_control_node-1] 1.03138 0.00775923 0.00391
[ros2_control_node-1] 1.03445 0.00683183 0.00307
[ros2_control_node-1] 1.03975 0.00813431 0.0053
[ros2_control_node-1] 1.04168 0.00606059 0.00193

Anyone know what's going on here?

[saikishor]

Hello @Nate711!

Thank you for reporting the issue. If you analyze the data properly, the difference of the time does match with the computed actual period right?. However, it doesn't match the period which is pretty weird. If this is the case, then the problem could be in the below lines:

https://github.com/ros-controls/ros2_control/blob/01bc0f4119f05f39be34c400ee5c5379d91b0bc8/controller_manager/src/ros2_control_node.cpp#L73-L75

Can you share is you are using the RT-preempt patch? and also the rate of your controller_manager?

Thank you,

[Nate711]

Thanks! Sorry there is a misunderstanding. I calculated my actual_period column by differencing the time column after collecting the data. It doesn't come from the code and is unrelated to controller_actual_period.

I'm using the low-latency kernel and the rate is set to 250hz. I realized I didn't set up the realtime group limits. Posting data below.

I manually changed priority of all processes and subprocesses of ros2_control_node using sudo chrt -r -p 49 [PID] but that actually didn't change the consistency or lag at all!

[Nate711]

After fixing the realtime group limits, I took some more data. Despite the ros2_control_node being at -51 priority (or -50 I don't remember), the jitter was very high.

Here the columns are time(s) period (from update function) actual_period (calculated by differencing the time)

[ros2_control_node-1] 30.0005 0.0104926 0.00497838 
[ros2_control_node-1] 30.0019 0.00796171 0.00146806 
[ros2_control_node-1] 30.0075 0.00953464 0.00557779 
[ros2_control_node-1] 30.0118 0.0098591 0.00432405 
[ros2_control_node-1] 30.0139 0.00795837 0.00209824 
[ros2_control_node-1] 30.0208 0.0108438 0.0068811 
[ros2_control_node-1] 30.0219 0.00795822 0.00112445
[ros2_control_node-1] 30.0275 0.00951415 0.00554861
[ros2_control_node-1] 30.0316 0.00962382 0.00410996
[ros2_control_node-1] 30.034 0.00802634 0.00239318
[ros2_control_node-1] 30.0401 0.0101579 0.00615198 
[ros2_control_node-1] 30.0427 0.00873144 0.00257694 
[ros2_control_node-1] 30.0475 0.00951212 0.00477023 
[ros2_control_node-1] 30.0516 0.00963201 0.00411326 

[saikishor]

Hello @Nate711!

I believe you will need real-time kernel fixes. If you say that you have a low latency program, I recommend making a program that executes simple things and then logging the period and the time for the calculation and trying to run it at this same frequency and please compare the latency results. This might give us some ideas.

Thank you

[Nate711]

@saikishor Thanks. I ended up figuring out it was reading from our I2C IMU that was causing the large jitter. If I don't read from the IMU the loop rate is very consistent even with only the low-latency kernel and not the real time kernel.

However, the "period" argument for the update method still seems wrong.

[saikishor]

Hey @Nate711!

I'm glad you figured out the issue. I've tried checking it on my end, but I couldn't figure out the issue on normal PC. Unfortunately, I don't have a RPi to test.

I've a question here, are you using the use_sim_time argument? because the time I see from your output seem to start from 0.

Thank you

[Nate711]

Good question but no, I was subtracting a time recorded from on_init...

[saikishor]

@Nate711 Can you compute the period here in the ros2_control node and also print it at the same time?. That might give some answers. Printing the current_time, previous_time and measured_period should help

https://github.com/ros-controls/ros2_control/blob/01bc0f4119f05f39be34c400ee5c5379d91b0bc8/controller_manager/src/ros2_control_node.cpp#L73-L75