Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enable usage of GenericHWInterfaces inside a combined_robot_hw #49

Draft
wants to merge 4 commits into
base: noetic-devel
Choose a base branch
from

Conversation

RobertWilbrandt
Copy link
Collaborator

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.

The new class is a pure adapter which forwards the whole
RobotHW interface to an internal GenericHWInterface. It provides the
interface required for combind_robot_hw usage, in particular:
- A parameterless constructor, needed for dynamicly loading the HW
  interface
- An init function with the correct RobotHW signature, which does the
  full GenericHWInterface initialization
This will use a combined_robot_hw to simulate an rrbot using two
seperate SimHWInterfaces.
@RobertWilbrandt
Copy link
Collaborator Author

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.

Correctly export plugin description and sort TARGET list
@Tuebel
Copy link

Tuebel commented Dec 14, 2020

  • 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
Copy link
Collaborator Author

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
Copy link

Tuebel commented Dec 15, 2020

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
Copy link
Collaborator Author

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
Copy link
Collaborator Author

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
Copy link

Tuebel commented Dec 16, 2020

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

Export SimHWInterface for combined_robot_hw
2 participants