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 88.46154% with 18 lines in your changes missing coverage. Please review.

Please upload report for BASE (master@4eabd25). Learn more about missing BASE report.
Report is 2 commits behind head on master.

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

Additional details and impacted files

@@            Coverage Diff            @@
##             master    #1756   +/-   ##
=========================================
  Coverage          ?   87.27%           
=========================================
  Files             ?      122           
  Lines             ?    12253           
  Branches          ?     1093           
=========================================
  Hits              ?    10694           
  Misses            ?     1163           
  Partials          ?      396           

Flag	Coverage Δ
unittests	87.27% <88.46%> (?)

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

Files with missing lines	Coverage Δ
controller_manager/src/controller_manager.cpp	77.62% <90.00%> (ø)
...ontroller_manager/test/test_release_interfaces.cpp	90.35% <93.10%> (ø)
..._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.

[christophfroehlich]

Thanks for the explanation. The changes and the test looks fine for me.

[mergify]

This pull request is in conflict. Could you fix it @saikishor?

[destogl]

Isn't this in the update method? Can we do this ahead of time?

[saikishor]

@destogl I was handling it in a different PR: #1801