Fix problems found when testing added logged signals (IMUs, End Effector Wrenches, FTs)

[nunoguedelha]

We've tested with @S-Dafarra the changes #123 , #124 , #125 and #126 last Friday 02/09/2022 and ran into a few problems, which we are fixing here.

• commit 33e7290953dd15aab99b6df3dadadb3553fedd9a:
  • [x] Tested #123 => OK!
  • [x] Tested #124 => IMU sensor modalities order was wrong in the parser, resulting in a mismatched display of the measurements. --> bf79b9c2db91f116a4e0203009aa46e3032569e0
  • [x] Angular measurements had wrong unit: should be degrees. --> b6416b45efa834145842b6801a319ca846d56cf3
  • [x] Tested #125 => OK!
• commit 63ec06ef158f174df4f9478329d8e9b34023cca5:
  • [x] Tested #126 => Telemetry server startup fails with error message similar to "Cannot set property 'x' of undefined" around https://github.com/ami-iit/yarp-openmct/blob/63ec06ef158f174df4f9478329d8e9b34023cca5/iCubTelemVizServer/icubtelemetry.js#L210-L213 --> 413e96dcc3b795ddaf97d66a43af23dfec256799
  • [x] the modalities wrong order in the parser also affected the FT sensors parsing. --> b6416b45efa834145842b6801a319ca846d56cf3
  • [x] After the above error was fixed, there was also a typo on the for loop and the parseFTmasData() function had the bind missing, required for using this keyword in the function body. --> bf79b9c2db91f116a4e0203009aa46e3032569e0

[nunoguedelha]

CC @S-Dafarra .

[nunoguedelha]

IMU sensor modalities order was wrong in the parser

As pointed out by @S-Dafarra, The structure of the MAS data writen to a YARP port is defined through a Thrift yarp/src/devices/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift:

https://github.com/robotology/yarp/blob/3535832d6234829187d94fcf09296ec9ac2a1e65/src/devices/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift#L13-L16

https://github.com/robotology/yarp/blob/3535832d6234829187d94fcf09296ec9ac2a1e65/src/devices/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift#L22-L34

Which gives the oder of sensor modalities. Furthermore, we can see in the generated files (idl_generated_code folder) the actual order in which the [measurement|template] chunks of data are written into the output buffer, and how they are grouped in case of multiple sensors of same modality.

Writing nested sensor measurements grouped by modality

yarp/src/devices/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp#L390:

void MultipleAnalogSensorsServer::run()
{
    => Prepares the data (mapped to structure SensorStreamingData)
    => Fills the structure SensorStreamingData
    => Writes the data to the output port

The data is written by m_streamingPort.write(), the write method of yarp::os::BufferedPort<SensorStreamingData>, which calls SensorStreamingData.write():

bool SensorStreamingData::write(const yarp::os::idl::WireWriter& writer) const
{
    if (!write_ThreeAxisGyroscopes(writer)) {
        return false;
    }
    if (!write_ThreeAxisLinearAccelerometers(writer)) {
        return false;
    }
    ...
}

So the data is written in the order set in the thrift file yarp/src/devices/multipleAnalogSensorsMsgs/multipleAnalogSensorsSerializations.thrift:

struct SensorStreamingData
{
  1: SensorMeasurements ThreeAxisGyroscopes;
  2: SensorMeasurements ThreeAxisLinearAccelerometers;
  3: SensorMeasurements ThreeAxisMagnetometers;
  4: SensorMeasurements OrientationSensors;
  5: SensorMeasurements TemperatureSensors;
  6: SensorMeasurements SixAxisForceTorqueSensors;
  7: SensorMeasurements ContactLoadCellArrays;
  8: SensorMeasurements EncoderArrays;
  9: SensorMeasurements SkinPatches;
  10: SensorMeasurements PositionSensors;
}

Writing the nested measurement block

write(yarp::os::ConnectionWriter& connection) ends up calling write(const yarp::os::idl::WireWriter& writer)... https://github.com/robotology/yarp/blob/3535832d6234829187d94fcf09296ec9ac2a1e65/src/devices/multipleAnalogSensorsMsgs/idl_generated_code/SensorMeasurement.h#L42-L43

    // Write structure on a Connection
    bool write(yarp::os::ConnectionWriter& connection) const override;

https://github.com/robotology/yarp/blob/3535832d6234829187d94fcf09296ec9ac2a1e65/src/devices/multipleAnalogSensorsMsgs/idl_generated_code/SensorMeasurement.cpp#L51-L63

bool SensorMeasurement::write(const yarp::os::idl::WireWriter& writer) const
{
    if (!nested_write_measurement(writer)) {
        return false;
    }
    if (!write_timestamp(writer)) {
        return false;
    }
    if (writer.isError()) {
        return false;
    }
    return true;
}

...resulting, for each measurement, in the nested block data format (e.g. orientation):

((Euler x-y-z) timestamp)

Overall nested format

( ((Gyroscope_1_x-y-z) timestamp) ((Gyroscope_2_x-y-z) timestamp) ) ( ((Accelerometer_1_x-y-z) timestamp) ((Accelerometer_2_x-y-z) timestamp) ) ( ((Magnetometer_1_x-y-z) timestamp) ((Magnetometer_2_x-y-z) timestamp) ) ( ((Euler_1_x-y-z) timestamp) ((Euler_2_x-y-z) timestamp) ) ( <temp> ) ( <FT> ) ( ... ) ( ... ) ( ... ) ( ... ) 

So, as the sensors are grouped together by modality, we should index the nested blocks inwards as follows:

• modality index -> sensor index -> measurements or timestamp selection index -> component (e.g. x, y or z) index.

[nunoguedelha]

Telemetry server startup fails with error message similar to "Cannot set property 'x' of undefined"

This was caused by a missing string chunk ("sens.") for selecting the telemetry entries sens.leftFootHeelFT, sens.leftFootToetipFT, sens.rightFootHeelFT and sens.rightFootToetipFT when iterating over the [subId,sensIdx] combinations. https://github.com/ami-iit/yarp-openmct/blob/63ec06ef158f174df4f9478329d8e9b34023cca5/iCubTelemVizServer/icubtelemetry.js#L315-L319