[gazebo_ros_control] support hwinterface switching

[fmessmer]

This PR requires ros_control#184.

Before giving details on the changes proposed in this PR, I will point out the motiviation for this PR and the use-case we had in mind.

Motivation
The current gazebo_ros_control_plugin uses the DefaultRobotHWSim class to implement a HardwareInterface that connects gazebo with ros_control. The DefaultRobotHWSim offers all hardware_interface::PositionJointInterface, hardware_interface::VelocityJointInterfaceand hardware_interface::EffortJointInterface in a single class. Thus allowing to use position_controllers, velocity_controllers and effort_controllers to be used with gazebo.
Unfortunately, with the current implmentation only one HardwareInterface can be used at a time!
This is due to how DefaultRobotHWSim::initSim() currently handles hardwareInterface-tag of the transmissions. Although, all hardwareInterface-tags from the transmissions are parsed, only the first is used lateron.

This PR now extends DefaultRobotHWSim::initSim() in a way that several HardwareInterfaces can be specified for a joint, e.g. like this:

    <transmission name="arm_1_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="arm_1_joint">
        <hardwareInterface>PositionJointInterface</hardwareInterface>
        <hardwareInterface>VelocityJointInterface</hardwareInterface>
      </joint>
      <actuator name="arm_1_motor">
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>

This PR introduces means to keep track of the currently selected HardwareInterface by providing according data structures.

This PR also provides a specific GazeboControllerManager which derives from the base controller_manager::ControllerManager class. This new class implements the notifyHardwareInterface()-function proposed in ros_control#184.
The notifyHardwareInterface()-function is called during switchController()-function, i.e. when a new controller is meant to be started, Before switching the actual controllers, the new notifyHardwareInterface()-function allows the GazeboControllerManager to also switch the HardwareInterface required by a resource of the controller to be started.

With this, it is also possible to have controllers requiring different HardwareInterface within the same gazebo session without needing to change the hardwareInterface-tag in the transmission (URDF).

For example:

#position controller
arm_1_joint_position_controller:
  type: position_controllers/JointPositionController
  joint: arm_1_joint

#velocity controller
arm_1_joint_velocity_controller:
  type: velocity_controllers/JointVelocityController
  joint: arm_1_joint

can be loaded to the parameter server and then those two controllers can be switched using the ControllerManagers switch_controller-Service "on-the-fly".

Note: With this, no "PID-Parameter-Tuning" would be required for simulation anymore, as gazebo supports all the (Standard-)HardwareInterfaces.

This PR does not change the DefaultRobotHWSim::readSim() and DefaultRobotHWSim::writeSim() functions as those are already capable of handling various HardwareInterfaces, i.e. joint_control_methods_.

---

I would like to thank @ipa-mdl for his help implementing this feature!

[fmessmer]

In case it is not desired to have this functionality in the default gazebo_ros_control-plugin and DefaultRobotHWSim, we could also have this in a custom Gazebo-Plugin. However, as only initSim() is affected by this new feature, it would be great if the "RobotHWSim" could derive from DefaultRobotHWSim.
This would require to split the implementation of DefaultRobotHWSim into a Header-File (default_robot_hw_sim.h) and a Code-File (default_robot_hw_sim.cpp).

I'd be happy to hear comments, suggestions and concerns regarding this new feature!

[fmessmer]

Ping? Any comments on this PR?
What's the opinion with respect to spliting up DefaultRobotHWSim into a Header-File and a Source-File?

[adolfo-rt]

I only have general thoughts on this at the moment. The executive summary is:

• The ros_control end of things should be doing most of the heavy lifting.
• The Gazebo backend should mostly deal with mapping joint mode identifiers to the right Gazebo API calls (send positions, or velocities, or efforts).

---

Switching the control mode of a joint is a feature that makes sense both for simulated and real hardware settings, i.e., its usefulness is agnostic of the underlying robot backend. As such, I'd like to see this feature added with existing simulation and hardware reference implementations. I'm concerned that this PR is focusing mostly on getting the simulation end of things working, and not taking the whole picture into account.

As you probably know, @bmagyar has been looking into this, as time permits. If I'm correct, he has already validated an implementation in simulation, and is presently performing a hardware validation. Having the mode-switching logic should live in a place where it can benefit real and simulated settings without code duplication indeed turned out to be a bit tricky, and is something we can discuss in greater detail when a PR to this effect comes forth. Unfortunately I can't point you to the branch where development is taking place, as it's in a private repo.

What I'd like to avoid here is the roll-out of a feature with non-trivial API implications without good reference implementations supporting the major usecases. My weariness is not without reason. Last year, I made the mistake of introducing transmission support following a two-step process: simulation in hydro first (thanks to @davetcoleman), and then real hardware in indigo. Not having validated both in parallel introduced some ripples which are still being felt today.

[fmessmer]

I'm concerned that this PR is focusing mostly on getting the simulation end of things working, and not taking the whole picture into account.

Well, this PR indeed is the simulation part of our concept, yes. But the idea is not limited to simulation.

Some more details probably will clariffy this:
We will use the function notifyHardwareInterface() introduced in ros_control#184 to notify/prepare our hardware/driver according to the "control_mode" required by controllers to be started, i.e. the hardware_interface of the controller.
We do this in our implementation/derivation of hardware_interface::RobotHW. The according class connects to the driver for our hardware. The driver offers several "control_modes" that can be set/mapped to postion or velocity hardware interface respectively.
As the function notifyHardwareInterface() is called from within the ControllerManager::switchController() function, we are able to catch a requested controller switch and switch the "control_mode" of our hardware by calling according functions of the driver - before the controllers actually become activ.

Another advantage of this concept is that there is no need for an additional joint\_mode\_controller that needs to keep track of the current "control_mode" of the hardware (stored in the string) and needs to be part of the switching procedure. (At least if this still works as it was proposed in the original "joint_mode_controller".)

I hope these additional comments clarify our concept.

Thus, I hope the need for ros_control#184 becomes clearer. Without having this notification function that we could derive in our controller manager, we would need to implement our own controller_manager as a whole, as most functions of the ControllerManager class are not virtual.
As the new function notifyHardwareInterface() has no effect on the actual behaviour of the ControllerManager but would really help us using most of ros\_control with our hardware, we would be very happy to see ros_control#184 being merged.

We then still could discuss what happens with this PR proposed here!
As to this PR, I'm still in favor for being able to derive from gazebo_ros_control::DefaultRobotHWSim class in order to implement a specification of the plugin. But for that a split into Header and Source file is needed.
I'm happy to do this if this helps getting these features in!

[fmessmer]

Ping?
Has anyone actually tested this feature to see the beauty of this concept (mainly made up by @ipa-mdl)?
(@davetcoleman? @adolfo-rt?)

Just wanted to ask whether my additional comments helped to elimate the concerns regarding this concept being to much focused on simulation...

I understand that @bmagyar is working on the joint_mode switching approach.
However, up till now, I only see that an additional controller has been added.
What needs to be done on the HardwareInterface side (both simulation and real hardware) is still not clear to me.

Are there any reasons against having two concepts available for those wanting to use them?
(As default behaviour is not touched at all!)

To further facilitate the decision, I moved the multi_hw_support into a new gazebo_ros_control plugin to not touch the behaviour of the default_hw_sim. (see #267)
However, in order to re-use code of DefaultRobotHWSim as much as possible, I split up the code into cpp and header file in #266

[davetcoleman]

Switching the control mode of a joint is a feature that makes sense both for simulated and real hardware settings, i.e., its usefulness is agnostic of the underlying robot backend. As such, I'd like to see this feature added with existing simulation and hardware reference implementations. I'm concerned that this PR is focusing mostly on getting the simulation end of things working, and not taking the whole picture into account.

+1

Are there any reasons against having two concepts available for those wanting to use them?

Seems like it would add unnecessary complexity and maintenance. We should stick to one.

Thanks for all these efforts - I never use Gazebo anymore so its hard for me to really weight in on these issues.

[fmessmer]

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

I will eventually close this PullRequest, too, as the required changes affecting gazebo_ros_pkgs have been merge in #266
I'll just leave it open a while longer for further discussion until ros_control#184 is merged as it contains additional pre-requisites for the feature proposed in this PR.

[fmessmer]

I wanted to give an update on this issue as there has been quite some progress on this issue in the ros_control repository:

In ros-controls/ros_control#200, an API for hardware_interface switching (or joint_mode switching) is proposed that - after a detailed discussion and several iterations - is now almost finalized and ready to be merged (minor beautification is still to be done).

In conjunction to the efforts of getting to this API version, we continuously tested the APIs with an extension of the default gazebo_ros_control plugin (i.e. hwi_switch_gazebo_ros_control plugin), that has originally been proposed in this PR!
Since the beginning of the discussion, the API and the plugin frequently changed, so that this PR here definitely will be closed after ros-controls/ros_control#200 is merged.

The current version of the hwi_switch_gazebo_ros_control plugin making use of the hardware_interface switching API now lives here (and will be merged into ipa320 soon.

However, as I still think of this gazebo_ros_control_plugin being useful for the greater gazebo_ros_pkgs community, I wanted to open up the discussion whether it is desirable to have this hwi_switch_gazebo_ros_control plugin back in gazebo_ros_pkgs again.

Either way, comments and suggestions are welcome!

@adolfo-rt @davetcoleman @ipa-mdl
FYI

[fmessmer]

This is the PR for a gazebo_ros_control plugin using the new strict hwi switch API introduced in https://github.com/ros-controls/ros_controll/pull/200

Thus, closing this one!