Add notifier function to allow hwinterface switching

[fmessmer]

This PR is motivated by the feature proposed in gazebo_ros_pkgs#256
For more details see gazebo_ros_pkgs#256
#184 adds a notification function that is called in ControllerManager::switchController. This function can be used to switch the HardwareInterface for the resources of the controller that is meant to be started, i.e. "Switch HardwareInterfaces on Switching Controllers.

Default behaviour is simply return true - no effect.

[fmessmer]

Ping? Any comments on this PR?

[fmessmer]

In this comment I explain a bit more about how the new function can be used together with real hardware as well.
Also, although gazebo_ros_pkgs#256 depends on this PR, this PR here actually has not effect on the behaviour of the ControllerManager but allows us to re-use the ControllerManager instead of re-writing it, because derivation is not possible as the functions we would need to change are not marked virtual

[davetcoleman]

Should you also have

stop_request_.clear();
start_request_.clear();

similar to the locations above?

[fmessmer]

We decided to host the new gazebo_ros_control_plugin proposed in #256 in our own repository ipa320/cob_gazebo_plugins
Currently it's under further development in my own fork ipa-fxm/cob_gazebo_plugins

Still, the changes proposed in this PullRequest are a prerequisite for the plugin.
Is there any chance to merge this PR?

[davetcoleman]

I suggest @adolfo-rt merge this!

[fmessmer]

squashing...full of hope...

[mathias-luedtke]

Ping. Still waiting for this pull request to be merged..
This is the last piece we need to unify our setup for simlation and hardware (incl. mode switching).

[floweisshardt]

@adolfo-rt: do you need any further input on this before you can/will merge it? It's currently blocking us from releasing our packages.

[davetcoleman]

Looks good to me.

I'll merge this today if you could first document in a paragraph how this feature works, on the wiki: http://wiki.ros.org/ros_control

[fmessmer]

I will document this here, as I don't know where to put it:

Within ControllerManager::switchController() several checks are done before actually switching the controllers (e.g. check for resource conflicts,...).
The function ControllerManager::notifyHardwareInterface() proposed in this PR is meant to be used to allow switching between controllers that use different HardwareInterfaces (e.g. JointPositionInterface and JointVelocityInterface).
In order to switch between such controllers, the HardwareInterface needs to be switched as well. Thus notifyHardwareInterface() can be re-implemented in a class that derives from ControllerManager in order to perform all necessary steps in order to prepare the HardwareInterface for the requested controller switch.
As these steps are depending on which HardwarInterface is used, it's up to you to fill this function accordingly.

An example, where this function is used with Gazebo and the RobotHWSim can be found here. For RobotHWSim mode switching is done by simply assigning the proper control mode to joint_control_methods_ (see here).

If the HardwareInterface cannot be switched, notifyHardwareInterface() would return false here, and the controllers will not be switched as well. and switchControllers() returns false as well.
The default behaviour of the function is simply to return true, i.e. do nothing in the HardwareInterface and continue switching the controllers as before.

More explanation can be found here

Can we please merge this?

[adolfo-rt]

Long time no bump.

We've been spending some cycles on this, and we've reviewed this strategy and that proposed in #180 and pal-robotics-forks/ros_controllers#41.

Long story short, we would like joint mode switching to be triggered by controller_manager/switch_controller service calls as we use them today, and not resort to additional controllers whose only purpose is that of changing the joint mode. A more detailed account on this respect can be found here.

I think that the general idea of this proposal is a very good one. The biggest change I would suggest would be to not make notifyHardwareInterface() a member of the ControllerManager class, but a member of RobotHW, as it's the specific robot implementation that knows whether it can switch modes to the requested configuration or not. I can think of these alternatives:

• Simply add the mode switching checks and requests in a custom implementation of checkForConflict. This might not be very nice as mode switching is not necessarily conflict checking.
• Adding a new virtual method to RobotHW with the same signature as checkForConflict. I would try to look for something more descriptive than notifyHardwareInterface(), but I don't have a good name in mind yet.
• Add new protected virtual members to RobotHW that would be called from checkForConflict, that would determine if a joint can operate in a specific mode. This is partly based on your original PR, something like:

class RobotHW : public InterfaceManager
{
public:
  virtual bool checkForConflict(const std::list<ControllerInfo>& info) const;
protected:
  virtual bool canSwitch(const std::string& resource_name,
                         const std::string& hw_iface) const {return true;}

  virtual bool doSwitch(const std::string& resource_name,
                        const std::string& hw_iface) {return true;}
};

Please let me know what you think. I'l likely take these ideas for a test-drive, and see how they fare.

[mathias-luedtke]

I came to the same conclusion to add it in RobotHW since this class must be inherited anyway.
In my first implementation I hijacked checkForConflict to switch the modes. Later we added the non-const notify function to the controller_manager, just to put it in somewhere, related to the controller switching. We changed the name of the function several times in between ;)
In addition I like the idea of switching the joints one by one, in particular for debugging/logging.

Some thoughts on the proposed interface:

• providing hw_iface is not enough, the controller name should be supplied as well (or hardware_interface::ControllerInfo to have it all). For example within the CANopen motor profiles the same HW interface can be offered by multiple drive modes, the interface name alone is not expressive enough. Actually we do not use the interface name, but the controller name for drive mode look-up in our current implementation.
• Why did you choose to separate can_switch & do_switch? Unsupported interfaces should not be registered anyway. If you want to send queries to the device beforehand, then can_switch should better not be const.

Another issue that we had to face lately: What 's happening if the mode could not be switched for one device, but for the others? Idea: Stop the old and the new controller. This results in mixed modes, but at least nothing will move.

Last but not least there should be a way to notify the hardware that all (claiming) controllers have been stopped and therefore none of the commands should be processed.

[adolfo-rt]

Some thoughts on the proposed interface:

• providing hw_iface is not enough, the controller name should be supplied as well (or hardware_interface::ControllerInfo to have it all). For example within the CANopen motor profiles the same HW interface can be offered by multiple drive modes, the interface name alone is not expressive enough. Actually we do not use the interface name, but the controller name for drive mode look-up in our current implementation.

I don't understand why you need to know the controller name for a (potential) mode switch. IMO what's relevant is to know that resource foo_joint needs to be controlled using the VelocityJointInterface, for instance. RobotHW should then check if the joint is already in that mode, and if not make the switch (if possible).

At any rate, if you have a usecase where knowing the controller name is relevant, let's further discuss it.

• Why did you choose to separate can_switch & do_switch?

So we can check whether the transition is possible for all resources before attempting something that could end up in an inconsistent state. For this to be actually true, we should make doSwitch() return void and require that it can't fail.

If we only call doSwitch() (and don't use canSwitch()) and fail at some point, we'd then need to cache the previous state and roll it back.

Unsupported interfaces should not be registered anyway.

Some actuators might not allow switching between any two control modes, or might not be able to do so in a single control cycle.

If you want to send queries to the device beforehand, then can_switch should better not be const.

No problem to review the constness of the method.

Another issue that we had to face lately: What 's happening if the mode could not be switched for one device, but for the others? Idea: Stop the old and the new controller. This results in mixed modes, but at least nothing will move.

I believe I answered this above.

Something that should also be done and was not mentioned in my previous post is take the strictness parameter into account (STRICT vs BEST_EFFORT). We can determine which controllers are OK for switching like so:

• In case of BEST_EFFORT, if canSwitch() returns false for any resource of a given controller, that controller won't be switched. Others might.
• In case of STRICT, if canSwitch() returns false for any resource of any controller, then no switching is done.

[adolfo-rt]

Thinking more about this, the RobotHW API should actually make canSwitch() aware of the complete controller configuration that will result. There are inconsistent situations that cannot be detected by running canSwitch() on individual resources.

The example I have in mind is that of mechanical transmissions that couple multiple actuators and joints. It might be impossible to change the mode of only one of the coupled joints without changing the others. Consider a differential, where each joint is commanded by the combined contribution of two actuators. It's very likely that your hardware will only support changing the mode of the two joints at the same time, and not separately. To catch these possible inconsistencies, we need to know the complete controller configuration that will result.

So, the RobotHW API could look like:

class RobotHW : public InterfaceManager
{
public:
  virtual bool checkForConflict(const std::list<ControllerInfo>& info) const;
  virtual bool canSwitch(const std::list<ControllerInfo>& info) const {return true;}
  virtual void doSwitch(const std::list<ControllerInfo>& info) {return true;}
};

On constness, I propose that canSwitch remains const, to make explicit the logical constness of the method. If the implementation can't preserve physical constness because it needs to modify internal fields without changing its logical state, then those fields should be made mutable. I expect that hardware should typically not be attacked during calls to canSwitch() and doSwitch(), but rather that these methods register actions to be taken during RobotHW::write().

[mathias-luedtke]

Those are good points!

In addition to the coupled joints, there are drives that do not map interfaces to modes one-to-one.
We use our switching implementation to control CANopen devices with 5 different drive modes.
For example positions can be transmitted in interpolated (=streaming) oder profiled mode (=ptp).
The mode support varies from device to device and the user needs to select the appropriate one for the use case.

Our driver has an internal lookup from supported drive modes to hardware_interfaces that is used at start-up to register all supported interfaces.
Every time we switch a controller we read the drive mode from the parameter server (example config).

"(const std::list& info, const std::string& hw_iface)" is redundant, since the hardware_interface ist part of the controller. Why are the new functions protected? (In other words: Which component calls these functions?)

[adolfo-rt]

"(const std::list& info, const std::string& hw_iface)" is redundant, since the hardware_interface ist part of the controller.

You are right. It should suffice to pass only the list of ControllerInfo. I edited the previous post to reflect this.

Why are the new functions protected? (In other words: Which component calls these functions?)

Originally I thought we could call the switching methods from within RobotHW::checkForConflict, so no need to make them public then. To make this happen, we sould then also pass the strictness policy as a parameter.

We might as well cal the switching methods from the ControllerManager, case in which they should be public, and don't require passing the strictness policy around. I chose to use this alternative in the edited previous post as well.

Thanks for correcting my sloppy API proposal :)

[fmessmer]

This PR will be closed in favor of #200 soon.
I'll just keep it open until everything is "cut and dried"...

[fmessmer]

As #200 is merged, closing this one...