Visual Servoing Platform  version 3.6.1 under development (2023-12-07)
vpV4l2Grabber.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See httpS://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Framegrabber based on Video4Linux2 driver.
33  *
34 *****************************************************************************/
35 
42 #include <visp3/core/vpConfig.h>
43 
44 #ifdef VISP_HAVE_V4L2
45 
46 #include <errno.h>
47 #include <fcntl.h>
48 #include <iostream>
49 #include <stdio.h>
50 #include <stdlib.h>
51 #include <sys/ioctl.h>
52 #include <sys/mman.h>
53 #include <sys/stat.h>
54 #include <sys/time.h>
55 #include <sys/types.h>
56 #include <unistd.h>
57 
58 #include <visp3/core/vpFrameGrabberException.h>
59 #include <visp3/sensor/vpV4l2Grabber.h>
60 //#include <visp3/io/vpImageIo.h>
61 #include <visp3/core/vpImageConvert.h>
62 #include <visp3/core/vpImageTools.h>
63 
64 const unsigned int vpV4l2Grabber::DEFAULT_INPUT = 2;
65 const unsigned int vpV4l2Grabber::DEFAULT_SCALE = 2;
66 const __u32 vpV4l2Grabber::MAX_INPUTS = 16;
67 const __u32 vpV4l2Grabber::MAX_NORM = 16;
68 const __u32 vpV4l2Grabber::MAX_FORMAT = 32;
69 const unsigned int vpV4l2Grabber::MAX_CTRL = 32;
70 const unsigned int vpV4l2Grabber::MAX_BUFFERS = 32;
71 const unsigned int vpV4l2Grabber::FRAME_SIZE = 288;
72 #define vpCLEAR(x) memset(&(x), 0, sizeof(x))
73 
147  : fd(-1), device(), cap(), streamparm(), inp(nullptr), std(nullptr), fmt(nullptr), ctl(nullptr), fmt_v4l2(), fmt_me(), reqbufs(),
148  buf_v4l2(nullptr), buf_me(nullptr), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0),
149  streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
150  m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
151 {
152  setDevice("/dev/video0");
153  setNBuffers(3);
158 
159  init = false;
160 }
161 
203  : fd(-1), device(), cap(), streamparm(), inp(nullptr), std(nullptr), fmt(nullptr), ctl(nullptr), fmt_v4l2(), fmt_me(), reqbufs(),
204  buf_v4l2(nullptr), buf_me(nullptr), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(verbose), m_nbuffers(3), field(0),
205  streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
206  m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
207 {
208  setDevice("/dev/video0");
209  setNBuffers(3);
214 
215  init = false;
216 }
217 
248 vpV4l2Grabber::vpV4l2Grabber(unsigned input, unsigned scale)
249  : fd(-1), device(), cap(), streamparm(), inp(nullptr), std(nullptr), fmt(nullptr), ctl(nullptr), fmt_v4l2(), fmt_me(), reqbufs(),
250  buf_v4l2(nullptr), buf_me(nullptr), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0),
251  streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
252  m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
253 {
254  setDevice("/dev/video0");
255  setNBuffers(3);
257  setInput(input);
258  setScale(scale);
259 
260  init = false;
261 }
262 
294 vpV4l2Grabber::vpV4l2Grabber(vpImage<unsigned char> &I, unsigned input, unsigned scale)
295  : fd(-1), device(), cap(), streamparm(), inp(nullptr), std(nullptr), fmt(nullptr), ctl(nullptr), fmt_v4l2(), fmt_me(), reqbufs(),
296  buf_v4l2(nullptr), buf_me(nullptr), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0),
297  streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
298  m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
299 {
300  setDevice("/dev/video0");
301  setNBuffers(3);
303  setInput(input);
304  setScale(scale);
305 
306  init = false;
307 
308  open(I);
309 }
310 
343 vpV4l2Grabber::vpV4l2Grabber(vpImage<vpRGBa> &I, unsigned input, unsigned scale)
344  : fd(-1), device(), cap(), streamparm(), inp(nullptr), std(nullptr), fmt(nullptr), ctl(nullptr), fmt_v4l2(), fmt_me(), reqbufs(),
345  buf_v4l2(nullptr), buf_me(nullptr), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0),
346  streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
347  m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
348 {
349  setDevice("/dev/video0");
350  setNBuffers(3);
352  setInput(input);
353  setScale(scale);
354 
355  init = false;
356 
357  open(I);
358 }
359 
366 
370 void vpV4l2Grabber::setInput(unsigned input) { this->m_input = input; }
371 
384 void vpV4l2Grabber::setScale(unsigned scale)
385 {
386  if ((scale < 1) || (scale > 16)) {
387  close();
388 
389  vpERROR_TRACE("Wrong scale %d, scale should be between 1 and 16", scale);
391  }
392 
393  setWidth(640 / scale);
394  setHeight(480 / scale);
395 }
396 
408 {
409  open();
410 
411  if (v4l2_ioctl(fd, VIDIOC_S_INPUT, &m_input) == -1) {
412  std::cout << "Warning: cannot set input channel to " << m_input << std::endl;
413  }
414 
415  vpV4l2PixelFormatType req_pixelformat = getPixelFormat();
416 
417  try {
418  setFormat();
419 
420  startStreaming();
421  }
422  catch (...) {
423  if (m_verbose) {
424  std::cout << "Requested pixel format [" << req_pixelformat << "] not compatible with camera" << std::endl;
425  std::cout << "Try to found a compatible pixel format..." << std::endl;
426  }
427 
428  // try to fing a compatible format
429  for (int format = 0; format < (int)V4L2_MAX_FORMAT; format++) {
430  if (format == req_pixelformat) {
431  continue;
432  }
433  try {
435  setFormat();
436  startStreaming();
437  if (m_verbose)
438  std::cout << "This format [" << m_pixelformat << "] is compatible with camera" << std::endl;
439 
440  break;
441  }
442  catch (...) {
443  if (m_verbose)
444  std::cout << "This format [" << m_pixelformat << "] is not compatible with camera" << std::endl;
445  if (format == (int)V4L2_MAX_FORMAT) {
446  std::cout << "No pixel format compatible with the camera was found" << std::endl;
447  close();
448 
450  "No pixel format compatible with the camera was found"));
451  }
452  }
453  }
454  }
455 
456  I.resize(height, width);
457 
458  init = true;
459 }
460 
471 {
472  open();
473 
474  if (v4l2_ioctl(fd, VIDIOC_S_INPUT, &m_input) == -1) {
475  std::cout << "Warning: cannot set input channel to " << m_input << std::endl;
476  }
477 
478  vpV4l2PixelFormatType req_pixelformat = getPixelFormat();
479 
480  try {
481  setFormat();
482 
483  startStreaming();
484  }
485  catch (...) {
486  if (m_verbose) {
487  std::cout << "Requested pixel format [" << m_pixelformat << "] not compatible with camera" << std::endl;
488  std::cout << "Try to found a compatible pixel format..." << std::endl;
489  }
490 
491  // try to fing a compatible format
492  for (int format = 0; format < (int)V4L2_MAX_FORMAT; format++) {
493  if (format == req_pixelformat) {
494  continue;
495  }
496  try {
498  setFormat();
499  startStreaming();
500  if (m_verbose)
501  std::cout << "This format [" << m_pixelformat << "] is compatible with camera" << std::endl;
502 
503  break;
504  }
505  catch (...) {
506  if (m_verbose)
507  std::cout << "This format [" << m_pixelformat << "] is not compatible with camera" << std::endl;
508  }
509  }
510  }
511 
512  I.resize(height, width);
513 
514  init = true;
515 }
516 
528 {
529  struct timeval timestamp;
530  vpRect roi;
531 
532  acquire(I, timestamp, roi);
533 }
534 
547 {
548  struct timeval timestamp;
549 
550  acquire(I, timestamp, roi);
551 }
552 
572 void vpV4l2Grabber::acquire(vpImage<unsigned char> &I, struct timeval &timestamp, const vpRect &roi)
573 {
574  if (init == false) {
575  open(I);
576  }
577 
578  if (init == false) {
579  close();
580 
581  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "V4l2 frame grabber not initialized"));
582  }
583 
584  unsigned char *bitmap;
585  bitmap = waiton(index_buffer, timestamp);
586 
587  if (roi == vpRect())
588  I.resize(height, width);
589  else
590  I.resize((unsigned int)roi.getHeight(), (unsigned int)roi.getWidth());
591  switch (m_pixelformat) {
592  case V4L2_GREY_FORMAT:
593  if (roi == vpRect())
594  memcpy(I.bitmap, bitmap, height * width * sizeof(unsigned char));
595  else
596  vpImageTools::crop(bitmap, width, height, roi, I);
597  break;
598  case V4L2_RGB24_FORMAT: // tested
599  if (roi == vpRect())
600  vpImageConvert::RGBToGrey((unsigned char *)bitmap, I.bitmap, width * height);
601  else {
603  vpImageConvert::RGBToGrey((unsigned char *)bitmap, tmp.bitmap, width * height);
604  vpImageTools::crop(tmp, roi, I);
605  }
606  break;
607  case V4L2_RGB32_FORMAT:
608  if (roi == vpRect())
609  vpImageConvert::RGBaToGrey((unsigned char *)bitmap, I.bitmap, width * height);
610  else {
612  vpImageConvert::RGBaToGrey((unsigned char *)bitmap, tmp.bitmap, width * height);
613  vpImageTools::crop(tmp, roi, I);
614  }
615 
616  break;
617  case V4L2_BGR24_FORMAT: // tested
618  if (roi == vpRect())
619  vpImageConvert::BGRToGrey((unsigned char *)bitmap, I.bitmap, width, height, false);
620  else {
622  vpImageConvert::BGRToGrey((unsigned char *)bitmap, tmp.bitmap, width, height, false);
623  vpImageTools::crop(tmp, roi, I);
624  }
625  break;
626  case V4L2_YUYV_FORMAT: // tested
627  if (roi == vpRect())
628  vpImageConvert::YUYVToGrey((unsigned char *)bitmap, I.bitmap, width * height);
629  else {
631  vpImageConvert::YUYVToGrey((unsigned char *)bitmap, tmp.bitmap, width * height);
632  vpImageTools::crop(tmp, roi, I);
633  }
634  break;
635  default:
636  std::cout << "V4L2 conversion not handled" << std::endl;
637  break;
638  }
639 
640  queueAll();
641 }
642 
654 {
655  struct timeval timestamp;
656  vpRect roi;
657 
658  acquire(I, timestamp, roi);
659 }
660 
673 {
674  struct timeval timestamp;
675 
676  acquire(I, timestamp, roi);
677 }
678 
698 void vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, struct timeval &timestamp, const vpRect &roi)
699 {
700  if (init == false) {
701  open(I);
702  }
703 
704  if (init == false) {
705  close();
706 
707  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "V4l2 frame grabber not initialized"));
708  }
709 
710  unsigned char *bitmap;
711  bitmap = waiton(index_buffer, timestamp);
712 
713  if (roi == vpRect())
714  I.resize(height, width);
715  else
716  I.resize((unsigned int)roi.getHeight(), (unsigned int)roi.getWidth());
717 
718  // The framegrabber acquire aRGB format. We just shift the data from 1 byte
719  // all the data and initialize the last byte
720 
721  switch (m_pixelformat) {
722  case V4L2_GREY_FORMAT:
723  if (roi == vpRect())
724  vpImageConvert::GreyToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width * height);
725  else
726  vpImageTools::crop(bitmap, width, height, roi, I);
727  break;
728  case V4L2_RGB24_FORMAT: // tested
729  if (roi == vpRect())
730  vpImageConvert::RGBToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width * height);
731  else {
733  vpImageConvert::RGBToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width * height);
734  vpImageTools::crop(tmp, roi, I);
735  }
736  break;
737  case V4L2_RGB32_FORMAT:
738  if (roi == vpRect()) {
739  // The framegrabber acquire aRGB format. We just shift the data
740  // from 1 byte all the data and initialize the last byte
741  memcpy(static_cast<void *>(I.bitmap), static_cast<void *>(bitmap + 1), height * width * sizeof(vpRGBa) - 1);
742  I[height - 1][width - 1].A = 0;
743  }
744  else {
745  for (unsigned int i = 0; i < I.getHeight(); i++) {
746  memcpy(static_cast<void *>(I.bitmap),
747  static_cast<void *>(bitmap + 1 + (unsigned int)(roi.getTop() * width + roi.getLeft())),
748  I.getWidth() * sizeof(vpRGBa) - 1);
749  I[i][I.getWidth() - 1].A = 0;
750  }
751  }
752  break;
753  case V4L2_BGR24_FORMAT: // tested
754  if (roi == vpRect())
755  vpImageConvert::BGRToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width, height, false);
756  else {
758  vpImageConvert::BGRToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width, height, false);
759  vpImageTools::crop(tmp, roi, I);
760  }
761  break;
762  case V4L2_YUYV_FORMAT: // tested
763  if (roi == vpRect())
764  vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width, height);
765  else {
767  vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width, height);
768  vpImageTools::crop(tmp, roi, I);
769  }
770  break;
771  default:
772  std::cout << "V4l2 conversion not handled" << std::endl;
773  break;
774  }
775 
776  queueAll();
777 }
795 {
796  if (field == 2)
797  return 0; // top field
798  else if (field == 3)
799  return 1; // bottom field;
800  else {
801  close();
802 
803  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "V4l2 returns a bad frame field"));
804  return false;
805  }
806 }
822 {
823  this->m_framerate = framerate;
824 
825  if (framerate == vpV4l2Grabber::framerate_25fps)
826  setFrameFormat(V4L2_IMAGE_FORMAT);
827  else
828  setFrameFormat(V4L2_FRAME_FORMAT);
829 }
830 
841 
846 {
847  stopStreaming();
848  streaming = false;
849 
850  if (fd >= 0) {
851  // vpTRACE("v4l2_close()");
852  v4l2_close(fd);
853  fd = -1;
854  }
855 
856  if (inp != nullptr) {
857  delete [] inp;
858  inp = nullptr;
859  }
860  if (std != nullptr) {
861  delete [] std;
862  std = nullptr;
863  }
864  if (fmt != nullptr) {
865  delete [] fmt;
866  fmt = nullptr;
867  }
868  if (ctl != nullptr) {
869  delete [] ctl;
870  ctl = nullptr;
871  }
872  if (buf_v4l2 != nullptr) {
873  delete [] buf_v4l2;
874  buf_v4l2 = nullptr;
875  }
876  if (buf_me != nullptr) {
877  delete [] buf_me;
878  buf_me = nullptr;
879  }
880 }
881 
893 void vpV4l2Grabber::open()
894 {
895  /* Open Video Device */
896  struct stat st;
897 
898  if (-1 == stat(device.c_str(), &st)) {
899  fprintf(stderr, "Cannot identify '%s': %d, %s\n", device.c_str(), errno, strerror(errno));
900  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Cannot identify video device"));
901  }
902 
903  if (!S_ISCHR(st.st_mode)) {
904  fprintf(stderr, "%s is no device\n", device.c_str());
906  }
907  fd = v4l2_open(device.c_str(), O_RDWR | O_NONBLOCK, 0);
908  if (fd < 0) {
909  close();
910 
911  vpERROR_TRACE("No video device \"%s\"\n", device.c_str());
912  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Can't access to video device"));
913  }
914 
915  if (inp != nullptr) {
916  delete [] inp;
917  inp = nullptr;
918  }
919  if (std != nullptr) {
920  delete [] std;
921  std = nullptr;
922  }
923  if (fmt != nullptr) {
924  delete [] fmt;
925  fmt = nullptr;
926  }
927  if (ctl != nullptr) {
928  delete [] ctl;
929  ctl = nullptr;
930  }
931  if (buf_v4l2 != nullptr) {
932  delete [] buf_v4l2;
933  buf_v4l2 = nullptr;
934  }
935  if (buf_me != nullptr) {
936  delete [] buf_me;
937  buf_me = nullptr;
938  }
939 
940  inp = new struct v4l2_input[vpV4l2Grabber::MAX_INPUTS];
941  std = new struct v4l2_standard[vpV4l2Grabber::MAX_NORM];
942  fmt = new struct v4l2_fmtdesc[vpV4l2Grabber::MAX_FORMAT];
943  ctl = new struct v4l2_queryctrl[vpV4l2Grabber::MAX_CTRL * 2];
944  buf_v4l2 = new struct v4l2_buffer[vpV4l2Grabber::MAX_BUFFERS];
945  buf_me = new struct ng_video_buf[vpV4l2Grabber::MAX_BUFFERS];
946 
947  /* Querry Video Device Capabilities */
948  if (v4l2_ioctl(fd, VIDIOC_QUERYCAP, &cap) == -1) {
949  close();
950  fprintf(stderr, "%s is no V4L2 device\n", device.c_str());
951  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Is not a V4L2 device"));
952  }
953  if (m_verbose) {
954  fprintf(stdout,
955  "v4l2 info:\n"
956  " device: %s\n"
957  " %s %d.%d.%d / %s @ %s\n",
958  device.c_str(), cap.driver, (cap.version >> 16) & 0xff, (cap.version >> 8) & 0xff, cap.version & 0xff,
959  cap.card, cap.bus_info);
960  if (cap.capabilities & V4L2_CAP_VIDEO_OVERLAY)
961  fprintf(stdout, " Support overlay\n");
962  else
963  fprintf(stdout, " Does not support overlay\n");
964  if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)
965  fprintf(stdout, " Support capture\n");
966  else
967  fprintf(stdout, " Does not support capture\n");
968  if (cap.capabilities & V4L2_CAP_TUNER)
969  fprintf(stdout, " Support tuning\n");
970  else
971  fprintf(stdout, " Does not support tuning\n");
972  if (cap.capabilities & V4L2_CAP_STREAMING)
973  fprintf(stdout, " Support streaming capture.\n");
974  else
975  fprintf(stdout, " Does not support streaming capture\n");
976  if (cap.capabilities & V4L2_CAP_ASYNCIO)
977  fprintf(stdout, " Support asynchronous I/O methods\n");
978  else
979  fprintf(stdout, " Does not support asynchronous I/O methods\n");
980  if (cap.capabilities & V4L2_CAP_TIMEPERFRAME)
981  fprintf(stdout, " Support time per frame field\n");
982  else
983  fprintf(stdout, " Does not support time per frame field\n");
984  // Get framerate
985  streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
986  if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) != -1) {
987  fprintf(stdout, " Current acquisition framerate: %d fps\n", streamparm.parm.output.timeperframe.denominator);
988  }
989  }
990 
991  getCapabilities();
992 }
993 
1000 void vpV4l2Grabber::getCapabilities()
1001 {
1002  for (__u32 ninputs = 0; ninputs < MAX_INPUTS; ninputs++) {
1003  inp[ninputs].index = ninputs;
1004  if (v4l2_ioctl(fd, VIDIOC_ENUMINPUT, &inp[ninputs]))
1005  break;
1006  }
1007  for (__u32 nstds = 0; nstds < MAX_NORM; nstds++) {
1008  std[nstds].index = nstds;
1009  if (v4l2_ioctl(fd, VIDIOC_ENUMSTD, &std[nstds]))
1010  break;
1011  }
1012  for (__u32 nfmts = 0; nfmts < MAX_FORMAT; nfmts++) {
1013  fmt[nfmts].index = nfmts;
1014  fmt[nfmts].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1015  if (v4l2_ioctl(fd, VIDIOC_ENUM_FMT, &fmt[nfmts]))
1016  break;
1017  }
1018 
1019  streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1020  if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) == -1) {
1021  close();
1022 
1023  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't get video parameters"));
1024  }
1025 }
1026 
1040 void vpV4l2Grabber::setFormat()
1041 {
1042  fmt_me.width = width;
1043  fmt_me.height = height;
1044  // fmt_me.bytesperline = width; // bad (normally width * depth / 8), but
1045  // works
1046  // because initialized later by an ioctl call to VIDIOC_S_FMT
1047 
1048  switch (m_pixelformat) {
1049  case V4L2_GREY_FORMAT:
1050  fmt_me.pixelformat = V4L2_PIX_FMT_GREY;
1051  if (m_verbose)
1052  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_GREY)\n");
1053  break;
1054  case V4L2_RGB24_FORMAT:
1055  fmt_me.pixelformat = V4L2_PIX_FMT_RGB24;
1056  if (m_verbose)
1057  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_RGB24)\n");
1058  break;
1059  case V4L2_RGB32_FORMAT:
1060  fmt_me.pixelformat = V4L2_PIX_FMT_RGB32;
1061  if (m_verbose)
1062  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_RGB32)\n");
1063  break;
1064  case V4L2_BGR24_FORMAT:
1065  fmt_me.pixelformat = V4L2_PIX_FMT_BGR24;
1066  if (m_verbose)
1067  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_BGR24)\n");
1068  break;
1069  case V4L2_YUYV_FORMAT:
1070  fmt_me.pixelformat = V4L2_PIX_FMT_YUYV;
1071  if (m_verbose)
1072  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_YUYV)\n");
1073  break;
1074 
1075  default:
1076  close();
1077 
1078  throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Bad format, probably do to a wrong scale"));
1079  }
1080 
1081  /* Get Video Format */
1082  vpCLEAR(fmt_v4l2);
1083 
1084  fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1085 
1086  if (v4l2_ioctl(fd, VIDIOC_G_FMT, &fmt_v4l2) == -1) {
1087  close();
1088 
1089  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't get video format"));
1090  }
1091  fmt_v4l2.fmt.pix.pixelformat = fmt_me.pixelformat;
1092  fmt_v4l2.fmt.pix.width = fmt_me.width;
1093  fmt_v4l2.fmt.pix.height = fmt_me.height;
1094  // printf("1 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width,
1095  // fmt_v4l2.fmt.pix.height);
1096 
1097  switch (m_frameformat) {
1098  case V4L2_FRAME_FORMAT:
1099  fmt_v4l2.fmt.pix.field = V4L2_FIELD_ALTERNATE;
1100  if (m_verbose) {
1101  fprintf(stdout, "v4l2: new capture params (V4L2_FIELD_ALTERNATE)\n");
1102  }
1103  break;
1104  case V4L2_IMAGE_FORMAT:
1105  fmt_v4l2.fmt.pix.field = V4L2_FIELD_INTERLACED;
1106  if (m_verbose) {
1107  fprintf(stdout, "v4l2: new capture params (V4L2_FIELD_INTERLACED)\n");
1108  }
1109  break;
1110  default:
1111  close();
1112 
1113  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Unrecognized frame format"));
1114  }
1115 
1116  // height and width of the captured image or frame
1117  if (m_frameformat == V4L2_FRAME_FORMAT && height > FRAME_SIZE) {
1118  height = FRAME_SIZE;
1119  }
1120  // printf("2 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width,
1121  // fmt_v4l2.fmt.pix.height);
1122 
1123  if (v4l2_ioctl(fd, VIDIOC_S_FMT, &fmt_v4l2) == -1) {
1124  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't set video format"));
1125  }
1126 
1127  if (fmt_v4l2.fmt.pix.pixelformat != fmt_me.pixelformat) {
1129  }
1130 
1131  /* Buggy driver paranoia. */
1132  unsigned int min = fmt_v4l2.fmt.pix.width * 2;
1133  if (fmt_v4l2.fmt.pix.bytesperline < min)
1134  fmt_v4l2.fmt.pix.bytesperline = min;
1135  min = fmt_v4l2.fmt.pix.bytesperline * fmt_v4l2.fmt.pix.height;
1136  if (fmt_v4l2.fmt.pix.sizeimage < min)
1137  fmt_v4l2.fmt.pix.sizeimage = min;
1138 
1139  fmt_me.width = fmt_v4l2.fmt.pix.width;
1140  fmt_me.height = fmt_v4l2.fmt.pix.height;
1141  fmt_me.bytesperline = fmt_v4l2.fmt.pix.bytesperline;
1142 
1143  if (m_verbose) {
1144  fprintf(stdout,
1145  "v4l2: new capture params (%ux%u, %c%c%c%c, %d byte, %d bytes "
1146  "per line)\n",
1147  fmt_me.width, fmt_me.height, fmt_v4l2.fmt.pix.pixelformat & 0xff,
1148  (fmt_v4l2.fmt.pix.pixelformat >> 8) & 0xff, (fmt_v4l2.fmt.pix.pixelformat >> 16) & 0xff,
1149  (fmt_v4l2.fmt.pix.pixelformat >> 24) & 0xff, fmt_v4l2.fmt.pix.sizeimage, fmt_v4l2.fmt.pix.bytesperline);
1150  }
1151 }
1160 void vpV4l2Grabber::startStreaming()
1161 {
1162  if (streaming == true) { // Acquisition in process.
1163  stopStreaming();
1164  streaming = false;
1165  }
1166 
1167  /* setup buffers */
1168  memset(&(reqbufs), 0, sizeof(reqbufs));
1169  reqbufs.count = m_nbuffers;
1170  reqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1171  reqbufs.memory = V4L2_MEMORY_MMAP;
1172 
1173  if (v4l2_ioctl(fd, VIDIOC_REQBUFS, &reqbufs) == -1) {
1174  if (EINVAL == errno) {
1175  fprintf(stderr,
1176  "%s does not support "
1177  "memory mapping\n",
1178  device.c_str());
1179  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Does not support memory mapping"));
1180  }
1181  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't require video buffers"));
1182  }
1183 
1184  for (unsigned i = 0; i < reqbufs.count; i++) {
1185  // Clear the buffer
1186  memset(&(buf_v4l2[i]), 0, sizeof(buf_v4l2[i]));
1187  buf_v4l2[i].index = i;
1188  buf_v4l2[i].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1189  buf_v4l2[i].memory = V4L2_MEMORY_MMAP;
1190  buf_v4l2[i].length = 0;
1191  if (v4l2_ioctl(fd, VIDIOC_QUERYBUF, &buf_v4l2[i]) == -1) {
1192  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't query video buffers"));
1193  }
1194  memcpy(&buf_me[i].fmt, &fmt_me, sizeof(ng_video_fmt));
1195  buf_me[i].size = buf_me[i].fmt.bytesperline * buf_me[i].fmt.height;
1196 
1197  buf_me[i].data = (unsigned char *)v4l2_mmap(nullptr, buf_v4l2[i].length, PROT_READ | PROT_WRITE, MAP_SHARED, fd,
1198  (off_t)buf_v4l2[i].m.offset);
1199 
1200  if (buf_me[i].data == MAP_FAILED) {
1202  }
1203 
1204  buf_me[i].refcount = 0;
1205 
1206  if (m_verbose)
1207  printBufInfo(buf_v4l2[i]);
1208  }
1209 
1210  /* queue up all buffers */
1211  queueAll();
1212 
1213  /* Set video stream capture on */
1214  if (v4l2_ioctl(fd, VIDIOC_STREAMON, &fmt_v4l2.type) < 0) {
1215  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't start streaming"));
1216  }
1217 
1218  streaming = true;
1219 }
1220 
1227 void vpV4l2Grabber::stopStreaming()
1228 {
1229  // nothing to do if (fd < 0) or if (streaming == false)
1230  if ((fd >= 0) && (streaming == true)) {
1231 
1232  // vpTRACE(" Stop the streaming...");
1233  /* stop capture */
1234  fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1235  if (v4l2_ioctl(fd, VIDIOC_STREAMOFF, &fmt_v4l2.type)) {
1236  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't stop streaming"));
1237  }
1238  /* free buffers */
1239  for (unsigned int i = 0; i < reqbufs.count; i++) {
1240  if (m_verbose)
1241  printBufInfo(buf_v4l2[i]);
1242  // vpTRACE("v4l2_munmap()");
1243 
1244  if (-1 == v4l2_munmap(buf_me[i].data, buf_me[i].size)) {
1245  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't unmap memory"));
1246  }
1247  }
1248  queue = 0;
1249  waiton_cpt = 0;
1250  streaming = false;
1251  }
1252 }
1253 
1267 unsigned char *vpV4l2Grabber::waiton(__u32 &index, struct timeval &timestamp)
1268 {
1269  struct v4l2_buffer buf;
1270  struct timeval tv;
1271  fd_set rdset;
1272 
1273  /* wait for the next frame */
1274 again:
1275 
1276  tv.tv_sec = 30;
1277  tv.tv_usec = 0;
1278  FD_ZERO(&rdset);
1279  FD_SET(static_cast<unsigned int>(fd), &rdset);
1280  switch (select(fd + 1, &rdset, nullptr, nullptr, &tv)) {
1281  case -1:
1282  if (EINTR == errno)
1283  goto again;
1284  index = 0;
1285  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't access to the frame"));
1286  return nullptr;
1287  case 0:
1288  index = 0;
1289  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't access to the frame: timeout"));
1290  return nullptr;
1291  }
1292 
1293  /* get it */
1294  memset(&buf, 0, sizeof(buf));
1295  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1296  buf.memory = V4L2_MEMORY_MMAP;
1297  if (-1 == v4l2_ioctl(fd, VIDIOC_DQBUF, &buf)) {
1298  index = 0;
1299  switch (errno) {
1300  case EAGAIN:
1301  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: EAGAIN"));
1302  break;
1303  case EINVAL:
1304  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: EINVAL"));
1305  break;
1306  case ENOMEM:
1307  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: ENOMEM"));
1308  break;
1309  default:
1311  break;
1312  }
1313  return nullptr;
1314  }
1315 
1316  waiton_cpt++;
1317  buf_v4l2[buf.index] = buf;
1318 
1319  index = buf.index;
1320 
1321  field = buf_v4l2[index].field;
1322 
1323  timestamp = buf_v4l2[index].timestamp;
1324 
1325  // if(m_verbose)
1326  // {
1327  // vpERROR_TRACE("field: %d\n", buf_v4l2[index].field);
1328 
1329  // vpERROR_TRACE("data adress : 0x%p\n", buf_me[buf.index].data);
1330  // }
1331  return buf_me[buf.index].data;
1332 }
1333 
1339 int vpV4l2Grabber::queueBuffer()
1340 {
1341  unsigned int frame = queue % reqbufs.count;
1342  int rc;
1343 
1344  if (0 != buf_me[frame].refcount) {
1345  if (0 != queue - waiton_cpt)
1346  return -1;
1347  fprintf(stderr, "v4l2: waiting for a free buffer..............\n");
1348  // ng_waiton_video_buf(h->buf_me+frame);
1349  std::cout << "Normalement call ng_waiton_video_buf(buf_me+frame); --------\n";
1350  }
1351 
1352  // std::cout << "frame: " << frame << std::endl;
1353  rc = v4l2_ioctl(fd, VIDIOC_QBUF, &buf_v4l2[frame]);
1354  if (0 == rc)
1355  queue++;
1356  else {
1357  switch (errno) {
1358  case EAGAIN:
1359  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: EAGAIN"));
1360  break;
1361  case EINVAL:
1362  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: EINVAL"));
1363  break;
1364  case ENOMEM:
1365  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: ENOMEM"));
1366  break;
1367  default:
1369  break;
1370  }
1371  }
1372  return rc;
1373 }
1374 
1380 void vpV4l2Grabber::queueAll()
1381 {
1382  for (;;) {
1383  if (queue - waiton_cpt >= reqbufs.count) {
1384  return;
1385  }
1386  if (0 != queueBuffer()) {
1387  return;
1388  }
1389  }
1390 }
1391 
1397 void vpV4l2Grabber::printBufInfo(struct v4l2_buffer buf)
1398 {
1399  std::string type;
1400 
1401  switch (buf.type) {
1402  case V4L2_BUF_TYPE_VIDEO_CAPTURE:
1403  type = "video-cap";
1404  break;
1405  case V4L2_BUF_TYPE_VIDEO_OVERLAY:
1406  type = "video-over";
1407  break;
1408  case V4L2_BUF_TYPE_VIDEO_OUTPUT:
1409  type = "video-out";
1410  break;
1411  case V4L2_BUF_TYPE_VBI_CAPTURE:
1412  type = "vbi-cap";
1413  break;
1414  case V4L2_BUF_TYPE_VBI_OUTPUT:
1415  type = "vbi-out";
1416  break;
1417  default:
1418  type = "unknown";
1419  break;
1420  }
1421 
1422  fprintf(stdout, "v4l2: buf %d: type %d (%s) ad: 0x%lx offset 0x%x+%d (=0x%x),used %d\n", buf.index, buf.type,
1423  type.c_str(), buf.m.userptr, buf.m.offset, buf.length, buf.length, buf.bytesused);
1424 }
1425 
1443 {
1444  this->acquire(I);
1445  return *this;
1446 }
1447 
1465 {
1466  this->acquire(I);
1467  return *this;
1468 }
1469 
1470 #elif !defined(VISP_BUILD_SHARED_LIBS)
1471 // Work around to avoid warning: libvisp_sensor.a(vpV4l2Grabber.cpp.o) has no
1472 // symbols
1473 void dummy_vpV4l2Grabber() { };
1474 #endif
Error that can be emitted by the vpFrameGrabber class and its derivates.
@ settingError
Grabber settings error.
@ initializationError
Grabber initialization error.
@ otherError
Grabber returned an other error.
unsigned int height
Number of rows in the image.
bool init
Set to true if the frame grabber has been initialized.
unsigned int width
Number of columns in the image.
static void YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, unsigned int width, unsigned int height)
static void YUYVToGrey(unsigned char *yuyv, unsigned char *grey, unsigned int size)
static void GreyToRGBa(unsigned char *grey, unsigned char *rgba, unsigned int width, unsigned int height)
static void RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int width, unsigned int height, bool flip=false)
static void RGBaToGrey(unsigned char *rgba, unsigned char *grey, unsigned int width, unsigned int height, unsigned int nThreads=0)
static void RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned int size)
static void BGRToGrey(unsigned char *bgr, unsigned char *grey, unsigned int width, unsigned int height, bool flip=false, unsigned int nThreads=0)
static void BGRToRGBa(unsigned char *bgr, unsigned char *rgba, unsigned int width, unsigned int height, bool flip=false)
static void crop(const vpImage< Type > &I, double roi_top, double roi_left, unsigned int roi_height, unsigned int roi_width, vpImage< Type > &crop, unsigned int v_scale=1, unsigned int h_scale=1)
Definition: vpImageTools.h:301
unsigned int getWidth() const
Definition: vpImage.h:240
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:793
Type * bitmap
points toward the bitmap
Definition: vpImage.h:139
unsigned int getHeight() const
Definition: vpImage.h:182
Definition: vpRGBa.h:61
Defines a rectangle in the plane.
Definition: vpRect.h:76
double getWidth() const
Definition: vpRect.h:224
double getLeft() const
Definition: vpRect.h:170
double getHeight() const
Definition: vpRect.h:163
double getTop() const
Definition: vpRect.h:189
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
@ framerate_25fps
25 frames per second
void setWidth(unsigned w)
static const __u32 MAX_NORM
void setFramerate(vpV4l2FramerateType framerate)
vpV4l2Grabber & operator>>(vpImage< unsigned char > &I)
static const unsigned int MAX_CTRL
static const unsigned int MAX_BUFFERS
static const __u32 MAX_INPUTS
vpV4l2PixelFormatType getPixelFormat()
static const unsigned int FRAME_SIZE
static const unsigned int DEFAULT_SCALE
static const unsigned int DEFAULT_INPUT
void setInput(unsigned input=vpV4l2Grabber::DEFAULT_INPUT)
static const __u32 MAX_FORMAT
void open(vpImage< unsigned char > &I)
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
void setHeight(unsigned h)
virtual ~vpV4l2Grabber()
void setPixelFormat(vpV4l2PixelFormatType pixelformat)
void setNBuffers(unsigned nbuffers)
vpV4l2FramerateType getFramerate()
void setDevice(const std::string &devname)
void acquire(vpImage< unsigned char > &I)
#define vpERROR_TRACE
Definition: vpDebug.h:388