Visual Servoing Platform  version 3.0.1
grabV4l2MultiCpp11Thread.cpp
1 #include <visp3/core/vpConfig.h>
2 
10 #if defined(VISP_HAVE_CPP11_COMPATIBILITY) && defined(VISP_HAVE_V4L2) && ( defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) )
11 
12 #include <iostream>
13 #include <limits>
14 #include <queue>
15 #include <mutex>
16 #include <thread>
17 #include <condition_variable>
18 
19 #include <visp3/sensor/vpV4l2Grabber.h>
20 #include <visp3/core/vpIoTools.h>
21 #include <visp3/core/vpImageFilter.h>
22 #include <visp3/core/vpDisplay.h>
23 #include <visp3/core/vpTime.h>
24 #include <visp3/gui/vpDisplayX.h>
25 #include <visp3/gui/vpDisplayGTK.h>
26 #include <visp3/io/vpParseArgv.h>
27 #include <visp3/io/vpVideoWriter.h>
28 
29 
30 #define GETOPTARGS "d:oh"
31 
32 
33 namespace {
34 
35 void usage(const char *name, const char *badparam)
36 {
37  fprintf(stdout, "\n\
38 SYNOPSIS:\n\
39  %s [-d <device count>] [-o] [-h]\n\
40 \n\
41 DESCRIPTION:\n\
42  Capture multiple camera streams and save the stream without slowing down the acquisition.\n\
43  \n\
44 OPTIONS: \n\
45  -d <device count> \n\
46  Open the specified number of camera streams.\n\
47  \n\
48  -o \n\
49  Save each stream in a dedicated folder.\n\
50  \n\
51  -h \n\
52  Print the help.\n\n",
53  name);
54 
55  if (badparam)
56  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
57 }
58 
59 bool getOptions(int argc, char **argv,
60  unsigned int &deviceCount,
61  bool &saveVideo)
62 {
63  const char *optarg;
64  const char **argv1=(const char**)argv;
65  int c;
66  while ((c = vpParseArgv::parse(argc, argv1, GETOPTARGS, &optarg)) > 1) {
67 
68  switch (c) {
69  case 'd': deviceCount = (unsigned int) atoi(optarg); break;
70  case 'o': saveVideo = true; break;
71  case 'h': usage(argv[0], NULL); return false; break;
72 
73  default:
74  usage(argv[0], optarg);
75  return false; break;
76  }
77  }
78 
79  if ((c == 1) || (c == -1)) {
80  // standalone param or error
81  usage(argv[0], NULL);
82  std::cerr << "ERROR: " << std::endl;
83  std::cerr << " Bad argument " << optarg << std::endl << std::endl;
84  return false;
85  }
86 
87  return true;
88 }
89 
90 //Code adapted from the original author Dan MaĊĦek to be compatible with ViSP image
91 class FrameQueue {
92 
93 public:
94  struct cancelled {
95  };
96 
97  FrameQueue() : m_cancelled(false), m_cond(), m_queueColor(), m_maxQueueSize(std::numeric_limits<size_t>::max()), m_mutex() {
98  }
99 
100  void cancel() {
101  std::lock_guard<std::mutex> lock(m_mutex);
102  m_cancelled = true;
103  m_cond.notify_all();
104  }
105 
106  //Push the image to save in the queue (FIFO)
107  void push(const vpImage<vpRGBa> &image) {
108  std::lock_guard<std::mutex> lock(m_mutex);
109 
110  m_queueColor.push(image);
111 
112  //Pop extra images in the queue
113  while(m_queueColor.size() > m_maxQueueSize) {
114  m_queueColor.pop();
115  }
116 
117  m_cond.notify_one();
118  }
119 
120  //Pop the image to save from the queue (FIFO)
121  vpImage<vpRGBa> pop() {
122  std::unique_lock<std::mutex> lock(m_mutex);
123 
124  while (m_queueColor.empty()) {
125  if (m_cancelled) {
126  throw cancelled();
127  }
128 
129  m_cond.wait(lock);
130 
131  if (m_cancelled) {
132  throw cancelled();
133  }
134  }
135 
136  vpImage<vpRGBa> image(m_queueColor.front());
137  m_queueColor.pop();
138 
139  return image;
140  }
141 
142  void setMaxQueueSize(const size_t max_queue_size) {
143  m_maxQueueSize = max_queue_size;
144  }
145 
146 private:
147  bool m_cancelled;
148  std::condition_variable m_cond;
149  std::queue<vpImage<vpRGBa> > m_queueColor;
150  size_t m_maxQueueSize;
151  std::mutex m_mutex;
152 };
153 
154 
155 class StorageWorker {
156 
157 public:
158  StorageWorker(FrameQueue &queue, const std::string &filename,
159  const unsigned int width, const unsigned int height) :
160  m_queue(queue), m_filename(filename), m_width(width), m_height(height) {
161  }
162 
163  //Thread main loop
164  void run() {
165  vpImage<vpRGBa> O_color(m_height, m_width);
166 
167  vpVideoWriter writer;
168  if(!m_filename.empty()) {
169  writer.setFileName(m_filename);
170  writer.open(O_color);
171  }
172 
173  try {
174  for (;;) {
175  vpImage<vpRGBa> image(m_queue.pop());
176 
177  if(!m_filename.empty()) {
178  writer.saveFrame(image);
179  }
180  }
181  } catch (FrameQueue::cancelled &) {
182  }
183  }
184 
185 private:
186  FrameQueue &m_queue;
187  std::string m_filename;
188  unsigned int m_width;
189  unsigned int m_height;
190 };
191 
192 
193 class ShareImage {
194 
195 private:
196  bool m_cancelled;
197  std::condition_variable m_cond;
198  std::mutex m_mutex;
199  unsigned char *m_pImgData;
200  unsigned int m_totalSize;
201 
202 public:
203  struct cancelled {
204  };
205 
206  ShareImage() : m_cancelled(false), m_cond(), m_mutex(), m_pImgData(NULL), m_totalSize(0) {
207  }
208 
209  virtual ~ShareImage() {
210  if(m_pImgData != NULL) {
211  delete []m_pImgData;
212  }
213  }
214 
215  void cancel() {
216  std::lock_guard<std::mutex> lock(m_mutex);
217  m_cancelled = true;
218  m_cond.notify_all();
219  }
220 
221  //Get the image to display
222  void getImage(unsigned char * const imageData, const unsigned int totalSize) {
223  std::unique_lock<std::mutex> lock(m_mutex);
224 
225  if (m_cancelled) {
226  throw cancelled();
227  }
228 
229  m_cond.wait(lock);
230 
231  if (m_cancelled) {
232  throw cancelled();
233  }
234 
235  //Copy to imageData
236  if(totalSize <= m_totalSize) {
237  memcpy(imageData, m_pImgData, totalSize*sizeof(unsigned char));
238  } else {
239  std::cerr << "totalSize <= m_totalSize !" << std::endl;
240  }
241  }
242 
243  bool isCancelled() {
244  std::lock_guard<std::mutex> lock(m_mutex);
245  return m_cancelled;
246  }
247 
248  //Set the image to display
249  void setImage(const unsigned char * const imageData, const unsigned int totalSize) {
250  std::lock_guard<std::mutex> lock(m_mutex);
251 
252  if(m_pImgData == NULL || m_totalSize != totalSize) {
253  m_totalSize = totalSize;
254 
255  if(m_pImgData != NULL) {
256  delete []m_pImgData;
257  }
258 
259  m_pImgData = new unsigned char[m_totalSize];
260  }
261 
262  //Copy from imageData
263  memcpy(m_pImgData, imageData, m_totalSize*sizeof(unsigned char));
264 
265  m_cond.notify_one();
266  }
267 };
268 
269 
270 void capture(vpV4l2Grabber * const pGrabber, ShareImage &share_image) {
271  vpImage<vpRGBa> local_img;
272 
273  //Open the camera stream
274  pGrabber->open(local_img);
275 
276  while(true) {
277  if(share_image.isCancelled()) {
278  break;
279  }
280 
281  pGrabber->acquire(local_img);
282 
283  //Update share_image
284  share_image.setImage((unsigned char *) local_img.bitmap, local_img.getSize()*4);
285  }
286 }
287 
288 void display(const unsigned int width, const unsigned int height, const int win_x, const int win_y,
289  const unsigned int deviceId, ShareImage &share_image, FrameQueue &queue, const bool save) {
290  vpImage<vpRGBa> local_img(height, width);
291 
292 #if defined VISP_HAVE_X11
293  vpDisplayX display;
294 #elif defined VISP_HAVE_GTK
295  vpDisplayGTK display;
296 #endif
297 
298  //Init Display
299  std::stringstream ss;
300  ss << "Camera stream " << deviceId;
301  display.init(local_img, win_x, win_y, ss.str());
302 
303  try {
305 
306  vpImage<unsigned char> I_red(height, width), I_green(height, width), I_blue(height, width), I_alpha(height, width);;
307  vpImage<unsigned char> I_red_gaussian(height, width), I_green_gaussian(height, width), I_blue_gaussian(height, width);
308  vpImage<double> I_red_gaussian_double, I_green_gaussian_double, I_blue_gaussian_double;
309 
310  bool exit = false, gaussian_blur = false;
311  while(!exit) {
312  double t = vpTime::measureTimeMs();
313 
314  //Get image
315  share_image.getImage((unsigned char *) local_img.bitmap, local_img.getSize()*4);
316 
317  //Apply gaussian blur to simulate a computation on the image
318  if(gaussian_blur) {
319  //Split channels
320  vpImageConvert::split(local_img, &I_red, &I_green, &I_blue, &I_alpha);
321  vpImageConvert::convert(I_red, I_red_gaussian_double);
322  vpImageConvert::convert(I_green, I_green_gaussian_double);
323  vpImageConvert::convert(I_blue, I_blue_gaussian_double);
324 
325  vpImageFilter::gaussianBlur(I_red_gaussian_double, I_red_gaussian_double, 21);
326  vpImageFilter::gaussianBlur(I_green_gaussian_double, I_green_gaussian_double, 21);
327  vpImageFilter::gaussianBlur(I_blue_gaussian_double, I_blue_gaussian_double, 21);
328 
329  vpImageConvert::convert(I_red_gaussian_double, I_red_gaussian);
330  vpImageConvert::convert(I_green_gaussian_double, I_green_gaussian);
331  vpImageConvert::convert(I_blue_gaussian_double, I_blue_gaussian);
332 
333  vpImageConvert::merge(&I_red_gaussian, &I_green_gaussian, &I_blue_gaussian, NULL, local_img);
334  }
335 
336  t = vpTime::measureTimeMs() - t;
337  std::stringstream ss;
338  ss << "Time: " << t << " ms";
339 
340  vpDisplay::display(local_img);
341 
342  vpDisplay::displayText(local_img, 20, 20, ss.str(), vpColor::red);
343  vpDisplay::displayText(local_img, 40, 20, "Left click to quit, right click for Gaussian blur.", vpColor::red);
344 
345  vpDisplay::flush(local_img);
346 
347  if(save) {
348  queue.push(local_img);
349  }
350 
351  if(vpDisplay::getClick(local_img, button, false)) {
352  switch(button) {
354  gaussian_blur = !gaussian_blur;
355  break;
356 
357  default:
358  exit = true;
359  break;
360  }
361  }
362  }
363  } catch(ShareImage::cancelled &) {
364  std::cout << "Cancelled!" << std::endl;
365  }
366 
367  share_image.cancel();
368 }
369 
370 } //Namespace
371 
372 
373 int main(int argc, char *argv[])
374 {
375  unsigned int deviceCount = 1;
376  unsigned int cameraScale = 1; //640x480
377  bool saveVideo = false;
378 
379  //Read the command line options
380  if (!getOptions(argc, argv, deviceCount, saveVideo)) {
381  return (-1);
382  }
383 
384  std::vector<vpV4l2Grabber *> grabbers;
385 
386  const unsigned int offsetX = 100, offsetY = 100;
387  for(unsigned int devicedId = 0; devicedId < deviceCount; devicedId++) {
388  try {
389  vpV4l2Grabber *pGrabber = new vpV4l2Grabber;
390  std::stringstream ss;
391  ss << "/dev/video" << devicedId;
392  pGrabber->setDevice(ss.str());
393  pGrabber->setScale(cameraScale);
394 
395  grabbers.push_back(pGrabber);
396  } catch(vpException &e) {
397  std::cerr << "Exception: " << e.what() << std::endl;
398  }
399  }
400 
401  std::cout << "Grabbers: " << grabbers.size() << std::endl;
402 
403  std::vector<ShareImage> share_images(grabbers.size());
404  std::vector<std::thread> capture_threads;
405  std::vector<std::thread> display_threads;
406 
407  //Synchronized queues for each camera stream
408  std::vector<FrameQueue> save_queues(grabbers.size());
409  std::vector<StorageWorker> storages;
410  std::vector<std::thread> storage_threads;
411 
412  std::string parent_directory = vpTime::getDateTime("%Y-%m-%d_%H.%M.%S");
413  if(saveVideo) {
414  std::cout << "Create parent_directory: " << parent_directory << std::endl;
415  vpIoTools::makeDirectory(parent_directory);
416  }
417 
418  for(size_t deviceId = 0; deviceId < grabbers.size(); deviceId++) {
419  //Start the capture thread for the current camera stream
420  capture_threads.emplace_back( capture, grabbers[deviceId], std::ref(share_images[deviceId]) );
421  int win_x = deviceId * offsetX, win_y = deviceId * offsetY;
422 
423  //Start the display thread for the current camera stream
424  display_threads.emplace_back( display, grabbers[deviceId]->getWidth(), grabbers[deviceId]->getHeight(),
425  win_x, win_y, deviceId, std::ref(share_images[deviceId]),
426  std::ref(save_queues[deviceId]), saveVideo );
427 
428  if(saveVideo) {
429  std::stringstream ss;
430  ss << parent_directory << "/Camera_Stream" << deviceId;
431  std::cout << "Create directory: " << ss.str() << std::endl;
432  vpIoTools::makeDirectory(ss.str());
433  ss << "/%06d.png";
434  std::string filename = ss.str();
435 
436  storages.emplace_back( std::ref(save_queues[deviceId]), std::cref(filename),
437  grabbers[deviceId]->getWidth(), grabbers[deviceId]->getHeight() );
438  }
439  }
440 
441  if(saveVideo) {
442  for(auto& s : storages) {
443  //Start the storage thread for the current camera stream
444  storage_threads.emplace_back(&StorageWorker::run, &s);
445  }
446  }
447 
448  //Join all the worker threads, waiting for them to finish
449  for(auto& ct : capture_threads) {
450  ct.join();
451  }
452 
453  for (auto& dt : display_threads) {
454  dt.join();
455  }
456 
457  //Clean first the grabbers to avoid camera problems when cancelling the storage threads in the terminal
458  for(auto& g : grabbers) {
459  delete g;
460  }
461 
462  if(saveVideo) {
463  std::cout << "\nWaiting for finishing thread to write images..." << std::endl;
464  }
465 
466  // We're done reading, cancel all the queues
467  for (auto& qu : save_queues) {
468  qu.cancel();
469  }
470 
471  //Join all the worker threads, waiting for them to finish
472  for (auto& st : storage_threads) {
473  st.join();
474  }
475 
476  return 0;
477 }
478 #else
479 #include <iostream>
480 
481 int main()
482 {
483  std::cout << "Warning: This example need to be build with cxx11 compiler flags, v4l2 and x11 or gtk 3rd partiess. " << std::endl;
484  return 0;
485 }
486 #endif
487 
void acquire(vpImage< unsigned char > &I)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void open(vpImage< unsigned char > &I)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Type * bitmap
points toward the bitmap
Definition: vpImage.h:134
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:153
void setDevice(const std::string &devname)
error that can be emited by ViSP classes.
Definition: vpException.h:73
static void split(const vpImage< vpRGBa > &src, vpImage< unsigned char > *pR, vpImage< unsigned char > *pG, vpImage< unsigned char > *pB, vpImage< unsigned char > *pa=NULL)
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
Definition: vpTime.cpp:351
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:76
static const vpColor red
Definition: vpColor.h:163
static void makeDirectory(const char *dirname)
Definition: vpIoTools.cpp:427
static void display(const vpImage< unsigned char > &I)
static void gaussianBlur(const vpImage< unsigned char > &I, vpImage< double > &GI, unsigned int size=7, double sigma=0., bool normalize=true)
Class that enables to write easily a video file or a sequence of images.
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:138
void saveFrame(vpImage< vpRGBa > &I)
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
void open(vpImage< vpRGBa > &I)
const char * what() const
void setFileName(const char *filename)
static void merge(const vpImage< unsigned char > *R, const vpImage< unsigned char > *G, const vpImage< unsigned char > *B, const vpImage< unsigned char > *a, vpImage< vpRGBa > &RGBa)
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
unsigned int getSize() const
Definition: vpImage.h:212