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