Add iFeel suit ports to the telemetry server and respective entries in the visualizer GUI

nunoguedelha commented 2 years ago

We discussed with @RiccardoGrieco on the requirements for visualizing the iFeel suit sensor information within the Open-CT based visualizer tool. We recap below a quick guide on how to add a new telemetry data source Yarp port to the modules iCubTelemVizServer and openmctStaticServer:

(throughout the following description, we shall use the IMU measurements entry as an example)

In iCubTelemVizServer/iCubTelemVizServer.js: add the port configuration entry in the portInConfig JSON string^(*) (fields: port Yarp and local names, data type).
```
var portInConfig = {
   "sens.imu": {"yarpName":'/icubSim/inertial', "localName":'/yarpjs/inertial:i',"portType":'bottle'},
   ...
}
```
In iCubTelemVizServer/icubtelemetry.js:
- Add a new entry to the JSON string this.state in the constructor ICubTelemetry, the internal buffer where we store the samples read from YARP and parsed.
- The new entry is key/value pair where the key is the chosen id of the telemetry entry, and the value is a number or a nested JSON string holding the signal sub-fields. The same id shall be used in the dictionary.js defining the telemetry entries in the visualizer client.
```
this.state = {
   "sens.imu": {
     "ori": {"roll": 0, "pitch": 0, "yaw": 0},
     "acc": {"x": 0, "y": 0, "z": 0},
     "gyr": {"x": 0, "y": 0, "z": 0},
     "mag": {"x": 0, "y": 0, "z": 0}
   },
   ...
}
```
- Add the data parser to ICubTelemetry.prototype.updateState.
```
switch(id) {
   case "sens.imu":
       this.state[id].ori.roll = sensorSample[0];
       this.state[id].ori.pitch = sensorSample[1];
       ...
}
```
- If the entry in this.state has nested structures, it has to be flattened during the telemetry sample generation in ICubTelemetry.prototype.generateTelemetry: add the entry id in the switch case which applies this.flatten().
```
switch(id) {
   case "sens.imu":
   case "sens.leftLegState":
   case "sens.leftLegEEwrench":
   case "sens.rightLegEEwrench":
   case "sens.batteryStatus":
       var telemetrySample = this.flatten({timestamp: timestamp, value: value, id: id});
       break;
   default:
       var telemetrySample = {timestamp: timestamp, value: value, id: id};
}
```
In openmctStaticServer/dictionry.json (defines th telemetry entries in the visualizer web interface):
- Add a new entry in the measurements list "measurements"^(**). The format should be as follows, for the IMU measurements example:
```
{
 "name": "iCub Telemetry",
 "key": "sc",
 "measurements": [
   {
     "name": "IMU sensor measurements",
     "key": "sens.imu",
     "values": [
       {
         "key": "value.ori.roll",
         "name": "Roll",
         "units": "degrees",
         "format": "float",
         "hints": {
           "range": 1
         }
       },
       ...
       {
         "key": "utc",
         "source": "timestamp",
         "name": "Timestamp",
         "format": "utc",
         "hints": {
           "domain": 1
         }
       }
     ]
   },
   ...
 ]
}
```
- In this example, "sens.imu"is the same id used in the this.state entry.
- Blocks having the "hints": {"range": x} assign the measurement component to the Y axis, while blocks having the "hints": {"domain": x} assign the component to the X axis.
- The key for each measurement component is the concatenation of keys to reach that component in the internal buffer variable this.state.
- In "hints": {"range": x}, "x" is the order of appearence in the Y axis drop down menu.

(*) A JSON string is a string containing nested JSON object literals. A JSON object literal is a list of key/value pairs. Refer to https://www.w3schools.com/js/js_json_objects.asp. (**) In this example the IMU measurements are included in the group "iCub Telemetry" but you can create a new group of measurements where to add your new telemetry entries.

nunoguedelha commented 2 years ago

Hi @RiccardoGrieco , let me know if I left some point obscure of it's clear enough, or feel free to amend the guide yourself.

nunoguedelha commented 2 years ago

CC @evalli-iit , @lrapetti , @S-Dafarra , @traversaro

S-Dafarra commented 2 years ago

I think this is way too complicated to be done every time we need to add a new port :thinking:

RiccardoGrieco commented 2 years ago

@nunoguedelha thank you for the recap, I think you have not missed anything.

Anyway, as discussed with @evalli-iit and @lrapetti, the idea is to monitor and control all the nodes of the iFeel system with a more "product-oriented" GUI, that also does not need YARP to run. Therefore the visualizer will not be used for this purpose.

The tool will instead be used to visualize data related to the human state, but further discussion is still needed.

For the moment, I'm keeping this issue in the icebox.

nunoguedelha commented 1 year ago

Ciao @RiccardoGrieco , could you please give me the following info:

port name(s) publishing the iFeel data
info on the data...
- If it uses a Multiple Analog Sensor wrapper, the device wrapper configuration files should be enough, OR
- if it uses any other wrapper, I need the details on the data structure and types (double, string, etc) and/or YARP device (wrapper) name (or class) publishing the data + respective config files.

RiccardoGrieco commented 1 year ago

At the following link the definition of the messages: https://github.com/robotology/wearables/blob/b9a6fe404b837e1b594c89ac1a1ac37dd46a1567/msgs/thrift/WearableData.thrift#L173-L191

Attached instead there is the output of yarp read. The port is/iFeelSuit/WearableData/data:o.

iFeelSuitMessages.txt

RiccardoGrieco commented 1 year ago

A small dataset that can be replayed with yarpdataplayer --withExtraTimeCol 2

ifeel_data.zip

nunoguedelha commented 1 year ago

@RiccardoGrieco , I would have a few questions regarding the format file. Below I mention the higher structure and as an example the Accelerometer structure: https://github.com/robotology/wearables/blob/b9a6fe404b837e1b594c89ac1a1ac37dd46a1567/msgs/thrift/WearableData.thrift#L173-L191

struct WearableData {
1: required string producerName;
2: optional map<string,Accelerometer> accelerometers;
3: optional map<string,EmgSensor> emgSensors;
4: optional map<string,Force3DSensor> force3DSensors;
5: optional map<string,ForceTorque6DSensor> forceTorque6DSensors;
6: optional map<string,FreeBodyAccelerationSensor> freeBodyAccelerationSensors;
7: optional map<string,Gyroscope> gyroscopes;
8: optional map<string,Magnetometer> magnetometers;
9: optional map<string,OrientationSensor> orientationSensors;
10: optional map<string,PoseSensor> poseSensors;
11: optional map<string,PositionSensor> positionSensors;
12: optional map<string,SkinSensor> skinSensors;
13: optional map<string,TemperatureSensor> temperatureSensors;
14: optional map<string,Torque3DSensor> torque3DSensors;
15: optional map<string,VirtualLinkKinSensor> virtualLinkKinSensors;
16: optional map<string,VirtualJointKinSensor> virtualJointKinSensors;
17: optional map<string,VirtualSphericalJointKinSensor> virtualSphericalJointKinSensors;
}

https://github.com/robotology/wearables/blob/b9a6fe404b837e1b594c89ac1a1ac37dd46a1567/msgs/thrift/WearableData.thrift#L89-L92

struct Accelerometer {
  1: SensorInfo info;
  2: VectorXYZ data;
}

https://github.com/robotology/wearables/blob/b9a6fe404b837e1b594c89ac1a1ac37dd46a1567/msgs/thrift/WearableData.thrift#L17-L20

struct SensorInfo{
  1: string name;
  2: SensorStatus status = SensorStatus.UNKNOWN;
}

Are the higher level map key and the SensorInfo.name always the same on the currently streamed data?
If that's the case, out of curiosity, why the redundancy?
Why not just use n array of Accelerometer structures, or the Accelerometer structure have only the status and data?

lrapetti commented 1 year ago

I will try to answer

Are the higher level map key and the SensorInfo.name always the same on the currently streamed data?

If that's the case, out of curiosity, why the redundancy?

Why not just use n array of Accelerometer structures, or the Accelerometer structure have only the status and data?

Yes, that seems to be the case, it can be seen from the code here and here. Actually, it seems to be redundant, but changing it would break the compatibility with all the previous dataset, so for the time being I would avoid to consider it.

nunoguedelha commented 1 year ago

Sure, it was just for my understanding. It's pretty clear, thanks @lrapetti .

nunoguedelha commented 1 year ago

Implementation steps

In order to properly handle the optional telemetry entries and group them conveniently, we shall use here the capability of adding sub-folders to a telemetry dictionary (#133 , implemented by #149 ).

[x] Finish the implementation for supporting sub-folders in a dictionary.
[x] Add the iFeel suit telemetry dictionary main entry and sub-folders (empty for now).
[x] Add the code skeleton for:
- the iFeel suit telemetry port configuration,
- the data parsing and telemetry generation,
- the dictionary generation based on the folders child entry IDs retrieved from the first received sample.
[x] Complete the telemetry sample parsing.
[x] Complete the folders child entry IDs retrieval implementation.

nunoguedelha commented 1 year ago

Add the iFeel suit telemetry dictionary main entry and sub-folders (empty for now)

Commit 80036a9f2ddf0e3d876ced686dc88c301a9960ab.

Dictionary structure

  "name": "iFeel Suit Telemetry",
  "key": "iFeelSuitTelemetry",
  ...
  "telemetryEntries": [
    {
      "name": "Accelerometers",
      "key": "accSens",
      "type": "folder",
      "telemetryEntries": []
    },
    {
      "name": "EMG Sensors",
      "key": "emgSens",
      "type": "folder",
      "telemetryEntries": []
    },
    ...
  ]

nunoguedelha commented 1 year ago

Edited https://github.com/ami-iit/yarp-openmct/issues/43#issuecomment-1275113568.

nunoguedelha commented 1 year ago

Updated the estimate to 5.

nunoguedelha commented 1 year ago

Add the code skeleton for the iFeel suit telemetry port configuration, the data parsing, the dictionary generation...

Parsing of the incoming Yarp data and notification to the web client:

All iFeel telemetry data is published on port port /iFeelSuit/WearableData/data:o and received on port iFeelSuitTelemetry.wearableData.
The telemetry data notification to the client is synched to incoming Yarp messages.
A class wearableDataParser is defined for parsing the incoming data.
Upon a data reading on port ID iFeelSuitTelemetry.wearableData, the method wearableDataParser.parse parses the data as follows:
- It generates a data sample per sensor subID and store it in state[subID].
- As usual, the subIDs match the ones used in the dictionary, following the tree structure used there: <dictionary-ID>.<sensor-modality-folder>.<sensor-sub-ID>.
- Initialises the respective history[subID] array if necessary.
- Returns the sensor subIDs. For now, the parser is a stub generating dummy data and returning subIDs iFeelSuit::acc::Node#10 and iFeelSuit::acc::Node#11.

https://github.com/ami-iit/yarp-openmct/blob/fd6931481ce0e098f82ae96fdb09c52e14361062/iCubTelemVizServer/wearableDataParser.js#L9-L19

Open-MCT populates the iFeel Telemetry tree through the objectAPI by calling the objectProvider and compositionProvider. During this process:

The composition provider has to return the namespace and keys of the input domainObject's child entries. This requires requesting the telemetry server to send back the telemetry IDs of the sensors associated to the domainObject. The request function requestFolderTelemetryEntryKeys takes the folder identifier key as input and returns the child telemetry entries keys (not the composed ones).
In the compositionProvider, in order to have a single block doing the processing on the telemetryEntries, wether they exist as a non-empty subfield of the input domainObject or have to be retrieved from the server as mentioned above, we treat both cases as a Promise. Implemented in getFolderTelemetryEntryKeys function.
requestFolderTelemetryEntryKeys is for now a stub returning two accelerometer sensor IDs: iFeelSuit::acc::Node#10 and iFeelSuit::acc::Node#11.
We add the domain "timestamp" and range "status" values to the dictionary.
A specific domain object generator was implemented, generateObject4iFeelSuitTelemetry, for returning the properly formatted object through the ObjectAPI.
The generator is specifically selected for leaf entries in the dictionary. A more compact solution could have been implemented, but this was more straightforward.

Results

The overall code data flow is working except that the telemetry entries, iFeelSuitTelemetry.accSens.iFeelSuit::acc::Node#10 and iFeelSuitTelemetry.accSens.iFeelSuit::acc::Node#11, are not clickable, so no subscription is ever sent to the telemetry server.

Debugging now the Vue.js...

nunoguedelha commented 1 year ago

Telemetry Entries not Clickable...

Vue.js Debugging

We observe the click event handling when e click on an telemetry entry.

Successful case: if we click on the the entry iCub Telemetry->IMU sensors->Legacy IMU sensor measurements, we get the following function call sequence:

handleClick
boundFn
navigateOrPreview
navigate
handleLocationChange
doPathChange

Details on sequence ending...

https://github.com/nasa/openmct/blob/ab7e2c57470d3c6076459ad7e3eaa1883cc3b8b0/src/ui/router/ApplicationRouter.js#L300-L318 ``` /** * @private * Compare new and old path and on change emit event 'change:path' * * @param {string} newPath new path of url * @param {string} oldPath old path of url */ doPathChange(newPath, oldPath) { if (newPath === oldPath) { return; } let route = this.routes.filter(r => r.matcher.test(newPath))[0]; if (route) { route.callback(newPath, route.matcher.exec(newPath), this.currentLocation.params); } this.emit('change:path', newPath, oldPath); } ```

The full function doPathChange is executed and the change:path event is emitted.

Failing case: Instead, if we click on the the entry iFeel Suit Telemetry->Accelerometers->iFeelSuit::acc::Node#10, we get the same function call sequence as before, but with a different execution in the last function call doPathChange.

https://github.com/nasa/openmct/blob/ab7e2c57470d3c6076459ad7e3eaa1883cc3b8b0/src/ui/router/ApplicationRouter.js#L300-L318

    /**
     * @private
     * Compare new and old path and on change emit event 'change:path'
     *
     * @param {string} newPath new path of url
     * @param {string} oldPath old path of url
     */
    doPathChange(newPath, oldPath) {
        if (newPath === oldPath) {
            return;
        }

        let route = this.routes.filter(r => r.matcher.test(newPath))[0];
        if (route) {
            route.callback(newPath, route.matcher.exec(newPath), this.currentLocation.params); <-------- (*)
        }

        this.emit('change:path', newPath, oldPath);
    }

(*) The route to the new path never returns and times out and the event change:path is never emitted.

This is due to the fact that the routed path was /browse/yarpopenmct:iFeelSuitTelemetry/yarpopenmct:iFeelSuitTelemetry.accSens/yarpopenmct:iFeelSuitTelemetry.accSens.iFeelSuit::acc::Node where #10 is missing.

That last segment was truncated in handleLocationChange by the URL constructor: https://github.com/nasa/openmct/blob/ab7e2c57470d3c6076459ad7e3eaa1883cc3b8b0/src/ui/router/ApplicationRouter.js#L355

   let newLocation = this.createLocation(pathString);

https://github.com/nasa/openmct/blob/ab7e2c57470d3c6076459ad7e3eaa1883cc3b8b0/src/ui/router/ApplicationRouter.js#L287-L290

   let url = new URL(
       pathString,
       `${location.protocol}//${location.host}${location.pathname}`
   );

For allowing a free naming of the iFeel suit sensors, while avoiding such issues, we'll use just numbers as the last segment of the domain object keys.

Fixed in commit f5ff44d8b18a99ab10beb4cd6a07ca8c8b5df58e.

nunoguedelha commented 1 year ago

Complete the folders child entry IDs retrieval implementation

We create a wearable metadata route (sub-server) in the telemetry server for fulfilling the folder child entries list requests from the client app.
Folder child entry keys are requested by the Composition Provider invoked for a given folder by the Open-MCT ObjectAPI.
A request of the folder child entry keys is composed as follows: http://<telem-server-address-host>:<telem-server-address-port>/wearableMetadata/<folder-domain-object-key(s)>/childTelemEntryKeys The request can target a single folder or a comma separated set of folder.
Telemetry entry names are requested by the Object Provider invoked for the folder sub-entries by the Open-MCT ObjectAPI.
A request of Telemetry entry names is composed as follows: http://<telem-server-address-host>:<telem-server-address-port>/wearableMetadata/<folder-domain-object-key(s)>/<entry-key(s)>/name The request targets a single folder but can target a single entry key or a comma separated set of entry keys in that folder.

Result

Here we replayed with the yarpdataplayer the dataset ifeel_data.zip twice to trigger tje pipeline but we were only sending dummy data to the visualizer client.

Implemented in 927ae72d10742028e03cda16db1d3c318fa24162.

nunoguedelha commented 1 year ago

Complete the telemetry sample parsing

The producer name iFeelSuit:: is parsed (sensorSample[0]) but not used nor transmitted to the client visualiser for now as it is not required and appears anyway as a namespace in the sensor names.

In WearableDataParser, we replace the telemKeyTree structure of key-value pairs into a Map which remembers the insertion order of the keys. This is important for mapping properly the sensor modality array in the telemetry sample. Refer to the WearableData.thrift structure: https://github.com/robotology/wearables/blob/b9a6fe404b837e1b594c89ac1a1ac37dd46a1567/msgs/thrift/WearableData.thrift#L173-L191

struct WearableData {
1: required string producerName;
2: optional map<string,Accelerometer> accelerometers;
3: optional map<string,EmgSensor> emgSensors;
4: optional map<string,Force3DSensor> force3DSensors;
5: optional map<string,ForceTorque6DSensor> forceTorque6DSensors;
6: optional map<string,FreeBodyAccelerationSensor> freeBodyAccelerationSensors;
7: optional map<string,Gyroscope> gyroscopes;
8: optional map<string,Magnetometer> magnetometers;
9: optional map<string,OrientationSensor> orientationSensors;
10: optional map<string,PoseSensor> poseSensors;
11: optional map<string,PositionSensor> positionSensors;
12: optional map<string,SkinSensor> skinSensors;
13: optional map<string,TemperatureSensor> temperatureSensors;
14: optional map<string,Torque3DSensor> torque3DSensors;
15: optional map<string,VirtualLinkKinSensor> virtualLinkKinSensors;
16: optional map<string,VirtualJointKinSensor> virtualJointKinSensors;
17: optional map<string,VirtualSphericalJointKinSensor> virtualSphericalJointKinSensors;
}

Each of the optional[^1] lines 2 to 17 of the above structure is defined as a map which translates into a nested bottle of data from sensors of same modality in the YARP message, and results as a nested array sensorSample[i][] in the parser inputs. Let i be the modality index, i $\in [2,17]$. Below, sensorSample[1][] is the array holding the data of 10 accelerometers:
```
2: optional map<string,Accelerometer> accelerometers;
```
sensorSample[i][j] holds the key-value pair of the metadata/data of the sensor j of modality i (for instance the second entry of map<string,Accelerometer> accelerometers).

We actually discard the key which is redundant w.r.t. to the sensor metadata (refer to discussion https://github.com/ami-iit/yarp-openmct/issues/43#issuecomment-1270134328). We parse only the content of sensorSample[i][j][1] highlighted below (e.g. => 0: <sensor-name>, 1: <status>, 2: <acc.x>, 3: <acc.y>, 4: <acc.z>):
the sensor data (elements indexed 2 through N) is parsed following the formats vectorXYZ, quaternionWXYZ and vectorRPY.

Implemented in 4ebb02676bd230e2ccec2f3bba6ed40aeeb48ec0.

[^1]: Each of the Map telemKeyTree entries has a respective index which matches the modality index i. E.g. "iFeelSuitTelemetry.accSens" matches modality index 0.

nunoguedelha commented 1 year ago

Test and Issues

Tested https://github.com/ami-iit/yarp-openmct/files/9714389/ifeel_data.zip and found the following issues:

[x] 1. No data is plotted for entries Free Body Acceleration Sensors and Orientation Sensors.
[x] 2. In entry Virtual Link Kinetic Sensors, plot appears different between X, Y and Z components of each modality, but seems to be the same between modalities, which would be wrong.

nunoguedelha commented 1 year ago

Analysis Issue 1

Reference Object API call sequence

E.g. for the entry iFeelSuit::gyro::Node#1, we get the sequence:

-> navigateToPath(identifier)
    -> pathToObjects(identifier)
        -> migrate(identifier)
            -> openmct.objects.get(identifier)
                -> provider.get(identifier)

For the identifier.key iteratively equal to iFeelSuitTelemetry, iFeelSuitTelemetry.gyroSens, iFeelSuitTelemetry.gyroSens.0, which follows the decomposition of the last object path. Subsequently, we get the sequence:

-> getOriginalPath(iFeelSuitTelemetry.gyroSens.0)
    -> migrate(iFeelSuitTelemetry.gyroSens.0)
        -> ...
    -> getOriginalPath(domainObject.location = iFeelSuitTelemetry)
        -> migrate(iFeelSuitTelemetry)
            -> ...
        -> getOriginalPath(domainObject.location = ROOT)
            -> migrate(ROOT)
                -> ...

Faulty Object API call sequence

For the entry iFeelSuit::fbAcc::Node#1, only the first sequence is executed for each of the domain objects in the path:

-> navigateToPath(identifier)
    -> pathToObjects(identifier)
        -> migrate(identifier)
            -> openmct.objects.get(identifier)
                -> provider.get(identifier)

The call sequence initiating in getOriginalPath() does not occur. The problem occurs in between.

We then look into the domain object returned by the ObjectProvider for the object iFeelSuitTelemetry.FBaccSens.0. We can see in the below image that the returned result has wrong content in telemetry.values.

That is probably the cause preventing the execution sequence that should have followed.

nunoguedelha commented 1 year ago

Analysis Issue 1 ...continued

dictionary.presetValuesBase has wrong content for keys FBaccSens, orientSens and positionSens.

{
  "presetValuesBase": {
    "FBaccSens": "[object Object],[object Object],[object Object]"
    "orientSens": "[object Object],[object Object],[object Object],[object Object]"
    "positionSens": "[object Object],[object Object],[object Object]"
  }
}

There was a JSON.stringify() missing for those entries.

Fixed in 7e980cb7cced49b5902c71f97b9b09506ed59792.

nunoguedelha commented 1 year ago

Analysis Issue 2

For instance, value.oriQuat.w, value.pos.x, value.linearVel.x are always identical.

This can already be observed on the sample received on the client side: https://github.com/ami-iit/yarp-openmct/blob/45035b8cda4af25310d1f352196da692f9912e5e/openmctStaticServer/plugins/realtime-telemetry-plugin.js#L10
The sample data received by the telemetry server is ok, as well as the parser input https://github.com/ami-iit/yarp-openmct/blob/7e980cb7cced49b5902c71f97b9b09506ed59792/iCubTelemVizServer/wearableDataParser.js#L32
The issue is in this section, where we don't select the proper elements of sensorData: https://github.com/ami-iit/yarp-openmct/blob/7e980cb7cced49b5902c71f97b9b09506ed59792/iCubTelemVizServer/wearableDataParser.js#L62-L77

Fixed in 62ba537a6a3da320fef3718f391c3588bdfb7df6.

nunoguedelha commented 1 year ago

Optimised wearable data parsing in 75a4b570170f3ba6d47878465911a63cc935be03 and 390b246cd628a1aff415d0375d935706f48dd28e. Refer to commit comments for further details.

The goal was to:

move most of the parsing code to the parser anonymous functions which are faster than regular functions
build those anonymous functions in the WearableDataParser constructor which is executed once at initialisation.

This allows to execute, in the anonymous functions, lines like:

[parsedData.oriQuat.w,parsedData.oriQuat.x,parsedData.oriQuat.y,parsedData.oriQuat.z,parsedData.pos.x,parsedData.pos.y,parsedData.pos.z,parsedData.linearVel.x,parsedData.linearVel.y,parsedData.linearVel.z,parsedData.angVel.x,parsedData.angVel.y,parsedData.angVel.z,parsedData.linearAcc.x,parsedData.linearAcc.y,parsedData.linearAcc.z,parsedData.angAcc.x,parsedData.angAcc.y,parsedData.angAcc.z] = sensorData;

directly mapping the sensorData table elements to the fields of parsedData, instead of iteratively parsing the quaternion data, then the position, linear velocity, etc. Such line is not hardcode, but instead generated in the constructor from the structure WearableDataParser.telemKeyTree.

nunoguedelha commented 1 year ago

Work complete.

CC @S-Dafarra @RiccardoGrieco

ami-iit / yarp-openmct