Fix exclusive hardware control mode switching on controller failed activation

[saikishor]

If the hardware supports only one active control mode at a time and when there is a failing controller activation, ros2_control is not propagating this information to the hardware, and the hardware might have the resource locked internally. This PR allows to solve such situations as explained in #1487 and #1486.

Fixes #1487
Closes #1492

[saikishor]

@firesurfer This PR has the fix that should solve your issue. If you can test it on your hardware and let us know, it would be great.
I could reproduce the same issue with a test and I added a fix, so that the test passes, so I'm confident that this might work for you.

[codecov]

Codecov Report

❌ Patch coverage is 84.61538% with 8 lines in your changes missing coverage. Please review.
✅ Project coverage is 89.32%. Comparing base (56fc052) to head (1037747).
⚠️ Report is 1 commits behind head on master.

Files with missing lines	Patch %	Lines
...ailed_activate/test_controller_failed_activate.cpp	66.66%	4 Missing ⚠️
controller_manager/src/controller_manager.cpp	80.00%	1 Missing and 1 partial ⚠️
...ontroller_manager/test/test_release_interfaces.cpp	91.30%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1522      +/-   ##
==========================================
+ Coverage   89.31%   89.32%   +0.01%     
==========================================
  Files         145      147       +2     
  Lines       16541    16589      +48     
  Branches     1396     1396              
==========================================
+ Hits        14773    14818      +45     
- Misses       1231     1234       +3     
  Partials      537      537              

Flag	Coverage Δ
unittests	89.32% <84.61%> (+0.01%)	⬆️

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

Files with missing lines	Coverage Δ
...ailed_activate/test_controller_failed_activate.hpp	100.00% <100.00%> (ø)
controller_manager/src/controller_manager.cpp	75.41% <80.00%> (+0.23%)	⬆️
...ontroller_manager/test/test_release_interfaces.cpp	90.51% <91.30%> (+0.16%)	⬆️
...ailed_activate/test_controller_failed_activate.cpp	66.66% <66.66%> (ø)

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[firesurfer]

@saikishor Thanks a lot. Just for my understanding: If a controller fails activation now, a command mode switch is issued so that the hardware interface can release internally locked hardware?

I try to test this patch with an iron based setup.

[firesurfer]

@saikishor I just tested your PR and for me it didn't work so far.

It could be that I did something wrong. What I did was:

1. Clone your repository into by workspace
2. Rebase your branch onto iron with: git rebase fix/exclusive_hw_interface_switching --onto=iron
3. Build it with: --allow-overriding controller_interface controller_manager hardware_interface ros2_control_test_assets hardware_interface_testing
4. Change my controller to fail activation

  on_activate()....{
      release_interfaces();
      return controller_interface::CallbackReturn::FAILURE;

5. Source it and run it

When I activate the controller it fails as it should. But when I then try to load another controller I run into the same issue as before - the interfaces haven't been stopped.

[saikishor]

@saikishor Thanks a lot. Just for my understanding: If a controller fails activation now, a command mode switch is issued so that the hardware interface can release internally locked hardware?

I try to test this patch with an iron based setup.

@firesurfer yes you are right! The hardware will be told to stop the interfaces that are started for the hardware that the controller needed. This should be good for the hardware component to internally release the locked control modes

[saikishor]

@firesurfer can you share the logs here?. Can you print something in your hardware interfaces to see if you receive the stop interfaces?. Basically, if your controller fails activation, it should immediately receive to release interfaces even before activating a new one. If you go over tests, I'm testing exactly the same

[saikishor]

@firesurfer for initial testing, maybe you can remove the installed dependencies and this might help. For me, after removing the installed dependencies, it worked

[firesurfer]

So I removed the installed packages:
sudo dpkg --force-all --remove ros-iron-controller-interface ros-iron-controller-manager ros-iron-hardware-interface just to make sure the ones from the workspace are used.

Log while failing activation:

[ros2_control_node-14] [ERROR] [1714461945.456711669] [controller_manager]: After activation, controller 'my_controller' is in state 'inactive' (2), expected 'active' (3).

Log during activation of a ForwardController for the same joints after the other one has failed:

[ros2_control_node-14] [ERROR] [1714461952.579709743] [my_hardware]: Joint already in used: my_joint [ros2_control_node-14] [ERROR] [1714461952.579871675] [resource_manager]: Component 'my_hardware' did not accept command interfaces combination:

I added a log message in the perform_command_mode_switch and did not get any log message.

I am not sure if perhaps rebasing on Iron broke something.

[saikishor]

So I removed the installed packages: sudo dpkg --force-all --remove ros-iron-controller-interface ros-iron-controller-manager ros-iron-hardware-interface just to make sure the ones from the workspace are used.

Log while failing activation:

[ros2_control_node-14] [ERROR] [1714461945.456711669] [controller_manager]: After activation, controller 'my_controller' is in state 'inactive' (2), expected 'active' (3).

Log during activation of a ForwardController for the same joints after the other one has failed:

[ros2_control_node-14] [ERROR] [1714461952.579709743] [my_hardware]: Joint already in used: my_joint [ros2_control_node-14] [ERROR] [1714461952.579871675] [resource_manager]: Component 'my_hardware' did not accept command interfaces combination:

I added a log message in the perform_command_mode_switch and did not get any log message.

I am not sure if perhaps rebasing on Iron broke something.

Hello @firesurfer!

It seems like you are not using the changes, because If you use the changes of this branch, your error should be like the following

[ros2_control_node-14] [ERROR] [1714461945.456711669] [controller_manager]: After activation, controller 'my_controller' is in state 'inactive' (2), expected 'active' (3). Releasing Interfaces!

ros2_control/controller_manager/src/controller_manager.cpp

Lines 1509 to 1515 in 4a4d780

	RCLCPP_ERROR(
	get_logger(),
	"After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d). Releasing "
	"interfaces!",
	controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(),
	hardware_interface::lifecycle_state_names::ACTIVE,
	lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

In your error log, it doesn't mention about the Releasing interfaces, so please crosscheck your setup.

Thank you!

[firesurfer]

@saikishor I made a mistake while rebasing onto iron. When I do it properly I run into a lots of conflicts. When I try to resolve them it doesn't compile anymore.

Do you have an recommendations how to easily test this with an iron setup ?

[christophfroehlich]

You should be able to compile the rolling stack (also this PR) on your iron distro directly. override should also work, no need to uninstall your binary install. (check with ros2 pkg xml ros2_control -t version)
edit: can be that you have to patch your hardware component to the new API

[saikishor]

You should be able to compile the rolling stack (also this PR) on your iron distro directly. override should also work, no need to uninstall your binary install. (check with ros2 pkg xml ros2_control -t version) edit: can be that you have to patch your hardware component to the new API

Thank you @christophfroehlich. Yes, you can do this. In case you continue to have issues, then try to just cherry-pick the last 2 commits of this PR (77e932e and 4a4d780) onto your iron branch

[firesurfer]

@saikishor Thanks. Cherry-picking worked :)

When I now have the controller failing I run into an endless loop of ros2control trying to activating the controller - failing - stopping the interfaces - starting them again - failing again...

[ros2_control_node-14] [ERROR] [1714469097.477339381] [controller_manager]: After activation, controller 'my_controller' is in state 'inactive' (2), expected 'active' (3). Releasing interfaces!

But I can confirm. The interfaces are stopped!.

[saikishor]

@saikishor Thanks. Cherry-picking worked :)

When I now have the controller failing I run into an endless loop of ros2control trying to activating the controller - failing - stopping the interfaces - starting them again - failing again...

[ros2_control_node-14] [ERROR] [1714469097.477339381] [controller_manager]: After activation, controller 'my_controller' is in state 'inactive' (2), expected 'active' (3). Releasing interfaces!

But I can confirm. The interfaces are stopped!.

@firesurfer I'm glad that the fix worked for you. If that's the case, do you mind reviewing this PR and approving it?

[firesurfer]

@saikishor shouldn't be the issue of running into an endless loop of activation -> failing... be addressed first?

[saikishor]

@saikishor shouldn't be the issue of running into an endless loop of activation -> failing... be addressed first?

@firesurfer I don't really see how there is an endless loop. CM never reactivates or tries if it is failing. Maybe the print you have added is printing many times, because you have this component per joint and it is trying to iterate over the HW components to do the prepare and perform switch. This is the behavior inside the Resource Manager and I'm not touching that part.

EDIT: How many times does it print the ....Releasing Interfaces! error?

[saikishor]

@firesurfer I can confirm after testing it again on our setup that the fix works and there is no looping activation from the CM side.

Thank you

[firesurfer]

@saikishor

EDIT: How many times does it print the ....Releasing Interfaces! error?

As I am seeing the activation failure message again and again (of the controller) I would say this is not just because of the printout in the hardware_interface. I had it running for like 10-20s before killing the process.

I am wondering if that could be because I cherry-picked it on the iron branch?

[saikishor]

As I am seeing the activation failure message again and again (of the controller) I would say this is not just because of the printout in the hardware_interface. I had it running for like 10-20s before killing the process.

I am wondering if that could be because I cherry-picked it on the iron branch?

That would be strange to have this in the iron branch. Can you share some logs?

EDIT: Can you also share how you are trying to activate this controller?

[firesurfer]

@saikishor I just tested in a clean dockerized environment instead of my host installation and it seems to work fine.

I will review and approve the PR then.

[github-actions]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

[mergify]

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

[christophfroehlich]

I'll merge this as CI is fine