implemented perform_command_mode_switch override in Ros2ControlSystem

[ralphieraccoon]

Description
Reusing the solution from here for Gazebo to allow for multiple command interfaces. Implements the perform_command_mode_switch method override to set joint control methods when controllers are activated/deactivated. Can't work out how to detect if a command interface is claimed inside Ros2ControlSystem::init() so you either have to run switch_controllers once to call the function or start with a position controller.

Related Issues
This pull request references the discussion #845 I started earlier.

Affected Packages

• webots_ros2_control

Tasks

• Work out how to detect if a command interface is claimed inside Ros2ControlSystem::init() so it's all set up properly at the beginning without having to run switch_controller.

Additional context
Credit to @ksotebeer for the original PR for gazebo_ros2_control.

[ralphieraccoon]

I'm not sure if discovering which command interfaces are claimed inside Ros2ControlSystem::init() is possible since it doesn't provide access to the ResourceManager which has this function. To make it work might require changes to the ros2_control base class. I can take this PR out of draft as is and the problem could be solved at a later date, what would be best?

[omichel]

I would prefer that you test and eventually solve the problem in this PR, rather than committing something which is unfinished or buggy.

[ralphieraccoon]

I would prefer that you test and eventually solve the problem in this PR, rather than committing something which is unfinished or buggy.

That's fair, though at the moment I'm not sure the issue is fixable without alterations to ros2_control. The PR doesn't break existing functionality, if a single command interface is used it works the same as before, and the issue can be worked around by calling switch_controllers. If anyone has any suggestions on how to solve the issue, it would be greatly appreciated.