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

bipedal-locomotion-framework/src/Perception/src/RealSense.cpp

Line 245 in bb20f7e

m_pimpl->frames = m_pimpl->pipe.wait_for_frames();

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).

bipedal-locomotion-framework/utilities/realsense-test/src/Module.cpp

Lines 93 to 118 in a0b753e

	if (rsDev->getColorImage("D435i", bgr))
	{
	cv::namedWindow(colorImgName);
	cv::imshow(colorImgName, bgr);
	cv::waitKey(1);
	}
	cv::Mat depth;
	if (rsDev->getColorizedDepthImage("D435i", depth))
	{
	cv::namedWindow(depthImgName);
	cv::imshow(depthImgName, depth);
	cv::waitKey(1);
	}
	cv::Mat ir;
	if (rsDev->getInfraredImage("D435i", ir))
	{
	cv::namedWindow(irImgName);
	cv::imshow(irImgName, ir);
	cv::waitKey(1);
	}
	}
	if (rsDev->getPointCloud("D435i", pcRaw))

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.

bipedal-locomotion-framework/src/Perception/src/RealSense.cpp

Lines 251 to 254 in bb20f7e

	colorImg = cv::Mat(cv::Size(genericWidth, genericHeight),
	CV_8UC3,
	(void*)m_pimpl->colorFrame.get_data(),
	cv::Mat::AUTO_STEP);

bipedal-locomotion-framework/src/Perception/src/RealSense.cpp

Lines 290 to 294 in bb20f7e

	depthImg = cv::Mat(cv::Size(genericWidth, genericHeight),
	CV_8UC1,
	(void*)m_pimpl->depthFrame.get_data(),
	cv::Mat::AUTO_STEP);

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

bipedal-locomotion-framework/src/Perception/src/RealSense.cpp

Line 400 in bb20f7e

m_pimpl->depthFrame = m_pimpl->frames.get_depth_frame();

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: IntelRealSense/librealsense#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 ;)