Modify resource manager logic to support multiple hardware components

[chama1176]

What does this implement/fix?

The current implementation requires that all components have the appropriate interface and will not work well if the hardware is split into multiple components.
It should only be necessary for one component to have an interface, so we will modify the logic accordingly.

Specifically, we are considering the following examples.
If you want to connect to follower_joint1/effort, it should be no problem if component 1 has the corresponding interface.

Hardware Component 1
        command interfaces
                follower_joint1/position [available] [unclaimed]
                follower_joint1/effort [available] [claimed]
Hardware Component 2
        command interfaces
                leader_joint1/position [available] [unclaimed]
                leader_joint1/effort [available] [claimed]

Does this close any currently open issues?

I don't have Franka Emika Panda so I am not able to reproduce the same situation completely,
but this PR may solve this issue.
#1177

[chama1176]

@christophfroehlich
I had missed the test. I have corrected it, could you please check again.

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 75.91%. Comparing base (786d5b5) to head (eaed55d).
Report is 200 commits behind head on master.

Additional details and impacted files

@@             Coverage Diff             @@
##           master    #1391       +/-   ##
===========================================
+ Coverage   47.49%   75.91%   +28.41%     
===========================================
  Files          41       41               
  Lines        3556     3363      -193     
  Branches     1938     1937        -1     
===========================================
+ Hits         1689     2553      +864     
+ Misses        459      417       -42     
+ Partials     1408      393     -1015     

Flag	Coverage Δ
unittests	75.91% <100.00%> (+28.41%)	⬆️

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

Files with missing lines	Coverage Δ
hardware_interface/src/resource_manager.cpp	76.99% <100.00%> (+28.16%)	⬆️

... and 31 files with indirect coverage changes

[bmagyar]

I'm sorry but I don't get the explanation you've provided.

From the code I'm guessing you are trying to implement something like "BEST_EFFORT" as defined in this message?

[chama1176]

@bmagyar
Thanks for checking the code.
I didn't know about that BEST_EFFORT setting, but it sounds a little different than what I want to do.

In the following example, two hardware components have different interfaces.

Hardware Component 1
        command interfaces
                follower_joint1/position [available] [unclaimed]
                follower_joint1/effort [available] [claimed]
Hardware Component 2
        command interfaces
                leader_joint1/position [available] [unclaimed]
                leader_joint1/effort [available] [claimed]

Suppose we have a controller with interfaces follower_joint1/effort and leader_joint1/effort, In the current implementation, each hardware component is checked to see if it has interfaces(follower_joint1/effort and leader_joint1/effort),
And if any component does not have an interface, it fails. Therefore, it cannot be used in this case.

[destogl]

I don't get the issue here. It seems to me that he Hardware Interface is not accepting this combination which is poor implementation of HW interface. So I see here nothing to fix.

We can discuss if we should give a component just joint that component has. This might be a good, but not so simple solution.

Can you write a test for this because I don't really understand the issue

[christophfroehlich]

@chama1176 any updates on this topic?

[chama1176]

@christophfroehlich
No...
The HW interface used for testing in this repository is capable of accepting multiple command interfaces.
On the other hand, in my environment, one HW interface accepts only one command interface. I think this is a common implementation in actual operation when you don't want different command values coming from multiple controllers.
I have not yet been able to suggest a good test case because of that difference

[atharva620]

Hello all, I am facing this exact issue. Specifically, when two hardware components are active at the same time, a controller trying to claim joint command interfaces from HW component 1, also mis-triggers a call to prepare_command_mode_switch in HW component 2 (and vice-versa) leading to the controller_manager node erroring out. I am willing to try out the fix proposed by @chama1176 but I have ros2_control installed as a binary. Does uninstalling the binary and installing it from source sound like a good idea to test this fix? Please advise.

[christophfroehlich]

You can compile and install ros2_control when building from source in your workspace, you don't have to remove the binary before. After sourcing your workspace, check with "ros2 pkg xml -t version" if the correct version was loaded.

[atharva620]

Okay so I tried this out and from what I can understand, the fix proposed here basically gets around the issue instead of resolving it. Specifically, the fix proposed here lets command_mode_switch happen for all hardware components if atleast 1 of them accepts the combination of start_interfaces and stop_interfaces, thereby disregarding an important question of whether or not other hardware components actually have the command interfaces that the controller is trying to claim through start_interfaces and stop_interfaces. This is the reason multiple components would call command_mode_switch whether or not they have those joints, leading to the CM node erroring out.

Instead what should actually happen as @destogl rightly pointed out above is we should only let a hardware component perform command_mode_switch if it has those joints in the first place. I can imagine while looping through hardware components, we can add conditions which let command_mode_switch happen only for a hardware component for which atleast one of the following is true:

1. Does the hardware component have either start_interfaces or stop_interfaces? If yes, and if atleast one of those are not empty, then only let that specific component call command_mode_switch. This would cover the case for a controller activation/deactivation call.
2. Does the hardware compoent have both start_interfaces and stop_interfaces? If yes, let that hardware component call command_mode_switch. This would cover the case for a switching of control modes.
3. Finally, let command mode switch happen if both start_interfaces and stop_interfaces are empty. I believe this would cover the case for joint_state_broadcaster controllers?

Please let me know if this sounds like a good, high level idea for the fix.

[chama1176]

@atharva620
Thank you for the proposal for the new implementation!
The logic is more complex because each component is handled differently, but it feels like a better solution.

[mergify]

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