Add Composite Robot Hardware Interface

[Igorec]

Composite robot hardware interface can dynamically load, configure and run together a set of robot devices. CompositeRobotHW derived from RobotHW class. It work like ControllerManager for controllers but for devices. CompositeRobotHW loads device libraries and configures each device at configure method.

Configure example:

device_list:
  left_arm:
      type: my_arm_hardware/MyArmHW
      address: 192.168.1.200
      root_name: l_base_link
      tip_name: l_tip_link

  right_arm:
      type: my_arm_hardware/MyArmHW
      address: 192.168.1.201
      root_name: r_base_link
      tip_name: r_tip_link

  left_gripper:
      type: my_gripper_hardware/MyGripperHW
      address: /dev/ttyS0
      palm_name: l_palm_link

Each device class should be derived from DeviceHW and exported as pluginlib module. All devices share the same interfaces thru CompositeInterfaceManager and may share resources thru ResourceInterfaceManager. Each device must implement configure method, for example:

using namespace composite_hardware_interface;

bool MyGripperHW::configure(SharedInterfaceManager& ifaces,
                            SharedDataManager& shared_data,
                            SharedUrdfModel &urdf_model,
                            const std::string& name,
                            const ros::NodeHandle& nh)
{
  using namespace hardware_interface;
  std::string command_joint = getCommandJoint(urdf_model.get(), nh);

  JointStateHandle state_h(command_joint, &position_, &velocity_, &effort_);

  // If interface with type JointStateInterface does not exists yet, the new 
  // one will be created and registered in the manger. 
  ifaces.registerHandle<JointStateInterface>(state_h);

  JointHandle pos_h(state_h, &position_cmd_);
  ifaces.registerHandle<PositionJointInterface>(pos_h);

  JointHandle eff_h(state_h, &effort_cmd_);
  ifaces.registerHandle<EffortJointInterface>(eff_h);

  // For example joint_mode - some data that must be shared between devices
  if( !shared_data.exists("joint_mode") )
  {
     // "joint_mode" does not yet match the key of any element in the cash, 
     //  so the get function inserts a new element with that key and returns 
     //  a pointer to its mapped value.
     JointModeHandle jm_h("joint_mode", shared_data.get<int>("joint_mode"));
     ifaces.registerHandle<JointModeInterface>(jm_h);
  }
  //  "joint_mode" matches the key of an element in the cash,so the 
  //  function returns a pointer to its mapped value.
  joint_mode_ = shared_data.get<int>("joint_mode");
}

int* joint_mode_;

[davetcoleman]

Can you better define what you mean by a "device"?

[Igorec]

Device means to me is a separate component that is a part of the robot. For example: a gripper, a force-torque sensor, a manipulator arm. Generally for each of these "devices" can be written independent driver. As part of the robot they work together. We want to implement coordinated control of all devices of the robot. This is what jbohren wrote about in the first post #75.

For example in our lab we have а robotics testbed, which consists of several industrial manipulators, sensors, grippers, etc. Depending on the task, we'll use a different combination of devices. Under the current architecture, we have to implement RobotHW for each configuration. When using CompositeRobotHW we implement class DeviceHW for each of our "devices" once. And then just specify in the configuration file which devices use in each case.

[davetcoleman]

This should be

COMPOSITE_HARDWARE_INTERFACE__COMPOSITE_ROBOT_HW_H

[davetcoleman]

+1

Thanks for explaining. I don't have a use case for this right now so won't be testing this, but I reviewed the code and it looks very well done. Just to make sure - using this composite class is not required and will not change existing methods, only add new functionality, correct?

We'll want to make sure your documentation at the top makes it to the ROS wiki.

[Igorec]

Thanks for review. Yes, it is not changed core methods, only add new functionality.

[adolfo-rt]

@Igorec. Your PRs require some review time, and I've been checking them little by little. Expect some comments soon.

[adolfo-rt]

First of all, thanks for taking the time to code your idea. From issues #75, #138 and this one it's clear that a convenience mechanism for easily composing/aggregating interfaces should be a top priority for ros_control, followed close behind by controllers supporting multiple hardware interfaces. A first round of comments follow.

My first comment would be to ask what drove you to propose an alternative design to #138, which largely tackles the same problems?.

I think the first problem to solve here is to have a convenient mechanism to aggregate already populated RobotHW instances. I like where #138 is going, as it only consists of minimal API additions. There is only one open issue with a memory leak, which (correct me if I'm wrong) is also present in your proposal.

I don't like much that CompositeRobotHW assumes that a URDF model is available, and that the same model is shared across all the managed interfaces. On the one hand, ros_control tries hard not to impose the URDF description in its core, and on the other hand, you might be interested in aggregating interfaces from different robots (c.f. teleoperation example of #75).

Concerning the DeviceHW class, my understanding is that you want to propose a plugin mechanism for loading robots (or subparts). Firstly, I think that device is a misnomer, as a robot subpart can still be considered a robot, and modeled as a RobotHW instance. Introducing a new term here is confusing. Secondly, I think we should further discuss the idea, because hardware platforms can be so different that I don't know if we'd be limiting developers too much by enforcing a specific interface (or requiring a shared URDF). Let me know your thoughts on this.

I'm being very critical about interfaces, because it's something we'll have to maintain over time once we commit to one, so please bear with me. I'm glad that we have one more person interested in composing RobotHW instances.

[Igorec]

@ adolfo Thanks for the attention. I thought it would be convenient to have a plugin mechanism to download hardware for robots that have several different configurations. As in our case, or, for example, in the case of "care-o-bot".

This is just another look at the issue. Maybe I did not quite understand # 138 , but it seemed to me that the interface merge mechanism is not very transparent. This variant is simpler.

About the name «device» and «robot». This is a philosophical question.) Device more general concept . Forse-torque sensor device is not a robot, but robot is a device.

With regard to implementation, it is all clean, I use boost :: ptr_vector.
URDF really optional element .

I understand your concern. Indeed, «DeviceHW» is a less flexible interface. If these PR's raise many questions and duplicate already existing functionality, they can be closed.

[adolfo-rt]

There is very valid code in your PR, as well as in #138, and I think something very interesting will end up coming out. The work of both you and @kphawkins has given me some ideas, and I'd like to try them out. I'll bring them forth for your consideration if they pan out.

P.S. Nice to hear that the care-o-bot folks are fiddling with ros_control.

[bmagyar]

Thanks for everyone who contributed.
I have just merged this PR that should work well for your use-case too. I am going to close this PR as CombinedRobotHW was merged, but please come forward and open new PR-s or issues if you have additions to that codebase!