Controller server loop-rate distorts controlling times

[SteveMacenski]

We found in development of MPPI and while I was benchmarking controllers for another project that the controller server will report that it missed the loop rate when in fact the controller is well capable of running much faster than the rates being asked of it.

An example, DWB can run at 250hz, yet running at 30hz at times will cause the error on occasion when each iteration takes a mere 3ms, typically. We can see this by logging the times for the algorithms to compute a velocity command.

Look into why this is the case and come with a resolution so that we can use the algorithms' full capabilities. I find that if I simply remove the loop sleep that we get it back, so maybe find another way to throttle the control loop or keep the threads resources "warmed up"?

@AlexeyMerzlyakov this would be a great medium sized, high impact project if you were interested

[afifswaidan]

Hello,
Can I get more details about which scripts are having this issue so I can also have a look?

[SteveMacenski]

nothing overly fancy. I was just noticing DWB complaining about missing its deadline when pushing to 30-50hz on my laptop and grew suspicious of that fact when I was working on a much heavier controller (#3350) at faster rates. I added prints in the controller server of a timer before and after the actual trajectory planner plugins times and noticed that it was running fast enough but the controller server reported that it couldn't keep up with the loop time.

The description above does a good job at explaining that when I removed sleeps, it was able to move at the natural rate.

[SteveMacenski]

I had a great conversation with Jan from Bosch at ROSCon that gave me a good idea on this! We can set the thread priority for the controller server's algorithm execution to be elevated relative to the rest of the stack so that we hit these timers more accurately. We don't mind delaying other systems a few milliseconds (global planning, etc) nearly as much as this thread.

He has a great idea to be able to set the priority on a per-interface basis, but for now, escalating the thread for controller will do the job for us until we have a RT executor option in rclcpp

[SteveMacenski]

#3914 resolves