Splitting a joint across two RobotHW plugins

[mikepurvis]

We've been exploring a path of fine-grained but highly generic RobotHW plugins using CombinedRobotHW, where typically each plugin is managing just a single joint. One thing that's been a frustration, though, is that we've had cases where the sensing for a joint is logically separate from the control of it. For example, we might have velocity control over a joint, where the low level control loop uses an encoder, but the actual sensing we want for our control purposes comes from a multi-turn potentiometer, since that gives us absolute position. But the low lever motor controller knows nothing about the potentiometer; they're completely isolated.

In this scenario, it's hard to write a generic RobotHW that exposes this particular combination of sensing+control— the logical thing is rather to break it up and have the sensing in one plugin and the control in the other. This is almost possible already with ros_control today, but the issue comes in that JointCommandHandle class expects to initialize itself from a JointStateHandle instance. If the command plugin initializes separate from the sensing plugin, it has no way of getting a reference to the JointStateHandle in order to pass it to its own JointCommandHandle's constructor.

I'm doubtful that it's going to be possible to resolve this without some kind of ABI break, but if we're willing to consider a longer term solution, there are some possibilities:

• Add a third, optional parameter to RobotHW::init, where it may be a passed a pointer to an InterfaceManager. This would just be a this reference to the parent when CombinedRobotHW is instantiating its plugins, and otherwise NULL, but it would allow later-initialized plugins to query for the interfaces registered by earlier-initialized plugins (main downside is RobotHW initialization order would matter whereas today it does not).
• Add some way to register a "deferred" JointHandle, which stores only the control pointer, and then after CombinedRobotHW has initialized all plugins, as part of its hardware interface combining step, it would resolve any deferred interfaces (unclear to me if this would be possible to do generically; it might have to be tied to joints, which would be a downside).
• Add more APIs to RobotHW, for example which would allow an InterfaceManager to be temporarily registered/queried. These APIs would be used by CombinedRobotHW but could otherwise be ignored.

Other options considered and rejected:

• Before calling RobotHW::init, CombinedRobotHW could simply add itself to each RobotHW by calling its InterfaceManager::registerInterfaceManager method. This would probably actually work, but seems weird and counter to the design intent of how InterfaceManager was meant to be used. Basically you'd end up with a recursive cascade from one plugin to the previous rather than a simple list in the CombinedRobotHW with all them added to it.
• Having the "extra functionality" for RobotHW be supplied by a mixin class, where there's some signal which tells CombinedRobotHW that it can cast the RobotHW to that other type and call the other APIs. This just seems fragile and confusing from a user's point of view. On the other hand, it might establish a precedent for how to extend RobotHW in the future in other ways without it becoming a kitchen sink.
• Introducing some kind of side channel for passing JointState references between cooperating RobotHW plugins, for example a singleton they all link to. Rejected because this would mean having to double-register them, and in general it seems pretty counter to the design intent of ros_control. Possibly the double-registering concern could be handled by hiding it in an intermediate class which inherits from RobotHW and overrides registerInterface, but at a philosophical level it still doesn't feel quite right.

@efernandez @p6chen @bmagyar for thoughts/comment.

[mikepurvis]

For interested parties, here's a very simple example of the third proposed scheme for implementing this:

https://github.com/ros-controls/ros_control/compare/melodic-devel...mikepurvis:parent-interface

Most of the code is changing resource claiming to use std::enable_if so that in the case where you are not claiming the resource (eg a JointStateHandle), the reference you get back is const. This means that it's possible to expose a const JointStateInterface, which can return only non-claimable const handles.

The usage which addresses the original motivation looks like so:

  try
  {
    auto state_handle = getParentInterfaceHandle<hardware_interface::JointStateInterface>(joint_name);
    hardware_interface::JointHandle command_handle(state_handle, &cmd_);
    joint_command_interface_.registerHandle(command_handle);
    registerInterface(&joint_command_interface_);
    ROS_INFO_STREAM("Registered JointHandle for " << joint_name << ".");
  }
  catch (const hardware_interface::HardwareInterfaceException& ex)
  {
    ROS_ERROR_STREAM("Unable to register JointHandle for " << joint_name << ": " << ex.what());
    return false;
  }

As far as the other options, if you add a default value parameter to a virtual method, overrides still have to change to match the new signature, so that's a non API compatible path; the deferred JointHandle option is intriguing, but I believe would ultimately require some more invasive changes to CombinedRobotHW, which I wanted to stay away from.