ami-iit/yarp-openmct

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

Closed this issue · 3 comments

nunoguedelha commented

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 33e7290:
    • Tested #123 => OK!
    • Tested #124 => IMU sensor modalities order was wrong in the parser, resulting in a mismatched display of the measurements. --> bf79b9c
    • Angular measurements had wrong unit: should be degrees. --> b6416b4
    • Tested #125 => OK!
  • commit 63ec06e:
    • Tested #126 => Telemetry server startup fails with error message similar to "Cannot set property 'x' of undefined" around

      yarp-openmct/iCubTelemVizServer/icubtelemetry.js

      Lines 210 to 213 in 63ec06e

      ICubTelemetry.prototype.parseFromId = function (id,sensorSample) {
      const parseFTmasData = function (subId,sensIdx,sensorSample) {
      this.state[subId].force.x = sensorSample[sensIdx][0][0][0];
      this.state[subId].force.y = sensorSample[sensIdx][0][0][1];
      --> 413e96d
    • the modalities wrong order in the parser also affected the FT sensors parsing. --> b6416b4
    • 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. --> bf79b9c
nunoguedelha commented

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 commented

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.

yarp-openmct/iCubTelemVizServer/icubtelemetry.js

Lines 315 to 319 in 63ec06e

case "sens.leftFootHeelTiptoeFTs":
for (let [subId,sensIdx] in [["leftFoot"+"HeelFT",0],["leftFoot"+"ToetipFT",1]]) {
parseFTmasData(subId,sensIdx,sensorSample);
}
break;

  • 👍1