Support for combined_robot_hw::CombinedRobotHW

[Tuebel]

First of all thanks for this library, it really helped setting up my custom gripper.
After a while I figured out that I need to use the combined_robot_hw::CombinedRobotHW if I want to pair my gripper with my robot arm.
However, the current implementation of the ros_control_boilerplate::GenericHWInterface is not compatible with it:

• the constructor is not parameterless (required for pluginlib)
• init on the other does not have the required NodeHandle parameters of hardware_interface::RobotHW
• read and write are fine since the original hardware_interface::RobotHW signature calls the simplified versions

Modifying the original ros_control_boilerplate::GenericHWInterface will certainly break the old API.
Thus, I have written a separate CombinableGenericHWInterface (is GenericCombinableHWInterfacebetter?) and opened this pull request.
A few things bother me which I would like to discuss:

1. Currently we loose the support, to load an URDF from another location than the robot_description parameter. Of course, a subclass could inject a custom URDF in its constructor and CombinableGenericHWInterface::init could check if it is available. However, this behaviour is a hidden side effect. An alternative is to create a new loadURDF method.
2. Most of the code is copy paste with the major difference of doing most of the work from the constructor in init. We could either use the CombinableGenericHWInterface as base and subclass the GenericHWInterface from it in an adapter pattern fashion. Alternatively, we could move the functions into some utility header in a more functional fashion. This would require a lot of work, since the methods currently modify a lot of member variables.
3. I did not really get why the root_nh and the robot_hw_nh are required in the RobotHW. But, loading the joint names from the parameter server via the namespaced NodeHandle and a hardcoded string feels hacky. In my code I have tried to improve the TODO of @davetcoleman a bit but it still does not feel right since it depends on name_ which has to be loaded in the constructor of the subclass. This is pretty much the same problem as loading the URDF in 1.

I'm looking forward to your feedback!

[RobertWilbrandt]

Hey, sorry for the late reply. Thanks for the PR, i should really get better at following up on issues i opened...

I don't really like the amount of code duplication here. When i created #34, my plan had a different architecture: We could create a new class (something like CombinedHwAdapter) which derives from RobotHW. This class would follow the layout dictated by combined_robot_hw and contain:

• A shared_ptr (or similar) to a SimHWInterface
• An empty constructor, which would set the pointer to nullptr
• An init(root_nh, robot_hw_nh) which would create a new SimHWInterface instance in the pointer and call the parameterless init() on it. It would then register the SimHWInterface as an interfaceManager (see registerInterfaceManager), effectively exposing all controller interfaces the SimHWInterface registered.
• read, write and doSwitch would just get handed over to the methods on the pointer

I once threw something like this together and it seemed to work (this is why i created the issue), but i didn't yet create a polished version of this. The advantages i'd see:

• No huge amount of code duplication, which in itself should be worth a lot
• No chance to have different behavior between SimHWInterface and the exposed adapter. You mentioned the option to inject different URDF models, but i cannot think of any use case where you would use a combined_robot_hw without using robot_description as your only urdf source.

I understand you currently have a custom gripper implemented as a subclass of GenericHWInterface. I think this use case could be handled by generalizing the approach above (e.g. making the adapter class a template of a specific adapted interface). Do you think this would work for you? I could provide an implementation of this this week (i promise this time!).

[Tuebel]

I agree with you on the code duplication issue. An adapter is an elegant solution which maximizes the code reuse.

I am however unsure if templating is the most idiomatic thing to do here. Alternatively, the adapter could have a set_hardware_interface(std::shared_ptr<GenericHWInterface> hw_interface) method.

• Pro template: The user must set the hardware type at compile time, less checks for nulltpr required
• Con template: I find unfamiliar templated code hard to understand, especially what class implements the required methods. C++20 concepts & constraints seem to tackle this issue. A clear documentation that any GenericHWInterface subclass works s could help.

In theory, I think that this can work for me. I am looking forward to your implementation!

[RobertWilbrandt]

Hey, i created a POC on the combined_robot_hw_draft branch. Could you check if this works with your GenericHWInterface and report back?

9fb6a76 exports SimHWInterface, you should be able to do that with your interface as well. 86e808d demos usage of this by creating an alternative rrbot_simulation.launch which uses two hardware interfaces in a combined_robot_hw, each controlling one joint. You can try this out as well using

roslaunch ros_control_boilerplate rrbot_combined_simulation.launch

As for the design: I decided to stick to a template-based solution. The main reason for this is that this allows you to use the exported interface without any interface-specific code in your combined_robot_hw-using main. This turns using an interface into "just configuration". I agree that i should document that more clearly, if this solution works for you i'll improve the documentation and do all the points listed in #34.

Things i don't like right now:

• I am not super keen on the name CombinableGenericHWAdapter
• Due to the strange interplay between polymorphism and overloading in c++ i had to define the read(time, period) and write(time, period) adapter functions in SimHWInterface again, even though i already added them to GenericHWInterface in Generalize GenericHWControlLoop to all types of RobotHW #38. An alternative solution would be to just call the read(elapsed_time) variant from the adapter (i think i prefer this right now), even though this leads to weird behaviors if anyone should ever derive from GenericHWInterface and override read(time, period) (not really sure why one would ever do that, but that's why i used this version in the draft).
• The adapter handles the whole RobotHW api, which is verbose. Not really a problem, but i don't like it.
• (I noticed GenericHWInterface still uses canSwitch, which doesn't really work since Prepare switch jade ros-controls/ros_control#218... Might have to fix this at some point)

Happy to hear your feedback, if this works for you i'd beautify this some more and create a PR for noetic (which should just get cherry-picked to melodic, this should not require any code change) and merge it after review.

[Tuebel]

Seeing your code, I got what you mean with interface-specific code. The export is elegant and definitely works for me! I will still have to test it with my hardware but it compiles and loads the controllers when running it without the hardware.

A few thoughts on the things you don't like:

• The name is quite verbose, maybe just CombinableGenericHW without the pattern name?
• I might have found a fix for the inheritance issue via SFINAE exclude. My first draft is on my fork. I have not figured out yet how to apply this pattern to a member function, that's why i put the functions into the combinable_hw_dispatch namespace.

Due to COVID it could take a few days until I'm in my office again where the robot hardware is.

[Tuebel]

I have tested it with the hardware today and at the moment I have the weird issue, that the start_controller list is empty when using the gripper in combined mode. It works fine when using it as the good old GenericHWInterface. I will try to figure out whether it is an issue in my configuration or a bug.

[Tuebel]

One thing I noticed in the CombinableGenericHWAdapter<ConcreteGenericHW>::init is that the robot_hw_nh is used instead of the root_nh which is used in the examples. This lead to some other problems but is not the root of the empty start_list

[RobertWilbrandt]

Hey, i will work on this more this saturday but to your points:

1. Name: True, CombinableGenericHW seems more reasonable
2. read() function: I got the suspicion i forgot the obvious solution: Shouldn't changing the type of adaptet_hw_interface_ to std::shared_ptr<GenericHWInterface> fix this without any need for template magic? I'll get to try this soon...
3. Empty start_controller list: Phew this sounds strange, do you get any error messages? Could this be related to ros_control#452? I was somehow expecting that to be a problem, but as it worked fine for my simulation i hoped it would also "just work" for you. Could you try using the ros_control master with the fix for this?
4. Namespace: Shouldn't this just be a matter of namespacing your parameters? I'd say any implementation using root_nh is inherently flawed, as it wouldn't even allow for using multiple hw interfaces of the same type.

[Tuebel]

Thanks for your help and suggestions, here is what I found:

1. I have changed the name in my fork.
2. Jup, this is as simple as it gets and works great.
3. Using the melodic-devel branch of ros_control gives me a non-empty controller list, indeed. But later when the controller of the robot arm is loaded, an empty list gets passed to my gripper which is based on the GenericHWInterface. At the moment, I just ignore it but is it the intended behaviour?
4. Yes, my quick hack was using absolute paths instead of relative ones. My problem was, that the rrbot and sim_hw examples both use the node handle in the root namespace but this is not suitable for multiple hardware interfaces. There is stil a TODO from Dave Coleman in GenericHWInterface where the hardware_interface/joints parameter is loaded. So I guess, that this is a separate issue.

You can find my changes here.

[RobertWilbrandt]

I created a PR with the other implementation approach in #49. I think it's better to discuss that approach over there, so i'll close this PR for now.