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