Yarp: Multiple Analog Sensors interfaces proposal: take two

[traversaro]

In the context of developing a reasonable YARP interface to the distributed inertial sensors (accelerometers, gyroscopes, and fully fledged IMUs as in STRAIN2) mounted on the iCub [1] we encounter several challenges, that we detail in the following. An original proposal to improve existing YARP interfaces to overcome those challenges was originally formulated in https://github.com/robotology/yarp/issues/747 , but it was not accepted in YARP due to some problems in the proposal. This is new proposal that intend to solve the same issues for which https://github.com/robotology/yarp/issues/747 was created, but solving the bad point emerged during the review of https://github.com/robotology/yarp/issues/747 .

In the following we will focus on the iCub use-case that is motivating this discussion, but clearly the proposed new interfaces will be totally robot-independent.

How distributed inertial sensors are currently published

Currently the distributed accelerometers are exposed using the IAnalogSensor interface. The
format with which they are published differs from CAN and ETH iCubs :

• On CAN iCub, accelerometers for each link are published on the same vector, using the format described in [1].
• On ETH iCubs, accelerometers for each body part are published on the same port, but in the measurement vector also metadata (the sensor location, type and timestamps) are published together with the data.

Problems with the current interfaces

• Tipically the IAnalogSensor or IGenericSensor are used to expose the readings of just one sensor (that then is published on a YARP port by the IAnalogWrapper). The high number of accelerometers present on the robot (~40) make the use of a YARP port for each sensor quite undesirable.
• If the IAnalogSensor/IGenericSensor publishes on the same vector (that is then published on a port by the AnalogWrapper) readings from multiple sensors, with no way to get at runtime information on what is published on the vector . This happens fo the distributed inertial sensors, but also for the classical iCub IMU [2] in which gyroscope, accelerometers, magnetometer and orientation measurements/estimations are published on the same vector, while the structure of the vector is only documented offline.
• The timestamp is published by a second interface IPreciselyTimed , so it is not possible to publish the different timestamp of the different sensor published on the same port. Furthermore data and timestamp are read (at C++ level) using two different methods, so there is the (small) probability that the user code read the measurement and then the timestamp relative to a different measurement, even if both methods implementation are correctly synchronized using a Mutex.

Desirable features for the new interfaces, server/wrapper and clients/remote

• Make it easy to get the reading of a specific sensor, even if a given device exposes several sensors
• Support reading a timestamp for each sensor.
• Support optional compatibility with the old YARP port representation with specific wrappers.
• Publish heterogeneous sensors on a single YARP port, to avoid the explosion of YARP ports exposed by the yarprobotinterface .

Proposed new interfaces

To cover all the existing use cases of YARP-using robots, I propose to add the following interfaces:

• IThreeAxisGyroscopes
• IThreeAxisLinearAccelererometers
• IOrientationSensors
• IThreeAxisMagnetometers
• ISixAxisForceTorqueSensors
• ITemperatureSensors
• IContactLoadCellArrays
• IEncoderArrays
• SkinPatches

See the documentation on the proposed interfaces to get a more clear idea about these interfaces:

• https://github.com/traversaro/yarp/blob/feature/multipleAnalog/src/libYARP_dev/include/yarp/dev/MultipleAnalogSensorsInterfaces.h

The main idea is to have one multi-sensor interface for each type of supported sensor, but explicitly support the case in which a single device exposes multiple of these interfaces, similarly to what is already happening with controlboard interfaces. In particular, I propose to have three devices to deal with this kind of interfaces:

• MultipleAnalogSensorsServer : publish data and metadata exposed by analog interface on the YARP network .
• MultipleAnalogSensorsClient : implement the multiple analog sensor interfaces by reading data from a set of ports opened by the MultipleAnalogSensorsServer .
• MultipleAnalogSensorsRemapper : merge sensors exposed by two or more different devices, select a subset of the sensors or change the ordering of the sensors in a device.

See the proposed headers for these devices for getting a more clear idea about their role:

• https://github.com/traversaro/yarp/blob/feature/multipleAnalog/src/libYARP_dev/src/devices/MultipleAnalogSensorsServer/MultipleAnalogSensorsServer.h
• https://github.com/traversaro/yarp/blob/feature/multipleAnalog/src/libYARP_dev/src/devices/MultipleAnalogSensorsClient/MultipleAnalogSensorsClient.h
• https://github.com/traversaro/yarp/blob/feature/multipleAnalog/src/libYARP_dev/src/devices/MultipleAnalogSensorsRemapper/MultipleAnalogSensorsRemapper.h

Concrete use case in iCub

• The embObjStrain device will inherit from IThreeAxisGyroscopes, IThreeAxisLinearAccelererometers, IThreeAxisMagnetometers, IOrientationSensors, IForceTorqueSensors and ITemperatureSensors . If it is connected to a STRAIN1 board, it will only expose 1 ForceTorqueSensor, while if it connects to a STRAIN2 board it will expose 1 ForceTorqueSensor, 1 ThreeAxisGyroscope, 1 ThreeAxisLinearAccelerometer, 1 ThreeAxisMagnetometer, 1 OrientationSensor and 2 TemperatureSensors .
• The embObjInertials device will inherit from from IThreeAxisGyroscopes, IThreeAxisLinearAccelererometers, IThreeAxisMagnetometers and IOrientationSensors, and will expose all the sensors for which it is configured.

Backward compatibility

• As this proposal uses completely new C++ interface, the compatibility between the old interfaces and the new interfaces can be obtained at the C++ level by just implementing both the old and the new interfaces in the same device.

• As the new server/client devices are much more generic, the on wire representation of the streaming data will not be compatible with the old serializations of the AnalogWrapper and the ServerInertial. To maintain compatibility with port published by this wrappers, we can implement some sensor-specific wrapper (that will be different from the generic MultipleAnalogSensorsServer) that will expose the information of a specific sensor (such as the six-axis FT sensor or the IMU) to a YARP port with the "old" format. This wrapper will not have a related client, but will just be used for backward compatibility. The same strategy can be used for maintaining the features of the existing ROS wrappers.

• In the current proposal, I did not include any support for the "calibrate" functions of the FT sensors. However, those methods can be easily added just to the FT interface.

I would be glad to receive some feedback on this proposal before proceeding with finalizing the design and implementing the actual device, or any suggestions on alternative solutions to solve the problem.

cc @barbalberto @randaz81 @aerydna @diegoferigo @nunoguedelha @prashanthr05

[traversaro]

cc @DanielePucci

[barbalberto]

I like the general idea, I'd add a few things:

• A way to get data from all sensors of the same type within each interface, like 'getThreeAxisGyroscopeMeasureS'
• A reference frame for each sensor
• Optionally add something (bottle?) to provide custom 'additional information' for each sensor.

[nunoguedelha]

I also like the proposal. I guess that the name returned by getThreeAxisGyroscopeName( ) would be the sensor name as defined in the URDF model. For instance, the MTB_10B2 accelerometer fixed on the upper left leg would named l_upper_leg_mtb_acc_10b2. This is the convention used in all our actual tools relying on inertial sensors measurements.

---

• A reference frame for each sensor

@barbalberto If you mean the parent link, as defined again in URDF model:

  <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
    <parent link="l_upper_leg"/>
    <origin rpy="-1.57079551857 -1.57068021034 -0.733039094713" xyz="0.04200163 0.031565 0.049132"/>
  </sensor>

This information is parsed by iDynTree and allows to match the sensor name to the parent link. We should avoid duplicating information. Or else we move the parser to the firmware, and we could even have the pose information <origin rpy="-1.57079551857 -1.57068021034 -0.733039094713" xyz="0.04200163 0.031565 0.049132"/> as the custom 'additional information' you mentioned.

[traversaro]

A way to get data from all sensors of the same type within each interface, like 'getThreeAxisGyroscopeMeasureS'

Cool, good idea, I will add it.

A reference frame for each sensor

That was basically the idea of the name (because we actually had in mind the iDynTree integration that @nunoguedelha mentioned). If we want to add also a separate frame name, I would like to have a concrete use case in mind (we can discuss about this f2f).

Optionally add something (bottle?) to provide custom 'additional information' for each sensor.

This make sense, so we can provide any additional metadata with the sensors.

[barbalberto]

If we want to add also a separate frame name, I would like to have a concrete use case in mind (we can discuss about this f2f).

Well, the sensor name and its reference frame are two different piece of information, so collapsing them is not good imho. When I see name of the sensor, sincerely I cannot imagine that I should put the reference frame there.

I see many good reason to have a 'getRefFrame'

• first the user may prefer a human-friendly name for the sensor, like "forearm skin", but the reference frame for the sensor may be "elbow something".

• I can move the sensor to a different place (imagine a IMU for motion capture or whatever) changing its reference frame, but not the name.
  Example: MY_SENS_01 was placed on ref frame Ref1, in the next experiment it is placed on Ref_2. I want to keep the name MY_SENS_01 because I have my database of sensors with all the settings associated to that name.

• URDF is distinguishing the two, why we shouldn't ?

• Another reason is for robots not based on URDF model and/or not using iDynTree (or not even robots like in the motion capture example).

Please don't get stuck on what iCub's people is doing, YARP is meant for any robot, where different teams may use different convention. This interface is describing a sensor, and must not be bound in any way with a particular library and should not force a way to be used.

As a paradox, the reference frame is for sure a required information (otherwise data are quite useless), while the sensor name may be optional.

[DanielePucci]

CC
@marcoaccame

[traversaro]

We discussed a bit about this today with @barbalberto . Wrapping up my point of view in the following.

Well, the sensor name and its reference frame are two different piece of information, so collapsing them is not good imho. When I see name of the sensor, sincerely I cannot imagine that I should put the reference frame there.

Unfortunately, the frame in which a sensor measurement is expressed is not always identified with a frame with a specified name. In particular, the URDF [1] and SDF [2] formats identify the frame in which the sensor measurement is expressed with an unnamed <pose> element that is expressed with respect to the parent element of the sensor (either a joint or a link).

Furthermore, the actual meaning of "expressed in the frame" is ambiguous for several sensors types, see for example this discussions regarding the "frame of expression" of the force torque sensors in Gazebo [3][4].

To add even more confusion to the situation, different part of ROS are relatively inconsistent with respect to this. In the IMU ROS sensor message [5], a frame_id field is contained through the inclusion of the header message. However, in the gazebo_ros_pkgs plugins, that field is not populated from the URDF or SDF model, but rather from a separate frameName configuration parameter [6, 7].

Given the complexity surrounding the concept of "expressed in a reference frame", I would strongly prefer to avoid including this kind of information in the interface, exactly to preserve its generality.

first the user may prefer a human-friendly name for the sensor, like "forearm skin", but the reference frame for the sensor may be "elbow something".

This may still be something done at the implementation level, the interface is not providing any constraint on this.

I can move the sensor to a different place (imagine a IMU for motion capture or whatever) changing its reference frame, but not the name.
Example: MY_SENS_01 was placed on ref frame Ref1, in the next experiment it is placed on Ref_2. I want to keep the name MY_SENS_01 because I have my database of sensors with all the settings associated to that name.

As the interface does not cover the details about the sensor reference frame, the mechanisms of how this is obtained are completely agnostic to the interface. In the case you are using a URDF file, then the sensor name will remain the same, but the <pose> tag will be update to reflect the new position of the sensor.

URDF is distinguishing the two, why we shouldn't ?

The URDF format is not using a string to refer to the frame of a sensor, but rather it uses a anonymous <pose> element expressed w.r.t. to the link to which the sensor is attached. [1]

Another reason is for robots not based on URDF model and/or not using iDynTree (or not even robots like in the motion capture example).

Exactly to avoid being tied to an implementation detail of a way or another to express the sensor frame information (such as using as string to identify the frame) I would prefer to avoid including this information in the interface.

Please don't get stuck on what iCub's people is doing, YARP is meant for any robot, where different teams may use different convention. This interface is describing a sensor, and must not be bound in any way with a particular library and should not force a way to be used.

As there is no consensus in the robotic software landscape about how to express this information (unnamed <pose> or string?) I think it is wise to avoid putting the reference frame in the interface.

Despite the discussion, I think we did not achieved a consensus on this. I think the opinion of either @randaz81 or @drdanz will be definitely helpful.

[1] : http://wiki.ros.org/urdf/XML/sensor
[2] : http://sdformat.org/spec?ver=1.6&elem=sensor
[3] : https://bitbucket.org/osrf/gazebo/issues/940/force-torque-sensor-forcetorquesensorcc#comment-9463459
[4] : https://bitbucket.org/osrf/sdformat/issues/130/position-part-of-force_torque-sensor-pose
[5] : http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html
[6] : https://github.com/ros-simulation/gazebo_ros_pkgs/blob/8c18d8e88c3eee3f04fe004823dca8aaa22b1b04/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp#L168
[7] : https://github.com/ros-simulation/gazebo_ros_pkgs/blob/8c18d8e88c3eee3f04fe004823dca8aaa22b1b04/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp#L107

[traversaro]

If non-iCub users of YARP interfaces such as @jgvictores and @PeterBowman want to provide some comments on this proposal, it would be great.

[jgvictores]

Regarding the general name scheme of functions in proposed API: while I usually like unified names (a simple getName rather than getThreeAxisGyroscopeName), I understand this particular use case and could live with the long names. Okay on that side. :+1:

Small alternative proposal if it's worth the discussion ((which would mean a change to singular of the proposed interface)): Many times I've thought of a IDeviceDriverContainer/IPolyDriverContainer to later be able to do things like IThreeAxisGyroscope * ipc = iDeviceDriverContainer.view(index); (note the singular) to be able to access the elements of a std::vector<PolyDriver> or similar instead of writing wrappers for each method (the original context of the idea is different, but the implementation could be useful for both use cases). However, the iteration mechanism of checking what device can be viewed as what interface could be quite cumbersome, so maybe just food for thought, in case anybody else likes the idea.

Regarding a getRefFrame method:

1. As a current non-iDynTree user, a getRefFrame method information is not duplicate for me.
2. Train of thought: getRefFrame -> like the idea -> see comments on ambiguity -> think that we can inherit and document specifics in inhereted class -> still give more thought to no standard pose representation -> search in YARP -> find IFrameTransform.
   
   
   
   • Conclusion: In the line of this comment, perhaps we'd end up duplicating the job of the FrameTransformServer/FrameTransformClient pair. This is, following this reasoning, I would currently decide to _not_ include getRefFrame, and instead work on the FrameTransformServer/FrameTransformClient (plugins for simulators, YARP devices for physical devices) for this task.
   
   
   • There are some drawbacks ((Hence, I would also understand whoever supports a getRefFrame method too xD)):
     
     
     
     
     
     1. Having to rely on unique names as only way to match sensor measurements and refFrame
     
     
     
     2. Two YARP devices for a single physical device.
     
     
     
   
   

[traversaro]

Thanks for the comments @jgvictores !

[traversaro]

Given that the discussion is on the details and not on the overall architecture, I started implementing the proposed devices (it would be easy to change them for small modifications).
I just implemented the remapper, and I am quite happy with it:
https://github.com/traversaro/yarp/tree/feature/multipleAnalog/src/libYARP_dev/src/devices/MultipleAnalogSensorsRemapper
In particular I was able to reduce the huge code duplication that was affecting the existing ControlBoardRemapper and wrapper, thanks to the use of http://en.cppreference.com/w/cpp/language/pointer#Pointers_to_member_functions ). Eventually we may want to port this pattern to the existing devices.

[jgvictores]

1. Very cool! :smiley::tada::tada::tada:
2. ((Affects MultipleAnalogSensorsInterfaces.h too))
   Maybe classes can now be...
   
   
   
   1. Changed to singular?
   
   
   2. API with shorter names as proposed above?
   
   

Minor comments:

1. Re: http://en.cppreference.com/w/cpp/language/pointer#Pointers_to_member_functions -> Have a certain intuition that this could be accomplished via polymorphism and it would be more c++-ish. However, no notes on this in YARP nor Google style guides, and the reference is actually cppreference.com so... :zipper_mouth_face: :-)
2. Small typos here and here: indeces -> indices.

[traversaro]

Regarding the general name scheme of functions in proposed API: while I usually like unified names (a simple getName rather than getThreeAxisGyroscopeName), I understand this particular use case and could live with the long names. Okay on that side. +1

The problem here is not just stylistic: if we want that a device can implement several interfaces, we need them to have distinct name. The alternative is to have non-sensor specific interfaces, but a feedback about that was exactly why https://github.com/robotology/yarp/issues/747 was rejected in the first place.

Re: http://en.cppreference.com/w/cpp/language/pointer#Pointers_to_member_functions -> Have a certain intuition that this could be accomplished via polymorphism and it would be more c++-ish

I am happy to learn eventual alternative solution to this kind of code duplication, that is really serious: check https://github.com/robotology/yarp/blob/master/src/libYARP_dev/src/devices/ControlBoardRemapper/ControlBoardRemapper.cpp#L516 or https://github.com/robotology/yarp/blob/master/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.cpp#L956 .

Small typos here and here: indeces -> indices.

Thanks! Apparently I am always prone to the same errors ( https://github.com/robotology/idyntree/issues/308 ).

[jgvictores]

Ok got it!

[barbalberto]

I also vote for different methods names. The only one which can be shortened is the getName instead of getThreeAxisGyroscopeName paying a but in complexity, but I think doens't worth the effort.

Back on getRefName.

Compare with ROS:

What ROS is doing for Gazebo plugin is simply this:

  if (!this->sdf->HasElement("frameName"))
  {
    ROS_INFO_NAMED("imu", "imu plugin missing <frameName>, defaults to <bodyName>");
    this->frame_name_ = link_name_;
  }
  else
    this->frame_name_ = this->sdf->Get<std::string>("frameName");

i.e. look for a param <frameName> in the config file and use it. If missing, it fallbacks to <bodyName> so, since URDF does not support this piece of information, they are adding it externally.
I suggest we should follow the same path.

perhaps we'd end up duplicating the job of the FrameTransformServer/FrameTransformClient pair.

Those devices provides a dynamic computation between reference frames in real time, and that's too much, we don't need it. We just need to state which ref frame the measures are referred to.

What we could do is provide a FrameTransform to a known reference frame. In this case we just need to fill in a data structure and send it via yarp to whoever is interested, like the GazeboYarpModelPosePublisher is doing.
So instead of getRefName it could be a getRefTransform returning a FrameTransform and this info can be read from URDF.

Anyway, methods in a interface can be marked as 'not implemented' where they don't fit, like many methods in the GazeboYarpPlugins

bool GazeboYarpControlBoardDriver::setImpedanceVelocityMode(int) //NOT IMPLEMENTED
{
    return false;
}
bool GazeboYarpControlBoardDriver::setMaxCurrent(int, double) //NOT IMPLEMENTED
{
    return true;
}
... and so on

Conclusion:

• Adding a method which does not fit into a particular implementation is not a problem, it can be marked as not implemented
• A possible default implementation could could return a default string like sensoName_frame or even do what ROS is doing. This will also easy the translation to/from ros for enhancing compatibility .
• If the needs of the getRefXXX will arise later on, it is much more difficult to add it when the interface is already been published and used due to backward compatibility. At that point setting a default value will be more complex.

[traversaro]

Fixed by https://github.com/traversaro/yarp/commit/8a9bd443a3402159ea51d2e32fab6c1438e0c703 and https://github.com/robotology/yarp/pull/1676 .