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++;
}
}
TEST(ConcurrentQueue, ParallelPushPopAscIntegerAndCalculateTotalSum) {
tlx::ThreadPool pool(8);
ConcurrentQueue<size_t, std::allocator<size_t> > queue;
std::atomic<size_t> count(0);
std::atomic<size_t> total_sum(0);
static constexpr size_t num_threads = 4;
static constexpr size_t num_pushes = 10000;
// have threads push items
for (size_t i = 0; i != num_threads; ++i) {
pool.enqueue([&queue]() {
for (size_t i = 0; i != num_pushes; ++i) {
queue.push(i);
}
});
}
// have threads try to pop items.
for (size_t i = 0; i != num_threads; ++i) {
pool.enqueue([&]() {
while (count != num_threads * num_pushes) {
size_t item;
while (queue.try_pop(item)) {
total_sum += item;
++count;
}
}
});
}
pool.loop_until_empty();
ASSERT_TRUE(queue.empty());
ASSERT_EQ(count, num_threads * num_pushes);
// check total sum, no item gets lost?
ASSERT_EQ(total_sum, num_threads * num_pushes * (num_pushes - 1) / 2);
}