Fix the controller deactivation on the control cycles returning ERROR

[saikishor]

Right now, we are just deactivating the controllers when we have an ÈRROR` returned from the hardware or from the controller update cycles, but there could be 2 different issues with this approach:

• When deactivating the controller in EFFORT mode it is not safe as any unsafe command it left in the interface and the hardware continues to maintain it. It is dangerous in some situations
• When performing this deactivation, the consecutive activation fails as the resources are currently occupied from the hardware point of view.

This PR proposes a solution for the above issues and a test is added to check for the proper functionality

[codecov]

Codecov Report

Attention: Patch coverage is 87.95181% with 20 lines in your changes missing coverage. Please review.

Project coverage is 86.79%. Comparing base (84e85f9) to head (cabe347).

Files with missing lines	Patch %	Lines
..._components/test_actuator_exclusive_interfaces.cpp	83.82%	10 Missing and 1 partial ⚠️
controller_manager/src/controller_manager.cpp	86.84%	1 Missing and 4 partials ⚠️
...ontroller_manager/test/test_release_interfaces.cpp	93.10%	4 Missing ⚠️

Additional details and impacted files

@@           Coverage Diff            @@
##           master    #1756    +/-   ##
========================================
  Coverage   86.79%   86.79%            
========================================
  Files         116      117     +1     
  Lines       10715    10868   +153     
  Branches      981      994    +13     
========================================
+ Hits         9300     9433   +133     
- Misses       1062     1077    +15     
- Partials      353      358     +5     

Flag	Coverage Δ
unittests	86.79% <87.95%> (+<0.01%)	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...r_manager/test/test_controller/test_controller.cpp	95.55% <100.00%> (+0.20%)	⬆️
...r_manager/test/test_controller/test_controller.hpp	100.00% <ø> (ø)
...ontroller_manager/test/test_release_interfaces.cpp	90.35% <93.10%> (+2.85%)	⬆️
controller_manager/src/controller_manager.cpp	77.53% <86.84%> (+0.05%)	⬆️
..._components/test_actuator_exclusive_interfaces.cpp	83.82% <83.82%> (ø)

[christophfroehlich]

Could you please briefly summarize your solution for me?

[saikishor]

Could you please briefly summarize your solution for me?

@christophfroehlich sure!

Right now, we have the above issues because the prepare_command_switch and perform_command_swirch are not called when deactivating the controller. So, the hardware is not aware of the resources not being used any more.

Now, I've added prepare_command_switch and perform_command_switch, so that the hardware will be aware of the changes and act accordingly. In our case, if the robot is EFFORT, FORCE or TORQUE control mode, we change the actuator to IDLE mode, if there are no more interfaces to activate for those motors. This way it is safer.

Moreover, as now the hardware is aware of the deactivated resources, when you try to activate the same controller again or a different controller that uses different resources of that joint, it will be able to properly activate the controller.