Avoid to call wait_for_frames in the get functions for the RealSense class

[GiulioRomualdi]

I noticed that all the get functions in the RealSense class call wait_for_frames for instance

https://github.com/ami-iit/bipedal-locomotion-framework/blob/bb20f7e4a267a0eb5c377f43a3e7c278ac48ce4b/src/Perception/src/RealSense.cpp#L245

As mentioned in the documentation the wait_for_frames blocks the application. This means that in the realsense-test the frames of depth, rgb and infrared are unsynchronized (Those frames are used only for displaying images).

https://github.com/ami-iit/bipedal-locomotion-framework/blob/a0b753e21d6253136202f411cbeb88d819f073cb/utilities/realsense-test/src/Module.cpp#L93-L118

We should restructure the RealSense class such that we have an advance function that can be used to collect the feedback. RealSense can inherit from System::Source.

Moreover, I also noticed that RealSense::getColorImage() and RealSense::getDepthImage() store the data in a cv::mat by calling the following constructor.

https://github.com/ami-iit/bipedal-locomotion-framework/blob/bb20f7e4a267a0eb5c377f43a3e7c278ac48ce4b/src/Perception/src/RealSense.cpp#L251-L254

https://github.com/ami-iit/bipedal-locomotion-framework/blob/bb20f7e4a267a0eb5c377f43a3e7c278ac48ce4b/src/Perception/src/RealSense.cpp#L290-L294

However as written here, this method does not perform the memory allocation so if m_pimpl->depthFrame or m_pimpl->colorFrame got destroyed the data is not valid anymore. Here it is important to notice that rs2::frame is a smart reference. See here so it does not hold the memory. This means that when the copy assignment operator is called in

https://github.com/ami-iit/bipedal-locomotion-framework/blob/bb20f7e4a267a0eb5c377f43a3e7c278ac48ce4b/src/Perception/src/RealSense.cpp#L400

the frame is not actually copied so in conclusion given the current implementation of RealSense class the following code does the following

cv::Mat bgr, depth;
rsDev->getColorImage("D435i", bgr);
rsDev->getColorizedDepthImage("D435i", depth);

1. rsDev->getColorImage("D435i", bgr) call wait_for_frames and blocks the application till a new frame is available
2. after rsDev->getColorImage("D435i", bgr); bgr is available and it points to the memory stored by the real sense sdk.
3. Once rsDev->getColorizedDepthImage("D435i", depth); is called wait_for_frames is called again. This means that the memory pointed by bgr is not valid anymore.

Associated issue: https://github.com/IntelRealSense/librealsense/issues/3287

cc @traversaro @prashanthr05

[GiulioRomualdi]

cc @xenvre

[prashanthr05]

@GiulioRomualdi Thanks for the insights. The problem of memory leak should have been caught earlier if this class had a test with Valgrind or leak/memory sanitizers. Instead, I chose to test it using the YARP Module, where things seemed fine when they actually are not. (My bad!)

[GiulioRomualdi]

Don't worry @prashanthr05! It's actually great to have this implementation here in the repo ;)