Fix wrong CM statistics + add fix for overruns

[saikishor]

There is a bug in the CM statistics in the first loop, but then apart from that, when there is a controller that takes longer in the loop, this causes an overrun in the system. In order to fix it, I've added an overrun check and adjust the time properly to trigger the loops.

Earlier, the triggered overruns are the cause for such fast loops, resulting in high periodicity reports

Tested on real hardware TIAGo at 1kHz

[codecov]

Codecov Report

Attention: Patch coverage is 38.46154% with 8 lines in your changes missing coverage. Please review.

Project coverage is 89.38%. Comparing base (7406bdc) to head (55bdc45).

Files with missing lines	Patch %	Lines
controller_manager/src/ros2_control_node.cpp	38.46%	7 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #2042      +/-   ##
==========================================
+ Coverage   89.35%   89.38%   +0.02%     
==========================================
  Files         138      138              
  Lines       14764    14767       +3     
  Branches     1270     1271       +1     
==========================================
+ Hits        13192    13199       +7     
+ Misses       1095     1094       -1     
+ Partials      477      474       -3     

Flag	Coverage Δ
unittests	89.38% <38.46%> (+0.02%)	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
controller_manager/src/ros2_control_node.cpp	56.06% <38.46%> (-7.10%)	⬇️

... and 4 files with indirect coverage changes