Bipedal-locomotion-framework: Discuss the usage of proper types to handle heterogeneous elements through the RobotInterface

[prashanthr05]

In general, we adhere to the usage of Eigen types in the public API provided by the libraries in this repository.
However, when it comes to dealing with heterogeneous types of data, which may further be of fixed size or variable size, single-dimensional or multi-dimensional, be classes or structs, it becomes unclear how these types must be handled.

We saw this ambiguity arise with the introduction of the ISensorBridge interface (https://github.com/dic-iit/bipedal-locomotion-framework/pull/87), where we had to deal with,

• fixed-size quantities (eg. a three-axis gyroscope measurement is of known size 3)
• pre-determined variable size quantities (eg. the number of joints to be read from the robot can be variable but is fixed during initialization)
• variable size quantities (variable set of points)
• matrices, structs, etc (images, point clouds)

Bipedal Locomotion Framework also provides the GenericContainer class which was meant to have a unique way to deal with multiple vector implementations that could have also been resized. This was coming in handy when dealing with parameters. Depending on the implementation and on the needs, we may have needed a Yarp::Sig::Vector, or iDynTree::VectorDynSize for example. This was before moving to Eigen in the main API. So, it would be suggestive to use GenericContainer::Vector in case one needs to deal with different vector implementation at the same time, especially in case you need to resize them (something that is not possible with an Eigen::map).

However, GenericContainer::Vector cannot be used for datatypes, for instance, images, that are usually stored as matrices.

Additionally, we must choose the types efficiently so that we do not introduce any additional overheads while converting from one type to another type, and passing it on to the user (who might/might not have to again convert to the desired type). This issue mainly comes into the picture in a SensorBridge scenario, where proper/inexpensive conversions to cv::Mat or pcl::PointCloud must be done.

Given the above pointers, in particular, it is not clear:

• when to use Eigen vectors vs GenericContainer vs templates https://github.com/dic-iit/bipedal-locomotion-framework/issues/95
• how to deal with matrices https://github.com/dic-iit/bipedal-locomotion-framework/issues/96
• how to deal with particular input classes, like point clouds

cc @S-Dafarra @GiulioRomualdi @traversaro @diegoferigo

[S-Dafarra]

Thanks a lot @prashanthr05 for opening this. Since the topic is indeed very big, I started opening a separate issue for the first point: https://github.com/dic-iit/bipedal-locomotion-framework/issues/95

[S-Dafarra]

Probably we can simply turn this into an epic and open an issue for each subpoint.

[S-Dafarra]

Eventually, we can use this specific issue to discuss mainly about the third point. @prashanthr05 did you have something in mind to deal with

how to deal with particular input classes, like point clouds

?

[prashanthr05]

Sorry that I come back to this a bit late.

@prashanthr05 did you have something in mind to deal with

how to deal with particular input classes, like point clouds

?

At this point, no. We must take a use-case scenario like a YARP implementation (or similar, what is practiced in the sensing and perception group) and try to understand how it sits within our SensorBridge interface design.

[prashanthr05]

In order to properly handle images and make use of the OpenCV library, I refactored the Camera interface from the ISensorBridge into a new class ICameraBridge which can be compiled only if OpenCV is found as a dependency of BLF. This allows us to bypass https://github.com/dic-iit/bipedal-locomotion-framework/issues/110.

The Yarp Implementation of the Camera bridge currently implements the bridge methods for IRGBDSensor and IFrameGrabber interfaces allowing us to read RGB and depth images. It is done so in the YarpCameraBridge class. (In the process of refactoring, I fixed some bugs which were undetected earlier in the Camera interface and the MAS interface).

It must be noted that in order to keep YarpCameraBridge storage light, it does not inherit from Advanceable and the user can initialize the camera bridge, set polydrivers list and is allowed to call the getters without running advance every time. (Mimics the Advanceable structure without the advance() method). This approach allows us to maintain only minimal buffers for transferring the image from the interface to the user. This is in line with the issue https://github.com/dic-iit/bipedal-locomotion-framework/issues/109.

I have tested the YarpCameraBridge with a Realsense D435i camera used with YARP realsense2withIMU driver.

The color of the RGB image seems to be incorrect. I remember that the Realsense device itself does some warm-up phase, until which the color remains as seen in the image, however it auto-corrects itself. This does not seem to be happening in this case, which needs to be verified. However, the interface streaming seems to be working as expected.

There was a key issue relating to yarp FlexImage type (RGB output of RGBD sensor) which has been fixed in the master branch of YARP and not available in yarp v3.4 (See https://github.com/robotology/yarp/pull/2368 and https://github.com/robotology/yarp/issues/2349) but the YARP relasense2 driver is not available in the master, so for proper testing, one needs to align the changes.

[S-Dafarra]

All the points above seem pretty reasonable @prashanthr05 :rocket:

[traversaro]

🚀🚀🚀

[prashanthr05]

The color of the RGB image seems to be incorrect.

Looks like realsense gives BGR image. I noticed that in https://github.com/IntelRealSense/librealsense/blob/34fc284d537a4b873a37b737a61f1f1da92dbb60/wrappers/pcl/pcl-color/rs-pcl-color.cpp#L133-L136