[Diagnostics] Add diagnostics of execution time and periodicity of the controllers and controller_manager

[Diagnostics] Add diagnostics of execution time and periodicity of the controllers and controller_manager #590