void queueConsumer( )
{
cv::Mat im, depth;
ros::Rate rate(30);
int nFrame = 0;
while( ros::ok() )
{
//TODO :
// This synchronization is not perfect. Should ideally also push the timestamps to the queue and do
// it based on time stamps of the pushed images
cout << "size : "<< colorQueue.getSize() << " " << depthQueue.getSize() << endl;
if( colorQueue.getSize() < 1 || depthQueue.getSize() < 1 )
{
ros::spinOnce();
rate.sleep();
continue;
}
bool pop_im_flag = colorQueue.try_pop(im);
bool pop_depth_flag = depthQueue.try_pop(depth);
cout << "im.type() : " << type2str( im.type() ) << endl;
cout << "depth.type() : " << type2str( depth.type() ) << endl;
double minVal, maxVal;
cv::minMaxLoc( depth, &minVal, &maxVal );
cout << "depth min : "<< minVal << endl;
cout << "depth max : "<< maxVal << endl;
// save to file
write_to_opencv_file( nFrame, im, depth );
cv::imshow( "im", im );
cv::Mat falseColorsMap;
applyColorMap(depth, falseColorsMap, cv::COLORMAP_AUTUMN);
cv::imshow( "depth", depth );
cv::waitKey(1);
ros::spinOnce();
rate.sleep();
nFrame++;
}
}
