Implement and Test VirtualJointKinSensor in ICub wearable device

[yeshasvitirupachuri]

Following the addition of VirtualJointKinSensor #31 in this issue, we track the details related to the implementation of the sensor in ICub wearable device. With this implementation, ICub wearable device will connect to the robot control boards and stream the joint quantities (position velocity acceleration) as wearable data.

This implementation is done in dae6687 and the data from the robot seems to be parsed correctly as wearable data from the producer side.

Soon I will test this on the consumer side through IWearRemapper and see if the wearable data is read correctly.

@diegoferigo @lrapetti @DanielePucci

[yeshasvitirupachuri]

The ICub wearable device is sometimes failing to run correctly and throwing the error from IWearWrapper

[ERROR]IWearWrapper : The status of the IWear interface is not Ok 0 )

Upon preliminary investigation, this error output is during the call to the ICub wearable device getStatus methods. I added a bit of debugging code and notice some of the joint sensors are throwing this error when sensor status Ok check fails. Also, different sensors are failing at different runs. This is quite fishy because during the joint sensor initialization, I am setting the sensor status to Ok here through the setStatus method. So, I would expect the sensor status to be maintained correctly to Ok as the status is not updated anywhere else in the ICub device.

One wild bug that may be happening is, that IWearWrapper is called well before the initialization of all the joints (as there are multiple control boards). So, I changed the config file to have only the head control board (that has only 3 joints), but still the same problem exists. 😕

@diegoferigo let me know if you can think of something. Thanks.

[yeshasvitirupachuri]

I did more debugging and discovered that the first joint sensor status setting is failing at times this is probably dude the failure to call correct encoder interfaces. So, I updated the interface pointer handling in joint sensor constructor e69b83a and now ICub wearable device is running without any errors from IWearRemapper due to sensor status failures.

[diegoferigo]

@Yeshasvitvs As discussed f2f, please move the code that can fail from the constructor to a separated method, and call it after the objects are contructed, handling possible errors. Also, check the return value of the yarp interfaces.

[yeshasvitirupachuri]

@diegoferigo I ran into a tricky situation!
When the new sensors are added and implemented in wearables library, the virtual methods are added to all the devices like Wrapper, Remapper, FTShoes, XsensSuit. Then while building all the devices are built correctly, and xsens suit device is built only if the driver is present. https://github.com/robotology-playground/wearables/blob/1857545b5c500dfac9a57355cb12894c366c516e/devices/CMakeLists.txt#L8

Now, the tricky part is, when I run wrapper and remapper, they expect some kinda update from the newly added sensor type as they are build with the new sensor implementation. But as the xsens suit device is not built, it will not give any update on this sensors I think. So, the wrapper is kinda stuck in waiting for first data during the device getStatus call. I need to double check this behaviour with online data tomorrow, but this is the behaviour I notice when I am trying to run the HDE with remapper called on the Xsens wearable data streamed through yarpdataplayer.

CC @lrapetti

[diegoferigo]

I didn't fully understood the problem @Yeshasvitvs. Is your recording fresh from yesterday?

[yeshasvitirupachuri]

No the recording is an old one without the ‘()’ for the newly added sensor. Is there a way to control the type of sensors to be handled by wrapper and remapped ?

[diegoferigo]

This is what I suspected. I fear that the thrift message must be complete at the moment, even if I marked the fields as optional. Though, this behavior has never been tested. I am not even sure that having an optional field means the possibility of not having the () (how would thrift understand it? There is no label for the fields as far as I remember).

[yeshasvitirupachuri]

I think the catch is inside the wrapper and remapper. Inside the wrapper run() we are calling all the sensors. If a wearable device doesn't have a sensor implemented, then that sensor data place holder in the wearable message is plain (). So, to have control over a sensor presence in a wearable device, we can add that as an optional parameter from the configuration file of the wrapper. What do you think? @diegoferigo

I know this is a bit of unnecessary control over the sensors but I think it will be a good option for future wearable devices that may depend on some sdk to be necessary to build a particular wearable device.

[diegoferigo]

to have control over a sensor presence in a wearable device, we can add that as an optional parameter from the configuration file of the wrapper. What do you think?

This would be an overkill and unnecessarily complicated. I am pretty sure that somehow this can be sorted out in the thrift level, but I did not have enough thrift hands-on to point you to the right solution.

The problem is still not 100% clear to be honest. If I understood, this happens only if producer (e.g. XsensSuit coupled with IWearWrapper) and consumer (e.g. IWearRemapper) are not compiled with the same thrift file, is this right? If this is true, and if both ends use the same generated class, this problem would not be present.

[yeshasvitirupachuri]

The problem is still not 100% clear to be honest

Addition of new sensors like the joint sensor needs an implementation of the sensor virtual methods in the wearable devices. The wrapper and remapper devices consider all the sensors present in the wearables library. So, when we attach a wearable device like the XsensSuit or ICub to the wrapper, the wrapper expects all the sensors virtual methods to be handled in the wearable device. The first problem is that, when running HDE with a wearables offline data (which does not have an additional place holder for the newly added joint sensor), So, the remapper present in the HDE config file that is reading /Xsens/WearableData port (from yarpdataplayer) is displaying

This message is displayed when the attached iWear interface getStatus() method is called. If I am not mistake, this call goes to the IWearRemapper getStatus(), where pImpl->firstRun is checked. I believe it is getting stuck here with remapper status as WaitingForFirstRead @diegoferigo

[diegoferigo]

Addition of new sensors like the joint sensor needs an implementation of the sensor virtual methods in the wearable devices.

when we attach a wearable device like the XsensSuit or ICub to the wrapper, the wrapper expects all the sensors virtual methods to be handled in the wearable device.

This is not the current situation. All our wearable devices (XsensSuit, FTShoes, ...) miss some sensors, and the logic is to add dummy inlined definition is their header (see here)

The wrapper and remapper devices consider all the sensors present in the wearables library.

They try to read all the sensors, but if the wearable device does not provide any sensor reading for a given type, the thrift map entry of the WearableData struct is empty.

The first problem is that, when running HDE with a wearables offline data (which does not have an additional place holder for the newly added joint sensor)

As I wrote above, you cannot assume that recordings serialized with an old thrift can be read also after you edited the thrift. This can be done only by telling thrift that some data is optional, and despite I assume there is a way to do it in thrift, how to do it has yet to be investigated.

In short, if you change the serialization, old recordings are no longer valid. I suspect that you get that message in HumanStateProvider because the callback on inputs ports of the remapper (the read the WearableData thrift) are never called, since they expect a message with a new field (the new sensors) and you are streaming an offline recorded data that does not match the expected type. This causes the message to be discarded, and the onRead callback is never called.

[yeshasvitirupachuri]

The callback onRead() of the wearable data buffered port is never called.

[yeshasvitirupachuri]

In short, if you change the serialization, old recordings are no longer valid. I suspect that you get that message in HumanStateProvider because the callback on inputs ports of the remapper (the read the WearableData thrift) are never called, since they expect a message with a new field (the new sensors) and you are streaming an offline recorded data that does not match the expected type. This causes the message to be discarded, and the onRead callback is never called.

Yeah, that makes sense!

[yeshasvitirupachuri]

I tested the ICub wearable device with VirtualJointKinSensor using both the robot in gazebo simulation and also on the real robot. The device is working correctly and the joint data (position, velocity, and acceleration) from the robot are correctly sent as wearable data.