release_interfaces when stopping controller

[mahaarbo]

Following up on the comment by @olivier-stasse #322/comment, switching a controller does not appear to work as expected when there is a resource conflict between the controllers.

Reproduction instruction

In ros2_control_demos, add an additional forward_position_controller2 to the config, that is basically a copy of forward_position_controller. Launch rrbot_system_position_only.launch.py, and run:

ros2 control load_configure_controller forward_position_controller
ros2 control load_configure_controller forward_position_controller2
ros2 control switch_controllers --start-controllers forward_position_controller

Then running ros2 control switch_controllers --start-controllers forward_position_controller2 --stop-controllers forward_position_controller yields the error message:

[controller_manager]: Resource conflict for controller 'forward_position_controller2'. Command interface 'joint1/position' is already claimed.

The same occurs if the command is split into two separate calls, one stopping the old and one starting the new controller.

Cause

There is no clearing of the LoanedCommandInterface when stopping a controller. The function exists (controller->release_interfaces()) but it does not appear to be called by the controller_manager. The only way to clear the interfaces at the moment is to unload the controller. I'm not sure if that is the desired behavior.

Fix

Release the interfaces when stopping a controller.

[bmagyar]

This looks good to me, probably fixing #345 too.

@destogl or @Karsten1987 please feel free to merge if you agree

[mahaarbo]

Is this the intended behavior though?
#345 brings up a good question, conceptually, should release_interfaces be handled by the controller_manager or the controller developer?
Looking at the current state of ros2_controllers we have:

• diff_drive_controller releases interfaces in reset
• effort_controller is a wrapper around forward_command_controller
• forward_command_controller does not release the interfaces (which is probably where most who test the code may have a problem)
• joint_trajectory_controller releases the interfaces in on_deactivate
• joint_group_position_controller is a wrapper around forward_command_controller
• joint_group_velocity_controller is a wrapper around forward_command_controller

so the issue can also be handled by clearing the interfaces in forward_command_controller.

According to the roadmap the controller manager "manages (e.g., loading, activating, deactivating, unloading) controllers and from them required interfaces" (roadmap/ros2_control_documentation) which sounds to me as though it should also release the interfaces when no longer needed.

[mahaarbo]

I've moved the release_interfaces to after deactivate. If the controller developer needs to perform any sanity checks or stopping procedures, the user can implement these in the deactivate function in the controller. If nothing is needed to be done, the controller manager will ensure that the interfaces are released upon deactivation.

[bmagyar]

Intended and actually good and smooth may be two different things. I think leaving releasing interfaces to whoever implements the controllers is a problem, the controller manager is meant to manage controllers ;)

[Karsten1987]

as per @ssumoo comment:

In short, the question is: why does neither of the following happen?:

• The controller manager calls the controller's release_interfaces in its stop_controllers function
• The controller calls its own release_interfaces in its deactivate function

I think both of the cases are valid. The beauty of the move semantic is that the command interfaces really only exist once and thus it doesn't really matter when they're released.
That being said, there's no way around calling release when the controller is being deactivated in order to make a switch of controllers happen. Given that we call assign within the controller manager, I believe it makes similarly sense to call release from within the controller manager. That should still transparently work even if the controller at random times decides to release the interfaces itself. Nevertheless, I believe we should come up with a follow up PR to remove the release calls on the existing controllers.

@mahaarbo I think your patch is ok, however I am a bit shocked that we didn't have these scenarios covered in the existing tests for switching controllers. As @bmagyar mentioned over here I think this should definitely be covered by tests. I think the existing test should be eligible for adding another test case for switching controllers with potential resource conflict.

[Karsten1987]

the patch itself looks good, I am requesting changes due to a definitely needed test case. Let me know if you need assistance with it.
Essentially what I'd like to see in this test case:

• Load controller_1 with command_interface_1.
• Load controller_2 with command_interface_1.
• Start controller_1 --> ok
• Start controller_2 --> false, resource conflict
• Stop controller_1 --> ok
• Start controller_2 --> ok
• Stop controller_2 --> ok
• Start controller_1 --> ok

[bmagyar]

2 cents: controller_1 and controller_2 need not be different at all, a different name is enough for the test to reproduce the issue.

As Karsten said, let us know if you need help with the test, we'd like to get this in ASAP.

[mahaarbo]

Thanks for the comments, working on the test case described by @Karsten1987 at the moment. I'm not very well-versed with gtest, and much of the ros2_control codebase, so any help is greatly appreciated. I've made a new test named test_release_interfaces, but it times out after 60 s on trying to switch to the first controller. It's probably some mistake in my test controller.

The existing test_controller does not claim any command interfaces, so I tried to make one that claims the joint2/velocity command interface, which should be available through the example URDF in controller_manager_test_common, I've uploaded my failing attempts if anyone has any good tips.

Edit: I hadn't noticed that I need to call cm_->update, and thought it was sufficient to run cm_->switch_controller. Now it runs, and I've added an additional test case where we try to start both controllers at the same time. This returns a success, so I've tried to just mention that through an RCLCPP_INFO. The file became quite long, but I can move the test case to test_controller_manager_srvs.cpp if you prefer to have it in there.

[destogl]

@mahaarbo Great job! Thanks!

P.S. just a few nitpicks if you are in the mood :)

[Karsten1987]

I've added a small commit on top of your branch which highlights a bit more what's exactly being tested. I hope you don't mind, but I feel it helps for understanding what's going on.

Overall, thanks for picking up the test! Good job!