[proposal] Support multiple hardware interfaces in controllers

[jbohren]

Introduction

As of this writing, the biggest limitation that I see in ros_control is its inability to connect a single controller to two different hardware interfaces / robot hardware abstractions. As far as I can tell, removing this limitation would fundamentally change the design of ros_control.

1. Closing a control loop between two RobotHW components at the level of ros_control (consider connecting a PHANToM Omni to a KUKA LWR via ros_control),
2. Modular RobotHW components which use the same low-level interface (consider a robot that can have different numbers of arms or arm components like grippers which all talk over CANBus like the WAM),
3. Performing coordinated realtime control between two robots with the same type of joint interface (a WAM and a KUKA LWR),
4. Performing coordinated realtime control between two robots with different types of joint interfaces (a WAM and a SCARA arm).

The pipeline currently connects like so:

• ControllerManager calls ControllerBase::initRequest()
• ControllerBase::initRequest() calls ControllerInterfaceType<HardwareInterfaceType>::init()
  • At this point we are already constrained to use a single type of hardware interface
• HardwareInterfaceType::init() is passed the single HardwareInterfaceType

All of this connection via template inference means:

• ControllerManager cannot connect a single controller to multiple hardware interfaces of different types from a single robot hardware abstraction and the only way to connect two different interfaces is to connect them with the RobotHW which means control computation would be happening in the hardware abstraction layer instead of the controllers.
• ControllerManager cannot connect a single controller to multiple robot hardware abstractions which means you need to create a RobotHW subclass for every robot configuration (including master/slave devices which might be fundamentally different hardware).

Multiple RobotHW in a Single ControllerManager

To support multiple robot hardware abstractions in a controller manager, we could take a hierarchical approach, and compose RobotHW subclasses in a single CompositeRobotHW class which combines the resources of each component's hardware interfaces. This would require some template magic and/or a redesign of the resource manager to enable the extraction and aggregating of handles of the same type.

As a slight alternative, we could modify the ControllerManager to keep its own aggregated hardware interfaces internally, and instead register RobotHW instances with it, and have the controller manager extract and aggregate the handles, itself.

Multiple Hardware Interfaces in a Single Controller

Controllers are currently strongly typed at compile time based on a single hardware interface. via template parameter. In order to allow a single controller to talk to two different hardware interfaces, we could change the lookup pattern from using templates to just using string identifiers. It's worth noting that the interface types are already being extracted from the RobotHW class in this way by calling internal::demangledTypeName<T>() and looking up the interface object in a std::map<std::string, HardwareInterface*>.

Feedback

Please comment on what you think about the described limitation and my sketches of workarounds, and feel free to suggest other solutions!

@wmeeusse @adolfo-rt @mikeferguson @davetcoleman

[adolfo-rt]

This is a big limitation, yes. You have summarized very well the usecases that are currently not supported. I haven't thought enough about the problem to fully understand the repercussions of your proposals, but I'll describe how I managed to hack a controller with multiple hardware interfaces.

The usecase is that of a biped walking controller that has one PositionJointInterface, but also requires two additional interfaces for force-torque and IMU sensors. The controller derives directly from ControllerBase and not from Controller<T>, so it has access to a RobotHW pointer, rather than to just a single hardware interface. The hack comes when implementing

virtual std::string getHardwareInterfaceType() const = 0;

which I set to PositionJointInterface :-/. This solution is not optimal, but got my foot out of the mud.

Preliminary thoughts

• Change the string getHardwareInterfaceType() const in the ControllerBase API to be plural vector<string> getHardwareInterfaces(). I haven't checked how this would impact the controller manager logic.
• Managing multiple RobotHW instances sounds easier than combining many into one, resource management could then be scoped to each RobotHW instance. If we go this way, controllers should also have access to potentially many RobotHW instances. What would be the pros of a single aggregator?.

[wmeeusse]

@jbohren Appreciate the motivation and usecases in your proposal! I'm on board with the idea that this should get fixed. I need to think through the consequences of each of the proposals, but at first glance the idea to pass multiple RobotHW instances to a controllers sounds cleanest.

[kphawkins]

I'd like to go ahead and resurface this issue since I'm now running into a similar problem.

My usecase: I have a UR arm and have recently written a RobotHW interface for moving the joints. I have also attached a Robotiq gripper as an end effector and will soon be mounting the arm to a linear actuator. I would like to write and release independent RobotHWs for both the gripper and the actuator such that I can write controllers which act independently, but also others which utilize multiple interfaces jointly.

Since there hasn't been much development here, I was wondering how best to proceed. I'm not sure I want to rewrite the several packages to maintain a list of RobotHWs, though this might be the only way to accomplish this. Anyone working on a solution here or found a good workaround?

[kphawkins]

Looking at it a little more, it seems like there is very difficult to "add" RobotHW/InterfaceManager instances into a new composite, without again respecifying every single interface added. Since the only way to obtain each interface is to call the templated method get(), you can't really "add" ResourceManager instances across different RobotHW. I thought you could achieve this by making a new ResourceManager when the "MultiRobotHW->get" gets called, but since it looks only for a pointer, you would create a memory leak.

It might make more sense to create a new MultiControllerManager class (or rewrite the existing) to take a list of RobotHW. These separate lines:

initialized = c->initRequest(robot_hw_, root_nh_, c_nh, claimed_resources);
bool in_conflict = robot_hw_->checkForConflict(info_list);

Are the primary places where robot_hw_ is used. If controller_manager.cpp were re-written to attempt initializing controllers through a list, and looking through the entire list for conflicts, then it could be updated relatively cleanly with a list. This would solve the multi-RobotHW problem, at least. The cross-interface/cross-RobotHW controller wouldn't work though.

Thoughts?

[wmeeusse]

I'd have to dig a bit deeper into the design again to make sure, but I think we can support controllers that use 1, 2, 3, or more interfaces from the same robot, by adding new templated controller classes that derive from the controller base class. Currently the only class that derives from the controller base is Controller, but we can add Controller<T1, T2> and ControllerT1, T2, T3> to support 2 or 3 interfaces. These new controller classes implement the same initRequest method as Controller, but they will have an init method with the multiple interfaces that were pulled out of the robot. We'd definitely have to make other smaller changes to make this work, but I think there might be a path forward here.

[kphawkins]

Suppose we created two new classes: RobotHWGroup and InterfaceGroup. The RobotHWGroup will contain and maintain a list of RobotHW and implement checkForConflict such that it checks for conflicts across the full list of RobotHW. The new group class would be the parameter passed to the ControllerManager method ControllerBase::initRequest. Thus, every "main" loop for the robot controller managers will need slight modification, to create a dummy group and add the only current RobotHW.

InterfaceGroup will be templated on a particular interface (with subclass HardwareResourceManager), and contain/maintain a list of pointers to interfaces. The interface group will also implement getHandle and getNames, which manages these across the multiple interfaces. The Controller::init method's first parameter would be changed from T* to InterfaceGroup* though every controller's init definition would need to be modified, the functionality would be identical to the original. By using a list of interfaces instead of combining them, each interface for each RobotHW is left to manage its own resources, even if the interfaces are the same.

Finally, the ControllerT1/T2/T3 problem can be resolved just as Wim mentioned. Now that the cross-RobotHW problem is handled by InterfaceGroups/RobotHWGroup, each controller has access to all RobotHW instances.

Let me know if this seems like a palatable solution. If I have time, I might try coding something up and see what I come up with.

[wmeeusse]

The idea of a RobotHWGroup seems pretty cool. If I understand it correctly, it would simply be an object that derives from RobotHW, and internally manages the RobotHW of multiple robots. So this would not require any changes to the ros_control framework, right? The only change would be in the main, where a user creates its own RobotHW.

As for the InterfaceGroup, I think we can replace that with The Controller<T1, T2> I talked about earlier. The init call of a Controller looks like init(T* iface, ...). The init call of a Controller<T1, T2> would look like init(T1* iface1, T2* iface2, ...), etc. That does mean we need to provide the Controller<T1, ... Tn> base class for every number of interfaces, but I don't think we need anything crazy like a hundred interfaces, and that would mean that we don't have to change any existing controller APIs, all we do is add extra APIs for new controllers that will be using multiple interfaces.

[kphawkins]

The problem which you need InterfaceGroups to overcome is multiple RobotHW instances with identical interfaces (like 2 identical arms). Since interfaces_ maintains a 1-1 mapping of interface types to instances, the T* get() method would be ambiguous for a RobotHWGroup.

One potential workaround is to make the get() call unambiguous by combining multiple interfaces. This essentially would require making new interfaces for every interface in the group. That would be fairly annoying, since you'd need to re-register all of the interfaces so that they could be re-instantiated (because InterfaceManager is totally ambivalent to the interface types).

Honestly, HardwareResourceManager seems like a perfect candidate for a singleton design pattern. Hands down, there should just only be one hardware interface instance for each type. If the changes were made to make this happen, then get() will always return the interface shared by all RobotHW. Biggest drawback is that every implementation of RobotHW will need slight redesign.

[wmeeusse]

@kphawkins I see your point and how this makes it harder to implement the RobotHWGroup object to support multiple robots that have the same type of interface (by combining multiple interfaces of the same type into one interface inside RobotHWGroup). On the other side, breaking existing interfaces is a pretty big deal, so if we can get away with spending more time implementing the RobotHWGroup and not changing any existing interface, I'd definitely favor that solution. The implementation of RobotHWGroup is a one time effort that we can use over and over again.

I do want to keep around all ideas we come up with to improve the overall design of ros_control, and when the time comes to create ros_control 2.0 and improve existing APIs, we can learn from our current experiences. I started a new wiki page where we can collect these ideas https://github.com/ros-controls/ros_control/wiki/Ros-control-2.0.

[kphawkins]

Can we make get and registerInterface virtual so that they could be properly overridden? ControllerBase::initRequest takes a RobotHW parameter.

[wmeeusse]

That sounds like a good plan, and should not affect our current APIs.

[adolfo-rt]

Catching up here. I haven't thought much about the full implications of the proposed solutions, but some comments do come to mind:

Controllers with multiple hardware interfaces

Allowing controllers with multiple hardware interfaces would require changing the ControllerBase class at least in the getHardwareInterfaceType method to return more than one string. We would also need to modify some messages like controller_manager_msgs/ControllerState, which only reports a single interface.

Multiple RobotHWs, alternative solution

Not that I dislike having multiple RobotHWs, but I wonder the extent to which we can use the existing infrastructure without significant changes. Your usecase is very common, and I think it should be supported with minimum complexity. To me a robot made up of multiple off-the-shelf components is still a single robot, and it makes sense to have its resources in a single RobotHW instance. The problem seems to be more that of conveniently populating this single RobotHW, so how about resorting to convenience methods or classes?. You would pass a pointer to a RobotHW instance, and it would get populated with the appropriate hardware interfaces (e.g., an arm's actuators). You could also specify a prefix for allowing multiple instances of a subpart (e.g., right/left arm).

Example:

class FooArm
{
public:
  FooArm(const std::string& prefix = ""); // Probably also needs hw-specific params to connect to it
  bool addInterfaces(RobotHW* hw); // Register hardware interfaces
  bool read(); // Read hardware state
  bool write(); // Send commands to hardware
private:
  // Raw data here
};

Then, when setting up your hardware abstraction, you would create as many instances of FooArm as you want, giving each an appropriate prefix. The main loop would call read() and write() on all instances.

Notice that addInterfaces() might add either joint or actuator interfaces. In the latter case, you could leverage the transmission-parsing code that is is the works for automatically populating joint interfaces from the URDF transmission spec.

[wmeeusse]

@adolfo-rt, some comments:

Controllers with multiple hardware interfaces

You're right, there will be a number of smaller changes needed to support controllers with multiple hardware interfaces. I think we can make those changes while maintaining backwards compatibility for most users.

Multiple RobotHWs

I think we can do this without changing any existing code. I had a bit more of a generic helper class in mind (if I understood your example correctly), called RobotHWGroup. The RobotHWGroup derives from RobotHW, so it would offer the same get method as all RobotHWs. You can register multiple RobotHW with the RobotHWGroup; the group will provide access too all the registered RobotHWs through its own get method. So, the RobotHWGroup pretty much pretends that multiple robots are just one single robot. At runtime, the get method of the RobotHWGroup can call through to all the get methods of the registered RobotHW, and see if it can find the correct interface, and check if there are no duplicates. Alternatively, we can register RobotHWs with a prefix, and then the prefix would tell the RobotHWGroup wich RobotHW to call get on. In any case, the RobotHWGroup can just have a templated implementation of get, and does not need to be specific to any interface type.

[adolfo-rt]

Controllers with multiple hardware interfaces

You're right, there will be a number of smaller changes needed to support controllers with multiple hardware interfaces. I think we can make those changes while maintaining backwards compatibility for most users.

I don't know if we can maintain 100% API compatibility, as methods and messages previously containing a single string for the hardware interface identifier now might contain a vector of strings instead. The migration path is not painful, but still exists.

Multiple RobotHWs

I think we can do this without changing any existing code. I had a bit more of a generic helper class in mind (if I understood your example correctly), called RobotHWGroup. The RobotHWGroup derives from RobotHW, so it would offer the same get method as all RobotHWs. You can register multiple RobotHW with the RobotHWGroup; the group will provide access too all the registered RobotHWs through its own get method. So, the RobotHWGroup pretty much pretends that multiple robots are just one single robot.

You're right. This would be such a convenience class, with the added benefit that it exposes an existing ros_control interface.

At runtime, the get method of the RobotHWGroup can call through to all the get methods of the registered RobotHW, and see if it can find the correct interface, and check if there are no duplicates.

Are you referring to no duplicate interfaces or handles?. Duplicate interfaces should definitely be allowed: if two arms have a JointStateInterface, I'd expect RobotHWGroup.get<JointStateInterface>() to return an interface containing handles for both arms. Duplicate handles should be dealt with in a similar fashion as we already do (new handle with existing identifier overwrites existing handle, and logs a warning message).

Alternatively, we can register RobotHWs with a prefix, and then the prefix would tell the RobotHWGroup wich RobotHW to call get on. In any case, the RobotHWGroup can just have a templated implementation of get, and does not need to be specific to any interface type.

Implementation-wise, we could internally keep a separate set of interfaces containing the aggregate of all registered RobotHWs and standalone interfaces (recall that we can still register interfaces through the inherited RobotHW API). Therse are the ones that get<T>() would return. When new interfaces are added, either via registerInterface() or through a new registerRobotHW() method, these internal aggregating interfaces would be updated as well.

I'm really liking this idea now.

[kphawkins]

I have opened a pull request here: #137

It doesn't alter any existing interfaces, and seems to work very much like we've discussed. Of course, I still haven't figured out same-interface-different-RobotHW problem, but that might require more detailed changes.

I added one major test case which seems to show it working properly, see robot_hw_group_test.cpp.

[adolfo-rt]

I just went through #137. Great initiative. I think that not supporting multiple identical interfaces is a big issue, and I don't know how to address it at this moment.

Futile comment: If C++ allowed reflection (get a type from a string, the opposite of the typeid operator), we could get this done inside the InterfaceManager class in a very elegant way. This is impossible, but I still drafted a non-compiling prototype here. The impossible parts are the first two lines of this code block. Removing this block should yield working code that I think does the same as your PR.

[adolfo-rt]

Implementation details apart, what's your opinion on having a separate RobotHWGroup class vs. supporting the behavior directly by RobotHW?. The RobotHWGroup seems more like an easy way to batch-add contents to a RobotHW. The idea can also be extented to batch-adding handles to interfaces:

• RobotHW would allow to batch-add interfaces by merging the contents of another RobotHW with its own.
• Hardware Interfaces would allow to batch-add resource handles to an interface, by adding all the handles of another interface (trivially implemented here).

[kphawkins]

I have submitted another pull request to fix the interface aliasing issue here: #138 See robot_hw_group_test.cpp for an example with new supported usage.

Though this is an extensively involved and potentially confusing implementation, it passes all unit tests, including a new added test which merges identical interfaces from separate RobotHW, all without changing any method definitions.

First, understand that after all RobotHW are registered in the RobotHWGroup, a new interface almost certainly must be created for RobotHWGroup::get<T>() calls. One of the biggest problems I had to tackle was the fact that the InterfaceManager::get<T>() cannot be overridden, and that it didn't have any explicit knowledge of what class types it would be managing. The unit tests imply that there is no limit to what type of class can be registered. Thus, I had to rewrite almost all of the get function to support both old and derived method functionality.

Since it would make sense that only HardwareResourceManager classes have the ability to be combined, you must write get<T>() such that it only compiles code which concatenates a list of interfaces when T is a HWRManager. This is the reason for the helper struct check_is_hw_resource_manager. In the C++03 standard, clunky techniques like those found there are used to resolve class types for templates. Thus, you are able to call the method HardwareResourceManager::concatManagers, only when the template in the get is of that type.

The added helper methods to resource_manager.h and hardware_resource_manager.h are not all that intrusive, and probably necessary to pull this off. They just allow you to produce a combined HardwareResourceManager from a list of them.

I was trying to avoid was requiring the user to call several templated "re-register interface" methods after all RobotHW were added. This might be an alternative, which would require less change to interface_manager.h, but would require a strange usage pattern which isn't all that intuitive.

[adolfo-rt]

This is a great step forward. You definitely tackled some of the big issues. I was expecting the implementation to get more involved to support this usecase. Here's some feedback from reading (but not running) the code:

• The biggest issue I anticipated with this work package was proper lifecycle handling of newly created interfaces. As you correctly mention, new combined interfaces are potentially created when calling get<T>(). Right now these new instances can be created, but they are never deleted, which IIUC the implementation, represents a memory leak.
• In check_is_hw_resource_manager, instead for checking if a type is HardwareResourceManager, you could check for its parent, ResourceManager. You are only using the ResourceManager API in the end. The benefit of doing this would be that non-hardware interfaces like transmissions would benefit from your work as well. By the way, great use of SFINAE here. Hats off to you.
• Minor: Types around the check_is_hw_resource_manager addition use snake_case instead of the CamelCase convention of the rest of the code.
• Can't we just add the RobotHWGroup functionality into InterfaceManager?. Is there any blocker forbidding this?. This would allow to compose non-hardware sets of interfaces as well (again, transmissions being an existing example). In such a case, registerHardware(...) should have a more generic name like registerInterfaces(...) or similar.
• The new getHandles() and registerHandles(...) methods of ResourceManager are not strictly needed. You could get away with getNames() and a loop around getHandle(name) with no public API additions.
• Concerning the tests, I think this sequence should also be explicitly tested when combining similar interfaces:

// Both h1 and h2 contain the Foo interface
hw.registerHardware(h1);
Foo* foo = hw.get<Foo>;
// Check we contain h1's handles
hw.registerHardware(h2); 
Foo* foo2 = hw.get<Foo>; // NOTE: get is called after each call to registerHardware
// Check we contain h1 and h2's handles

[kphawkins]

I'm about to add a few commits based on these comments:

• The memory leak is really hard to tackle, since at InterfaceManager's destruction, it has no idea what types it's storing internally. Thus it can't call the destructors properly. Furthermore, I find it difficult to actually consider this a memory leak. The usage pattern would require someone continually constructing a new RobotHWGroup over and over again. If someone is actually doing this enough to expand the memory significantly, I feel like they have bigger issues. I have marked it in code with a FIXME tag. If we could use Boost or C++11, we might be able to open ourselves to significantly easier solutions.
• I think I fixed the CamelCase problem, all I could find is the class name breaking it. I'd probably like to leave this to someone more particular about formatting to make sure this complies properly.
• Converted everything to base class ResourceManager
• Removed getHandles and registerHandles
• At this point I don't see a reason not to change InterfaceManager. I think I added RobotHWGroup at first to minimize change to other files, but at this point InterfaceManager is mostly rewritten.

[kphawkins]

#138 Has been updated. I have totally removed RobotHWGroup, and added InterfaceManager::registerInterface. This allows me to also remove the findInterfaceData methods.

One potential use-case which was potentially problematic is the fact that get<t>() calls essentially finalize what that method will return, once more than one interface is registered under the same type. This ties into the whole memory leak issue, since we probably don't want to be calling new every time get is called.

I have extended my original implementation to also create new instances if it seems like new interfaces have been registered in the meantime. I know this feels memory leaky, but tolerating news a few per datatype seems totally reasonable to me.

[kphawkins]

#138 Now includes multi_controller.h, which has an implementation of Controller<T1,T2>. It is a trivial reimplementation of Controller<T>, though I simply hacked around the ControllerBase::getHardwareInterfaceType problem by just concatenating the type names to form a new string "<(T1 name),(T2 name)>". I honestly don't see this particular method being used for any logic at the moment, so perhaps this is an acceptable solution for this problem? It would technically be unique for any controller which uses those two specific interfaces.

I have also added the functionality to do simple tests in controller_manager_tests using what might cover all of the use cases @jbohren first enumerated. Note multi_dummy_app.cpp and vel_effort_test_controller.cpp which are run by rostest controller_manager_tests multi_cm_test.test.

[adolfo-rt]

Checking in again.

First, from my inline code comment, I think that the getHardwareInterfaceType() should get some attention. There are potential complications that no test is covering right now.

Secondly, I don't feel great about leaking a few interface instances, but I agree that under the current design, there's no way that the InterfaceManager destructor can know the types of the stored interface pointers. Only get<T>() knows about them. I haven't spent time thinking if there are significantly different designs which would allow to do what we want without causing big API disruptions (sigh).

Lastly, It's great that you've been adding so many tests to the PR. I can't tell how much this is appreciated.

I've been slow on this review, and would like to excercise the code myself, but my response times will likely continue to be slow for the time being (especially because this issue is non-trivial and requires focus and concentration).

[Igorec]

Hi. What do you think about pull #151

[Igorec]

And what do you think about pull #153

[skohlbr]

We´re running into this issue right now (looking into implementing an admittance controller that needs both force/torque sensor interface and a joint interface). The quickest way to resolve this that comes to my mind without requiring changes to ros_control is creating a new custom interface that provides both data, but that of course would not be required if there´s a workable composite solution by now. Seeing how last post here is from April, I just wanted to ping and ask what the current state of affairs is.

[adolfo-rt]

@skohlbr. Your usecase is similar to a previous thread comment #75 (comment), which means that you can work around the current state of affairs from within your controller implementation. If you inherit from ControllerBase, instead of from Controller<T>, you have full access to the whole RobotHW data structure.

Since you only have one interface that actually sends commands, I would make the controller expose that as its main HW interface in getHardwareInterfaceType(). What you basically have to do is reimplement initRequest(...) such that it fetches all the desired interfaces from RobotHW. The implementation existing in Controller<T> is a good starting point.

I hope this can get you going. We use this trick and it works fine.

[skohlbr]

@adolfo-rt Thanks, that indeed looks like a workable solution. Have to admit I read all comments some time ago and missed the details while skimming over things this time around. We´ll use the proposed scheme and report back in case of issues.

[adolfo-rt]

Allowing multiple hardware interfaces in a single controller have been addressed in #204.

Composing multiple RobotHW instances remains the main pending feature for ros_control 0.x. Maybe in the next sprint...

[toliver]

Composing multiple RobotHW instances has been addressed in #235. I think all the use cases mentioned in this issue are now possible in kinetic-devel.

[bmagyar]

Thanks for the note.
@jbohren what do you think?

[mikepurvis]

@bmagyar @jbohren This issue can be closed, I think.

[bmagyar]

👍

[davetcoleman]

+1