Closed nunoguedelha closed 2 years ago
CC @S-Dafarra .
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:
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.
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;
}
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;
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)
( ((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:
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
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.
parseFTmasData()
function had thebind
missing, required for usingthis
keyword in the function body. --> bf79b9c2db91f116a4e0203009aa46e3032569e0