Enable usage of GenericHWInterfaces inside a combined_robot_hw

[RobertWilbrandt]

This creates an adapter class for GenericHWInterfaces which allows usage inside a combined_robot_hw. This adapter is then used to export SimHWInterface.

For demonstration, an additional rrbot demo is created. While creating this, some restructuring of rrbot_control was needed, which should reflect its function more clearly.

This starts as a draft, as i want to add more documentation and explore the possibility of a "generic" combined_robot_hw_runner node (or similar) in ros_control_boilerplate.

Fixes #34. Continuation of #42.

[RobertWilbrandt]

There were two issues from the discussion in #42 remaining:

• Empty controller list: @Tuebel after reading about this again i am not quite sure i understand exactly where you still get the empty controller list. In which functions do you see this empty list? In the doSwitch() function?
• As for the namespacing: I agree that this is inconsistent between the direct usage of a GenericHWInterface and usage inside a combined_robot_hw, but i don't think there is any way to fix this without a breaking change for most setups. Some of the uncertainty around this might stem in an error of mine in therrbot_control config files (some of the settings for the simple GenericHWInterface case were loaded even in the combined_robot_hw case), i tried to remedy this in the restructuring of rrbot_control config files. Could you try if a similar structure works for you? I don't think there is any need for absolute paths in this use case.

[Tuebel]

• I see the empty start_list in both functions: prepareSwitch and doSwitch.
• Yes, this structure works for me. Right now, I use the same name for the robot_hardware parameter and the namespace of robot_nh in the standalone node. This way, one config file works for both the standalone and combined hardware.

[RobertWilbrandt]

I just added some small debug output to these functions in SimHWInterface and could reproduce this issue with the upstream ros_control package. Running roslaunch ros_control_boilerplate rrbot_combined_simulation.launch yielded:

[...]
Loaded 'joint_state_controller'
Loaded 'position_trajectory_controller'
[ INFO] [1607972663.230785821]: prepareSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_1_hw):
[ INFO] [1607972663.230828164]: prepareSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_2_hw):
[ INFO] [1607972663.234206317]: doSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_1_hw):
[ INFO] [1607972663.234236261]: doSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_2_hw):
Started ['joint_state_controller'] successfully
[ INFO] [1607972663.237278510]: prepareSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_1_hw):
[ INFO] [1607972663.237313109]: prepareSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_2_hw):
[ INFO] [1607972663.237519892]: doSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_1_hw):
[ INFO] [1607972663.237551139]: doSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_2_hw):
Started ['position_trajectory_controller'] successfully
[...]

, but building ros_control from source (either melodic-devel or noetic-devel works correctly:

[...]
Loaded 'joint_state_controller'
Loaded 'position_trajectory_controller'
[ INFO] [1607972580.227317489]: prepareSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_1_hw):
[ INFO] [1607972580.227387877]: prepareSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_2_hw):
[ INFO] [1607972580.230209118]: doSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_1_hw):
[ INFO] [1607972580.230268160]: doSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_2_hw):
Started ['joint_state_controller'] successfully
[ INFO] [1607972580.234556711]: prepareSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_1_hw):
[ INFO] [1607972580.234610032]:   + position_trajectory_controller
[ INFO] [1607972580.234680437]: prepareSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_2_hw):
[ INFO] [1607972580.234709043]:   + position_trajectory_controller
[ INFO] [1607972580.236852555]: doSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_1_hw):
[ INFO] [1607972580.236889973]:   + position_trajectory_controller
[ INFO] [1607972580.236938568]: doSwitch (nh: /rrbot/rrbot_combined_hardware_interface/joint_2_hw):
[ INFO] [1607972580.236963784]:   + position_trajectory_controller
Started ['position_trajectory_controller'] successfully
[...]

can you verify again that

1. your prepareSwitch and doSwitch gets called
2. the lists are empty
3. You are on either melodic-devel or noetic-devel of ros_control

I don't doubt you did this correctly, i am just really confused as to what else could cause this problem.

If this problem persists, there should be a workaround: Changing the registerInterfaceManager call in CominableGenericHW<...>::init with these calls

registerInterface(adapted_hw_interface_->get<hardware_interface::JointStateInterface>());
registerInterface(adapted_hw_interface_->get<hardware_interface::PositionJointInterface>());
registerInterface(adapted_hw_interface_->get<hardware_interface::VelocityJointInterface>());
registerInterface(adapted_hw_interface_->get<hardware_interface::EffortJointInterface>());

does this work for you?

As for the naming: That's kind of clever, i might just steal that in future projects...

[Tuebel]

Maybe there is a bit of an misunderstanding here as my results are similar to yours. The funcitons prepareSwitch and doSwitch get called with a non-empty start_list but then again with an empty list. This seems to depend on the order in which the controllers are loaded. For the following results I have checked out this pull request, ros_control noetic-devel and launched the rrbot_combined_simulation.launch. I did add the logging.

<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false" output="screen"
args="spawn position_trajectory_controller joint_state_controller" />

Gives:

Loaded 'position_trajectory_controller'
Loaded 'joint_state_controller'
[ INFO] [1608049123.583915146]: prepareSwitch start_list size: 1
[ INFO] [1608049123.584019505]: prepareSwitch start_list size: 0
[ INFO] [1608049123.584666694]: doSwitch start_list size: 1
[ INFO] [1608049123.584742755]: doSwitch start_list size: 0
Started ['position_trajectory_controller'] successfully
[ INFO] [1608049123.592145302]: prepareSwitch start_list size: 0
[ INFO] [1608049123.592213115]: prepareSwitch start_list size: 0
[ INFO] [1608049123.594641366]: doSwitch start_list size: 0
[ INFO] [1608049123.594706177]: doSwitch start_list size: 0
Started ['joint_state_controller'] successfully

Swapping the order as:

<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false" output="screen"
args="spawn joint_state_controller position_trajectory_controller" />

leads to:

Loaded 'joint_state_controller'
Loaded 'position_trajectory_controller'
[ INFO] [1608049530.806072379]: prepareSwitch start_list size: 0
[ INFO] [1608049530.806140649]: prepareSwitch start_list size: 0
[ INFO] [1608049530.809190649]: doSwitch start_list size: 0
[ INFO] [1608049530.809245116]: doSwitch start_list size: 0
Started ['joint_state_controller'] successfully
[ INFO] [1608049530.812512765]: prepareSwitch start_list size: 1
[ INFO] [1608049530.812562018]: prepareSwitch start_list size: 0
[ INFO] [1608049530.815827487]: doSwitch start_list size: 1
[ INFO] [1608049530.815880670]: doSwitch start_list size: 0

In my setup I have stored the controller yamls separately for the robots. Thus, I multiple controllers instead of a single one. Splitting up the controller for the rrbot and spawning them via:

<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false" output="screen"
args="spawn position_trajectory_controller_1 position_trajectory_controller_2 joint_state_controller" />

leads to:

Loaded 'position_trajectory_controller_1'
Loaded 'position_trajectory_controller_2'
Loaded 'joint_state_controller'
[ INFO] [1608049761.242010679]: prepareSwitch start_list size: 1
[ INFO] [1608049761.242089090]: prepareSwitch start_list size: 0
[ INFO] [1608049761.245403474]: doSwitch start_list size: 1
[ INFO] [1608049761.245460553]: doSwitch start_list size: 0
Started ['position_trajectory_controller_1'] successfully
[ INFO] [1608049761.250531467]: prepareSwitch start_list size: 0
[ INFO] [1608049761.250588287]: prepareSwitch start_list size: 1
[ INFO] [1608049761.251965187]: doSwitch start_list size: 0
[ INFO] [1608049761.252011527]: doSwitch start_list size: 1
Started ['position_trajectory_controller_2'] successfully
[ INFO] [1608049761.255328591]: prepareSwitch start_list size: 0
[ INFO] [1608049761.255378649]: prepareSwitch start_list size: 0
[ INFO] [1608049761.258653497]: doSwitch start_list size: 0
[ INFO] [1608049761.258698951]: doSwitch start_list size: 0
Started ['joint_state_controller'] successfully

Which is similar to what I got for my setup earlier.

[RobertWilbrandt]

Ah now i understand the problem. The specific issue we see in every example is that the starting procedure calls prepareSwitch()/doSwitch() with empty lists for joint_state_controller and otherwise correctly splits the lists between both hardware interfaces. I did not even think about this behavior, but the cause for this is actually an inconsistency in the combined_robot_hw implementation.

In both of these functions, combined_robot_hw tries to figure out which hardware interfaces get affected by a controller switch. For this it looks at the list of claimed resources (mostly = joints) by the processed controller (a controller typically claims a resource if it wants to move it, this prevents multiple controllers from trying to move a joint at the same time). In your last example the controllers claim:

• position_trajectory_controller_1 and position_trajectory_controller_2 both claim one joint each. You can see the result of this: Both hardware interfaces receive a call for each starting controller, but only the hardware interface actually "owning" the particular joint sees the controller in its start_list.
• joint_state_controller does not attempt to control any joint, as it only reads and publishes the joint states. This is why it does not claim any resources and combined_robot_hw doesn't think any hardware interface needs to get notified about it getting loaded.

This is obviously inconsistent with the normal behavior, as a controller_manager doesn't do any of this filtering. I never realized this was actually inconsistent, as i only ever saw usage of these functions to check e.g. if the robot needs to change any internal state when moving from e.g. position-based goals to velocity-based goals (this always worked, as only controllers that would claim resources were relevant in these cases).

I will open an issue in ros_control about this, but don't know if there is any reasonable change that could be made to fix this - Not doing the filtering at all would probably break some existing code that just checked the type of interfaces affected and changing the behavior of controller_manager would probably be equally problematic.

In any case, i don't think this issue should block further development/merging of this feature, as the issue is in combined_robot_hw itself and not this specific implementation. You mentioned that you can just ignore this case - Is this procedure okay for you?

[RobertWilbrandt]

Well i huddled up some of the specifics in the explanation: The controllers get handles to interfaces that claim resources (position_trajectory_controller_X gets a handle from a PositionJointInterface which is claiming, while joint_state_controller only gets handles from a JointStateInterface, which is not claining). But the basic idea of controllers not claiming resources getting ignored in the start/stop lists should be correct.

[Tuebel]

Thanks for the explanation, now I understand the underlying issue.
For me it is totally fine to proceed with this feature as the empty start_list is easy to ignore. I also don't see how the empty lists would harm anyone else when iterating through it. More problematic would have been if an incorrect list was passed to the functions.