Add parameter period to set the output frequency of XSens device

[HosameldinMohamed]

Fixes #33.

[HosameldinMohamed]

CC @gabrielenava

[traversaro]

The latest style for most YARP devices is to use seconds to represent periods, see for example https://www.yarp.it//v3.5/classControlBoard__nws__yarp.html#details . Even if some icub devices still use milliseconds, I would try to be consistent with the new devices and use seconds here.

[HosameldinMohamed]

Ok now the expected period is in seconds 28bdd1b

[HosameldinMohamed]

Behavior test

I am testing with the following command (selecting the period)

yarpdev --device inertial --name /icub/xsens_inertial --period 0.002 --subdevice xsensmt --serial /dev/ttyXsens --xsensmt_period 2.7

It seems that the SDK is able to configure the IMU with the period I am selecting.

When I select xsensmt_period=2 (500Hz) I got a continuous stream of the warning

[WARNING] xsensmt: Missing magnetometer message, skipping magneto update.

This agrees with what @gabrielenava and @affafjunaidMOMIN found in the manual that the max output frequency is 400Hz (2.5ms) for MTi-300.

But I get the same behavior above (steam of warning regarding magnetometer) when I select the period 2.5ms.

Instead when selecting the period 2.6ms, I get the same warning for all the quantities (orientation, ACC, gyro, magneto), but only once in a while.

[traversaro]

You can also think of having separate parameters for each quantity.

[HosameldinMohamed]

This agrees with what @gabrielenava and @affafjunaidMOMIN found in the manual that the max output frequency is 400Hz (2.5ms) for MTi-300.

Update: the actual max period we can get is around 5ms (200Hz). This is checked by running the following:

• code at the current status: 5fa0899

• Running

yarpdev --device inertial --name /icub/xsens_inertial --period 0.003 --subdevice xsensmt --serial /dev/ttyXsens --xsensmt_period 3

Then yarpdatadumper

yarpdatadumper --name /dumper/xsens_inertial --connect /icub/xsens_inertial --rxTime --txTime

And checking the sender time of the logged data.

If I understood correctly from the code, the device publishes new line in the YARP port as soon as it gets a new data samples from the IMU. So the port sending time is indicative of the frequency of IMU (@traversaro can you confirm?)

So it seems that the IMU output frequency saturates at 200Hz. In the manual it says 400Hz, except for the Magnetometer (100Hz). See table 16 in page 32 of the manual.

[traversaro]

If I understood correctly from the code, the device publishes new line in the YARP port as soon as it gets a new data samples from the IMU. So the port sending time is indicative of the frequency of IMU (@traversaro can you confirm?)

No, the hardware device make the new measurements available in the C++ methods such as getOrientationSensorMeasureAsRollPitchYaw or read as soon as they arrive. Then the Network Wrapper Server device that is attached to the xsensmt device, such as inertial, calls read and publish it on the YARP port with the frequency given by its own thread.

[HosameldinMohamed]

No, the hardware device make the new measurements available in the C++ methods such as getOrientationSensorMeasureAsRollPitchYaw or read as soon as they arrive. Then the Network Wrapper Server device that is attached to the xsensmt device, such as inertial, calls read and publish it on the YARP port with the frequency given by its own thread.

Ah okay, then it is strange that port is published in not more than 5ms even if I put a lesser value. Maybe there's a saturation implemented in the Network Wrapper Service?

[HosameldinMohamed]

Ah okay, then it is strange that port is published in not more than 5ms even if I put a lesser value. Maybe there's a saturation implemented in the Network Wrapper Service?

I did a quick check, I am suspecting that https://github.com/robotology/yarp/blob/5804efb739319aee0bf1bfe408d80b31e7195238/src/devices/ServerInertial/ServerInertial.cpp#L284 is responsible.

[traversaro]

No, the hardware device make the new measurements available in the C++ methods such as getOrientationSensorMeasureAsRollPitchYaw or read as soon as they arrive. Then the Network Wrapper Server device that is attached to the xsensmt device, such as inertial, calls read and publish it on the YARP port with the frequency given by its own thread.

Ah okay, then it is strange that port is published in not more than 5ms even if I put a lesser value. Maybe there's a saturation implemented in the Network Wrapper Service?

Good point, we can check in the code: <insert link>.

[HosameldinMohamed]

Good point, we can check in the code: <insert link>.

I didn't understand,, wrong place?

[traversaro]

Good point, we can check in the code: <insert link>.

I didn't understand,, wrong place?

Sorry, I wanted to insert the link but I am lagging behind.

[HosameldinMohamed]

this? #34 (comment)

[traversaro]

this? #34 (comment)

Yes!

[traversaro]

@HosameldinMohamed please write me on Teams if you want to quickly align on this to proceed with this PR.

[HosameldinMohamed]

Ah okay, then it is strange that port is published in not more than 5ms even if I put a lesser value. Maybe there's a saturation implemented in the Network Wrapper Service?

I am testing with the fakeIMU device wrapped by the inertial device, using the command:

yarpdev --device inertial --name /fakeHa --period 0.001 --subdevice fakeIMU

The response is as expected, and the actual period is close to 1ms. I checked the timestamps using yarpdatadumper.