[T256] Stream pose data as numerical value

[HosameldinMohamed]

I am using the device to stream the T265 pose and I wanted to stream the data in Simulink using WB Toolbox.

The issue is that I had to launch the device as multipleanalogsensorsserver and can't be read directly using WBToolbox currently robotology/wb-toolbox#116.

So I modified it slightly to publish the data as analogServer by implementing the IAnalogSensor interface. So by running

yarpdev --device analogServer --name /t265 --period 10 --subdevice realsense2Tracking

I am able to read the port easily in Simulink. It publishes the pose as:

<positionX positionX positionX QuaternionW QuaternionX QuaternionY QuaternionZ>

I did the modifications in a fork https://github.com/HosameldinMohamed/yarp-device-realsense2/tree/streamSignalPort

Do you think it makes sense to add this feature? Or something similar? Thanks.

[Nicogene]

Hi @HosameldinMohamed !

For me is ok if

So I modified it slightly to publish the data as analogServer by implementing the IAnalogSensor interface

is added and is not replacing the MAS interfaces implementation

cc @randaz81

[HosameldinMohamed]

is added and is not replacing the MAS interfaces implementation

Yes, the IAnalogSensor as added without modifying the existing interface.

[Nicogene]

Ok cool, so feel free to proceed with the PR 👍🏻

[traversaro]

As @randaz81 noted in the PR comment, this may not be the most elegant solution, as the analogServer device and the IAnalogSensor in general are meant to be removed from YARP (@randaz81 @drdanz there is any notice/warning related to that?). However, this is just a immediate solution to the more general problem that data streamed by MultipleAnalogSensors devices is not easily interpretable and/or accessible from software that expect to interact with Vector or Vector-like data types, such as WB-Toolbox Simulink block YarpRead and the plotting tool yarpscope.

A better solution could be to have some mechanism in YARP to automatically publish MAS selected sensors on vectors (see https://github.com/robotology/yarp/issues/1693) or to modify software to read structured data (see https://github.com/robotology/wb-toolbox/issues/116 for WB-Toolbox, not sure if there is any plan for doing something similar at yarpscope level). However, this addition is not invasive, so I think we can adopt it until there is a better alternative.

[randaz81]

I created a port monitor to support your use case: https://github.com/robotology/yarp/pull/2610 Can you tell me if it fits your needs?

NB: I used RPY angles instead of quaternions. Please tell me if quaternions are mandatory for you.

[HosameldinMohamed]

I created a port monitor to support your use case: robotology/yarp#2610 Can you tell me if it fits your needs?

Thanks @randaz81! I am trying to test it but I am not able to do it.

I installed YARP on your branch and tried to follow the usage example https://github.com/robotology/yarp/pull/2610#issue-670508570, but I don't fully understand the command.

After launching this device:

yarpdev --device multipleanalogsensorsserver --name /t256 --period 10 --subdevice realsense2Tracking

I put

yarp connect /t256/measures:o /in tcp+recv.portmonitor+type.dll+file.sensorMeasurements_to_vector

But I am getting an error.

NB: I used RPY angles instead of quaternions. Please tell me if quaternions are mandatory for you.

Yes, it would be very nice if we can receive the quaternions.

[traversaro]

But I am getting an error.

Can you report the error you get? If you compiled YARP manually, did you enabled the ENABLE_yarpcar_portmonitor option (see https://github.com/robotology/robotology-superbuild/blob/master/cmake/BuildYARP.cmake#L67) enabled?

[HosameldinMohamed]

Can you report the error you get?

[ERROR] |yarp.os.Network| Failure: could not find destination port tcp+recv.portmonitor+type.dll+file.sensorMeasurements_to_vector://in

If you compiled YARP manually, did you enabled the ENABLE_yarpcar_portmonitor option (see https://github.com/robotology/robotology-superbuild/blob/master/cmake/BuildYARP.cmake#L67) enabled?

I double-checked, it's enabled.

@traversaro My mistake is that I didn't create the input port /in before running the connect.

After I did it I got when running the connect command mentioned above:

[INFO] |yarp.os.Network| Success: Added connection from /t256/measures:o to tcp+recv.portmonitor+type.dll+file.sensorMeasurements_to_vector://in

But then in the terminal of the input port I got this:

[INFO] |yarp.os.impl.PortCoreInputUnit| Receiving input from /t256/measures:o to /in using tcp+recv.portmonitor+type.dll+file.sensorMeasurements_to_vector
[ERROR] |yarp.os.YarpPluginSettings| Cannot find "sensorMeasurements_to_vector" plugin (not built in, and no .ini file found for it)Check that YARP_DATA_DIRS leads to at least one directory with plugins/sensorMeasurements_to_vector.ini or share/yarp/plugins/sensorMeasurements_to_vector.ini in it
Carrier "portmonitor" could not configure the send delegate.
[INFO] |yarp.os.impl.PortCoreInputUnit| Removing input from /t256/measures:o to /in

I am not sure if the sensorMeasurements_to_vector is installed correctly.

[traversaro]

Perhaps you need also to enable the option ENABLE_yarppm_sensorMeasurements_to_vector?

[HosameldinMohamed]

Perhaps you need also to enable the option ENABLE_yarppm_sensorMeasurements_to_vector?

Thanks @traversaro, I wasn't aware of that. Now it works fine OK.

Probably I am missing some basic knowledge of YARP ports. But with what I did in https://github.com/robotology/yarp-device-realsense2/issues/7#issuecomment-862418246, the /in port is an input port. So I can't read the sig::vector data using yarp read ... \in for example. Is there a way to publish it also in an output port?

[traversaro]

In general in YARP ports are bi-directional entities, unless you specify them as input or output ports in the code. The yarp read ... \in command is basically is a short-hand for yarp read ... (i.e. create a temporary port with a random name) and yarp connect <name_of_temporary_port> /in . I don't know if using yarp read ... /in you can specify the carrier. However, you should be able to read the data as a vector by doing:

yarp read /in
yarp connect /t256/measures:o /in tcp+recv.portmonitor+type.dll+file.sensorMeasurements_to_vector

[HosameldinMohamed]

However, you should be able to read the data as a vector by doing:

yarp read /in
yarp connect /t256/measures:o /in tcp+recv.portmonitor+type.dll+file.sensorMeasurements_to_vector

@traversaro Yes, I did the same. I could see the vector data being streamed in the terminal where I ran yarp read /in.

The problem is how to use the WB-Toolbox Simulink block YarpRead? I used it with the port /in but it didn't work. I assume it's related to that yarp read ... /in is not allowed. The latter gives the error:

[ERROR] |yarp.os.Network| Failure: Outputs not allowed

[traversaro]

The YarpRead block has two modalities (see https://github.com/robotology/wb-toolbox/blob/master/toolbox/library/src/YarpRead.cpp#L245):

• If autoconnect is true, the portName parameter specifies the source port, and a temporary port is create with the ... shorthand for the destination port.
• If autoconnect is false, then portName specified the destination port opened by WB-Toolbox block, and the user needs to create the connection manually via the yarp connect command.

To be able to use portmonitor with the autoconnection port set to true, we probably need to add a carrier option for the YarpRead block, that by default is tcp and that in this case we would like to set to tcp+recv.portmonitor+type.dll+file.sensorMeasurements_to_vector . Can you open an issue in WB-Toolbox for this? Thanks!

[traversaro]

Can you open an issue in WB-Toolbox for this? Thanks!

Actually this is already tracked in https://github.com/robotology/wb-toolbox/issues/42, no need to open a new issue.

[HosameldinMohamed]

Hi @traversaro @randaz81, how do you think we can proceed with this?

[HosameldinMohamed]

Hi! This issue is probably outdated! For us, our use has changed (the need to stream the values as a vectored port). We were using Simulink and WBT but not at the moment!

[traversaro]

Ok, at this point I think we can close the issue if there is no use case from you, thanks!