Make sure that controllers are properly sorted and executed in a chain

[destogl]

Is your feature request related to a problem? Please describe.

When loading controllers that are part of the chain, they have to be loaded in a certain order. Otherwise, there could be a lag in execution.

Describe the solution you'd like
Controllers are resorted in a proper order and calculations are correct.

Additional context

Check doc about controllers' chaining here: https://github.com/ros-controls/roadmap/blob/08d1454cd0d75e4f88cf16a05dad824c69caa302/design_drafts/cascade_control.md

1. Start writing a test for it that shows wrong calculation of controller, i.e., in the current state the test should fail.
   • Add here: https://github.com/ros-controls/ros2_control/blob/master/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp#L978
   • Start with coping the test test_chained_controllers and reorder controllers in add_controller block – the test should start to fail because calculation will not be correct.
2. Adjust controller manager when controller is getting loaded so that controllers are reordered in the controller's lists:
   • Add the solution code here: https://github.com/ros-controls/ros2_control/blob/master/controller_manager/src/controller_manager.cpp#L485
   • Helpful logic: https://github.com/ros-controls/ros2_control/blob/master/controller_manager/src/controller_manager.cpp#L620-L746

[sgmurray]

I will work on this.

[saikishor]

Hello @destogl and @sgmurray!

I would like to contribute to this. Shall I continue based on yours or shall I create a new one?
I would like to have the controllers with only state interface update first and then one of the chainable controllers and then comes the controllers with command interfaces. What do you think?.

Best Regards,
Sai

[bmagyar]

Please do! I think it's a sensible approach.
It is up to you whether to pick up the existing work or not. Please don't forget adding tests ;)

[saikishor]

Please do! I think it's a sensible approach. It is up to you whether to pick up the existing work or not. Please don't forget adding tests ;)

Sure!. Thank you!