Update pendulum_control demo

[carlossvg]

Motivation

Follow-up from: #21

The only real-time application in ros2 repositories is the pendulum_control demo.

This code is quite old now and it would be good if the demo is revisited and updated with the latest ROS 2 features.

Related resources:

• This demo was presented in:
  Real-time Performance in ROS 2
• A tutorial already exists in: https://docs.ros.org/en/galactic/Tutorials/Real-Time-Programming.html

Issues found

1. The first latency sample is very high. Check if there is a problem during the initialization phase.
   • This seems to be the issue: https://github.com/ros2/realtime_support/blob/master/rttest/src/rttest.cpp#L552-L553
2. The setpoint callback includes a printf => https://github.com/ros2/demos/blob/master/pendulum_control/include/pendulum_control/pendulum_controller.hpp#L108
3. The simulation is incomple. After some time we get weird values. Velocity always increases.
4. Data is shared between threads without any synchronization mechanism
5. The demo requires 8GB of RAM when locking the memory. See https://answers.ros.org/question/337066/why-rttest-allocates-8gb-of-memory/
6. The demo includes some instrumented code to show the statistics. This is not ideal now that ros2 tracing is available.
7. We have the executor period and the timers period. The timers are set at a frequency slightly faster than the executor loop frequency. This way, in every iteration we have one callback per timer pending to execute. An alternative approach would be better. See Execution workflow section.
8. print('# of samples overrun:', len(indices[0])) => https://github.com/ros2/realtime_support/blob/rolling/rttest/scripts/analyze.py#L46**

Possible improvements

1. Replace all the instrumented code and statistics tracking with ros2 tracing scripts
2. Separate real-time threads from non real-time threads using callback groups
3. Make use of node lifecyle to separate initialization phases and real-time execution
4. Fix or improve simulation
5. Use a wait-free mechanism to share data between threads (if necessary)
6. Use parameters instead of command line arguments (arguments are inherited from rttest).
7. Add some tests checking there are no dynamic memory allocations in the real-time path
   • use osrf_testing_tools_cpp. Replace the malloc memory hook available in the past. See Remove the malloc_hook from the pendulum_demo. ros2/demos#544
8. Add a tracetools_analysis script to generate html reports like with performance_report. No jupyter notebook, just a python script.
9. Remove rttest dependency. Create helper functions to set thread priorities and lock memory.
10. Communication latency is not measured. If there are no messages pending in one iteration this is not detected.

Execution workflow

1. RttExecutor::spin is called https://github.com/ros2/demos/blob/f008dbbeefd7da30c4133ad67655554d9f7f8519/pendulum_control/src/pendulum_demo.cpp#L239
2. Spin calls rttest_spin https://github.com/ros2/demos/blob/f008dbbeefd7da30c4133ad67655554d9f7f8519/pendulum_control/include/pendulum_control/rtt_executor.hpp#L95
3. rttest_spin eventually calls spin_period https://github.com/ros2/realtime_support/blob/0e3faef4b7aa7985726dba97c761a6783f62a958/rttest/src/rttest.cpp#L507-L531
   • spin_period will call spin_once in a while loop or in a for loop
4. spin_once implements a standard real-time loop
   • Uses clock_nanosleep with absolute time to set the next cycle wake up time
   • Execute the user function
   • Records latency statistics -> Note the measured latency measures the wake up time
5. User function is spin_some https://github.com/ros2/demos/blob/master/pendulum_control/include/pendulum_control/rtt_executor.hpp#L118
6. Spin some executes in every iteration the following things
   • constroller timer callback https://github.com/ros2/demos/blob/f008dbbeefd7da30c4133ad67655554d9f7f8519/pendulum_control/src/pendulum_demo.cpp#L185-L192
   • motor_publish_callback https://github.com/ros2/demos/blob/f008dbbeefd7da30c4133ad67655554d9f7f8519/pendulum_control/src/pendulum_demo.cpp#L175-L182
   • motor_subscribe_callback https://github.com/ros2/demos/blob/f008dbbeefd7da30c4133ad67655554d9f7f8519/pendulum_control/src/pendulum_demo.cpp#L110-L114
   • controller_subscribe_callback https://github.com/ros2/demos/blob/f008dbbeefd7da30c4133ad67655554d9f7f8519/pendulum_control/src/pendulum_demo.cpp#L123-L127
7. Additionally spin some may execute the following executables
   • logger_publish_callback (every 10ms) https://github.com/ros2/demos/blob/f008dbbeefd7da30c4133ad67655554d9f7f8519/pendulum_control/src/pendulum_demo.cpp#L195-L205
   • controller_command_callback (when a setpoint teleop command is recieved) https://github.com/ros2/demos/blob/f008dbbeefd7da30c4133ad67655554d9f7f8519/pendulum_control/src/pendulum_demo.cpp#L140-L144

Draft

Draft here: https://github.com/carlossvg/demos/tree/master/pendulum_control

Acceptance criteria

• The demo code is updated
• A new tutorial exists explaining how to run the demo