Fix update rate issues by working around MutliThreadedExecutor

[Rayman]

Currently the MutliThreadedExecutor performance is very bad. This leads to controllers not meeting their update rate. This PR is a temporary workaround for these issues.

The current approach uses a rclcpp timer to execute the control loop. When used in combination with the MutliThreadedExecutor, the timers are not execute at their target frequency. I've converted the control loop to a while loop on a separate thread that uses nanosleep to execute the correct update rate. This means that rclcpp is not involved in the execution and leads to much better performance.

I've done some simple performance benchmarking. Using https://github.com/ros-controls/ros2_control_demos
with update_rate set to 500 Hz.

fix/update-rate-issues branch

$ ros2 topic hz /joint_states -w 500
average rate: 444.370
	min: 0.002s max: 0.003s std dev: 0.00016s window: 500
average rate: 443.508
	min: 0.002s max: 0.003s std dev: 0.00016s window: 500
average rate: 442.099
	min: 0.002s max: 0.003s std dev: 0.00016s window: 500
average rate: 445.781
	min: 0.002s max: 0.003s std dev: 0.00014s window: 500
average rate: 445.271
	min: 0.002s max: 0.003s std dev: 0.00016s window: 500
average rate: 443.078
	min: 0.002s max: 0.003s std dev: 0.00017s window: 500
average rate: 443.917
	min: 0.002s max: 0.003s std dev: 0.00015s window: 500

master branch

$ ros2 topic hz /joint_states -w 10
average rate: 4.811
	min: 0.067s max: 0.388s std dev: 0.12711s window: 7
average rate: 4.768
	min: 0.023s max: 0.388s std dev: 0.14797s window: 10
average rate: 11.099
	min: 0.002s max: 0.279s std dev: 0.08028s window: 10
average rate: 9.588
	min: 0.009s max: 0.277s std dev: 0.07749s window: 10
average rate: 7.070
	min: 0.009s max: 0.488s std dev: 0.13346s window: 10
average rate: 4.990
	min: 0.017s max: 0.712s std dev: 0.19685s window: 10
average rate: 4.472
	min: 0.002s max: 0.712s std dev: 0.23221s window: 10

I'm curious what you guys think about this workaround. Without this patch, I'm unable to run a control loop at 50Hz.

[bmagyar]

Could you please elaborate on the effects of this approach?

[destogl]

This is related to #260. It seems there is an issue in the depths of rclcpp (ros2/rclcpp#1487). There is also a solution that needs some more time... (ros2/rclcpp#1168).

The solution is IMHO legit until this is solved in rclcpp.

[Rayman]

@bmagyar I've added a simple benchmark to the description

[Karsten1987]

This PR actually broke the build on OSX.

/Users/karsten/workspace/ros2/ros2_control_ws/src/ros-controls/ros2_control/controller_manager/src/ros2_control_node.cpp:53:9: error: use of undeclared identifier 'clock_nanosleep'
        clock_nanosleep(CLOCK_MONOTONIC, 0, &duration, NULL);

[Rayman]

Sorry, I did not know OSX was a requirment. Should there be a CI for that?

Good suggestion to replace the call with std::this_thread::sleep_for. It should function the same and is more portable.