Visual Servoing Platform  version 3.5.1 under development (2023-03-14)
vpV4l2Grabber.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 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 http://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(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
148  buf_v4l2(NULL), buf_me(NULL), 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(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
204  buf_v4l2(NULL), buf_me(NULL), 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(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
250  buf_v4l2(NULL), buf_me(NULL), 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(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
296  buf_v4l2(NULL), buf_me(NULL), 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(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
345  buf_v4l2(NULL), buf_me(NULL), 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  } catch (...) {
422  if (m_verbose) {
423  std::cout << "Requested pixel format [" << req_pixelformat << "] not compatible with camera" << std::endl;
424  std::cout << "Try to found a compatible pixel format..." << std::endl;
425  }
426 
427  // try to fing a compatible format
428  for (int format = 0; format < (int)V4L2_MAX_FORMAT; format++) {
429  if (format == req_pixelformat) {
430  continue;
431  }
432  try {
434  setFormat();
435  startStreaming();
436  if (m_verbose)
437  std::cout << "This format [" << m_pixelformat << "] is compatible with camera" << std::endl;
438 
439  break;
440  } catch (...) {
441  if (m_verbose)
442  std::cout << "This format [" << m_pixelformat << "] is not compatible with camera" << std::endl;
443  if (format == (int)V4L2_MAX_FORMAT) {
444  std::cout << "No pixel format compatible with the camera was found" << std::endl;
445  close();
446 
448  "No pixel format compatible with the camera was found"));
449  }
450  }
451  }
452  }
453 
454  I.resize(height, width);
455 
456  init = true;
457 }
458 
469 {
470  open();
471 
472  if (v4l2_ioctl(fd, VIDIOC_S_INPUT, &m_input) == -1) {
473  std::cout << "Warning: cannot set input channel to " << m_input << std::endl;
474  }
475 
476  vpV4l2PixelFormatType req_pixelformat = getPixelFormat();
477 
478  try {
479  setFormat();
480 
481  startStreaming();
482  } catch (...) {
483  if (m_verbose) {
484  std::cout << "Requested pixel format [" << m_pixelformat << "] not compatible with camera" << std::endl;
485  std::cout << "Try to found a compatible pixel format..." << std::endl;
486  }
487 
488  // try to fing a compatible format
489  for (int format = 0; format < (int)V4L2_MAX_FORMAT; format++) {
490  if (format == req_pixelformat) {
491  continue;
492  }
493  try {
495  setFormat();
496  startStreaming();
497  if (m_verbose)
498  std::cout << "This format [" << m_pixelformat << "] is compatible with camera" << std::endl;
499 
500  break;
501  } catch (...) {
502  if (m_verbose)
503  std::cout << "This format [" << m_pixelformat << "] is not compatible with camera" << std::endl;
504  }
505  }
506  }
507 
508  I.resize(height, width);
509 
510  init = true;
511 }
512 
524 {
525  struct timeval timestamp;
526  vpRect roi;
527 
528  acquire(I, timestamp, roi);
529 }
530 
543 {
544  struct timeval timestamp;
545 
546  acquire(I, timestamp, roi);
547 }
548 
568 void vpV4l2Grabber::acquire(vpImage<unsigned char> &I, struct timeval &timestamp, const vpRect &roi)
569 {
570  if (init == false) {
571  open(I);
572  }
573 
574  if (init == false) {
575  close();
576 
577  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "V4l2 frame grabber not initialized"));
578  }
579 
580  unsigned char *bitmap;
581  bitmap = waiton(index_buffer, timestamp);
582 
583  if (roi == vpRect())
584  I.resize(height, width);
585  else
586  I.resize((unsigned int)roi.getHeight(), (unsigned int)roi.getWidth());
587  switch (m_pixelformat) {
588  case V4L2_GREY_FORMAT:
589  if (roi == vpRect())
590  memcpy(I.bitmap, bitmap, height * width * sizeof(unsigned char));
591  else
592  vpImageTools::crop(bitmap, width, height, roi, I);
593  break;
594  case V4L2_RGB24_FORMAT: // tested
595  if (roi == vpRect())
596  vpImageConvert::RGBToGrey((unsigned char *)bitmap, I.bitmap, width * height);
597  else {
599  vpImageConvert::RGBToGrey((unsigned char *)bitmap, tmp.bitmap, width * height);
600  vpImageTools::crop(tmp, roi, I);
601  }
602  break;
603  case V4L2_RGB32_FORMAT:
604  if (roi == vpRect())
605  vpImageConvert::RGBaToGrey((unsigned char *)bitmap, I.bitmap, width * height);
606  else {
608  vpImageConvert::RGBaToGrey((unsigned char *)bitmap, tmp.bitmap, width * height);
609  vpImageTools::crop(tmp, roi, I);
610  }
611 
612  break;
613  case V4L2_BGR24_FORMAT: // tested
614  if (roi == vpRect())
615  vpImageConvert::BGRToGrey((unsigned char *)bitmap, I.bitmap, width, height, false);
616  else {
618  vpImageConvert::BGRToGrey((unsigned char *)bitmap, tmp.bitmap, width, height, false);
619  vpImageTools::crop(tmp, roi, I);
620  }
621  break;
622  case V4L2_YUYV_FORMAT: // tested
623  if (roi == vpRect())
624  vpImageConvert::YUYVToGrey((unsigned char *)bitmap, I.bitmap, width * height);
625  else {
627  vpImageConvert::YUYVToGrey((unsigned char *)bitmap, tmp.bitmap, width * height);
628  vpImageTools::crop(tmp, roi, I);
629  }
630  break;
631  default:
632  std::cout << "V4L2 conversion not handled" << std::endl;
633  break;
634  }
635 
636  queueAll();
637 }
638 
650 {
651  struct timeval timestamp;
652  vpRect roi;
653 
654  acquire(I, timestamp, roi);
655 }
656 
669 {
670  struct timeval timestamp;
671 
672  acquire(I, timestamp, roi);
673 }
674 
694 void vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, struct timeval &timestamp, const vpRect &roi)
695 {
696  if (init == false) {
697  open(I);
698  }
699 
700  if (init == false) {
701  close();
702 
703  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "V4l2 frame grabber not initialized"));
704  }
705 
706  unsigned char *bitmap;
707  bitmap = waiton(index_buffer, timestamp);
708 
709  if (roi == vpRect())
710  I.resize(height, width);
711  else
712  I.resize((unsigned int)roi.getHeight(), (unsigned int)roi.getWidth());
713 
714  // The framegrabber acquire aRGB format. We just shift the data from 1 byte
715  // all the data and initialize the last byte
716 
717  switch (m_pixelformat) {
718  case V4L2_GREY_FORMAT:
719  if (roi == vpRect())
720  vpImageConvert::GreyToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width * height);
721  else
722  vpImageTools::crop(bitmap, width, height, roi, I);
723  break;
724  case V4L2_RGB24_FORMAT: // tested
725  if (roi == vpRect())
726  vpImageConvert::RGBToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width * height);
727  else {
729  vpImageConvert::RGBToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width * height);
730  vpImageTools::crop(tmp, roi, I);
731  }
732  break;
733  case V4L2_RGB32_FORMAT:
734  if (roi == vpRect()) {
735  // The framegrabber acquire aRGB format. We just shift the data
736  // from 1 byte all the data and initialize the last byte
737  memcpy(static_cast<void *>(I.bitmap), static_cast<void *>(bitmap + 1), height * width * sizeof(vpRGBa) - 1);
738  I[height - 1][width - 1].A = 0;
739  } else {
740  for (unsigned int i = 0; i < I.getHeight(); i++) {
741  memcpy(static_cast<void *>(I.bitmap),
742  static_cast<void *>(bitmap + 1 + (unsigned int)(roi.getTop() * width + roi.getLeft())),
743  I.getWidth() * sizeof(vpRGBa) - 1);
744  I[i][I.getWidth() - 1].A = 0;
745  }
746  }
747  break;
748  case V4L2_BGR24_FORMAT: // tested
749  if (roi == vpRect())
750  vpImageConvert::BGRToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width, height, false);
751  else {
753  vpImageConvert::BGRToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width, height, false);
754  vpImageTools::crop(tmp, roi, I);
755  }
756  break;
757  case V4L2_YUYV_FORMAT: // tested
758  if (roi == vpRect())
759  vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width, height);
760  else {
762  vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width, height);
763  vpImageTools::crop(tmp, roi, I);
764  }
765  break;
766  default:
767  std::cout << "V4l2 conversion not handled" << std::endl;
768  break;
769  }
770 
771  queueAll();
772 }
790 {
791  if (field == 2)
792  return 0; // top field
793  else if (field == 3)
794  return 1; // bottom field;
795  else {
796  close();
797 
798  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "V4l2 returns a bad frame field"));
799  return false;
800  }
801 }
817 {
818  this->m_framerate = framerate;
819 
820  if (framerate == vpV4l2Grabber::framerate_25fps)
821  setFrameFormat(V4L2_IMAGE_FORMAT);
822  else
823  setFrameFormat(V4L2_FRAME_FORMAT);
824 }
825 
836 
841 {
842  stopStreaming();
843  streaming = false;
844 
845  if (fd >= 0) {
846  // vpTRACE("v4l2_close()");
847  v4l2_close(fd);
848  fd = -1;
849  }
850 
851  if (inp != NULL) {
852  delete[] inp;
853  inp = NULL;
854  }
855  if (std != NULL) {
856  delete[] std;
857  std = NULL;
858  }
859  if (fmt != NULL) {
860  delete[] fmt;
861  fmt = NULL;
862  }
863  if (ctl != NULL) {
864  delete[] ctl;
865  ctl = NULL;
866  }
867  if (buf_v4l2 != NULL) {
868  delete[] buf_v4l2;
869  buf_v4l2 = NULL;
870  }
871  if (buf_me != NULL) {
872  delete[] buf_me;
873  buf_me = NULL;
874  }
875 }
876 
888 void vpV4l2Grabber::open()
889 {
890  /* Open Video Device */
891  struct stat st;
892 
893  if (-1 == stat(device.c_str(), &st)) {
894  fprintf(stderr, "Cannot identify '%s': %d, %s\n", device.c_str(), errno, strerror(errno));
895  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Cannot identify video device"));
896  }
897 
898  if (!S_ISCHR(st.st_mode)) {
899  fprintf(stderr, "%s is no device\n", device.c_str());
901  }
902  fd = v4l2_open(device.c_str(), O_RDWR | O_NONBLOCK, 0);
903  if (fd < 0) {
904  close();
905 
906  vpERROR_TRACE("No video device \"%s\"\n", device.c_str());
907  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Can't access to video device"));
908  }
909 
910  if (inp != NULL) {
911  delete[] inp;
912  inp = NULL;
913  }
914  if (std != NULL) {
915  delete[] std;
916  std = NULL;
917  }
918  if (fmt != NULL) {
919  delete[] fmt;
920  fmt = NULL;
921  }
922  if (ctl != NULL) {
923  delete[] ctl;
924  ctl = NULL;
925  }
926  if (buf_v4l2 != NULL) {
927  delete[] buf_v4l2;
928  buf_v4l2 = NULL;
929  }
930  if (buf_me != NULL) {
931  delete[] buf_me;
932  buf_me = NULL;
933  }
934 
935  inp = new struct v4l2_input[vpV4l2Grabber::MAX_INPUTS];
936  std = new struct v4l2_standard[vpV4l2Grabber::MAX_NORM];
937  fmt = new struct v4l2_fmtdesc[vpV4l2Grabber::MAX_FORMAT];
938  ctl = new struct v4l2_queryctrl[vpV4l2Grabber::MAX_CTRL * 2];
939  buf_v4l2 = new struct v4l2_buffer[vpV4l2Grabber::MAX_BUFFERS];
940  buf_me = new struct ng_video_buf[vpV4l2Grabber::MAX_BUFFERS];
941 
942  /* Querry Video Device Capabilities */
943  if (v4l2_ioctl(fd, VIDIOC_QUERYCAP, &cap) == -1) {
944  close();
945  fprintf(stderr, "%s is no V4L2 device\n", device.c_str());
946  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Is not a V4L2 device"));
947  }
948  if (m_verbose) {
949  fprintf(stdout,
950  "v4l2 info:\n"
951  " device: %s\n"
952  " %s %d.%d.%d / %s @ %s\n",
953  device.c_str(), cap.driver, (cap.version >> 16) & 0xff, (cap.version >> 8) & 0xff, cap.version & 0xff,
954  cap.card, cap.bus_info);
955  if (cap.capabilities & V4L2_CAP_VIDEO_OVERLAY)
956  fprintf(stdout, " Support overlay\n");
957  else
958  fprintf(stdout, " Does not support overlay\n");
959  if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)
960  fprintf(stdout, " Support capture\n");
961  else
962  fprintf(stdout, " Does not support capture\n");
963  if (cap.capabilities & V4L2_CAP_TUNER)
964  fprintf(stdout, " Support tuning\n");
965  else
966  fprintf(stdout, " Does not support tuning\n");
967  if (cap.capabilities & V4L2_CAP_STREAMING)
968  fprintf(stdout, " Support streaming capture.\n");
969  else
970  fprintf(stdout, " Does not support streaming capture\n");
971  if (cap.capabilities & V4L2_CAP_ASYNCIO)
972  fprintf(stdout, " Support asynchronous I/O methods\n");
973  else
974  fprintf(stdout, " Does not support asynchronous I/O methods\n");
975  if (cap.capabilities & V4L2_CAP_TIMEPERFRAME)
976  fprintf(stdout, " Support time per frame field\n");
977  else
978  fprintf(stdout, " Does not support time per frame field\n");
979  // Get framerate
980  streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
981  if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) != -1) {
982  fprintf(stdout, " Current acquisition framerate: %d fps\n", streamparm.parm.output.timeperframe.denominator);
983  }
984  }
985 
986  getCapabilities();
987 }
988 
995 void vpV4l2Grabber::getCapabilities()
996 {
997  for (__u32 ninputs = 0; ninputs < MAX_INPUTS; ninputs++) {
998  inp[ninputs].index = ninputs;
999  if (v4l2_ioctl(fd, VIDIOC_ENUMINPUT, &inp[ninputs]))
1000  break;
1001  }
1002  for (__u32 nstds = 0; nstds < MAX_NORM; nstds++) {
1003  std[nstds].index = nstds;
1004  if (v4l2_ioctl(fd, VIDIOC_ENUMSTD, &std[nstds]))
1005  break;
1006  }
1007  for (__u32 nfmts = 0; nfmts < MAX_FORMAT; nfmts++) {
1008  fmt[nfmts].index = nfmts;
1009  fmt[nfmts].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1010  if (v4l2_ioctl(fd, VIDIOC_ENUM_FMT, &fmt[nfmts]))
1011  break;
1012  }
1013 
1014  streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1015  if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) == -1) {
1016  close();
1017 
1018  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't get video parameters"));
1019  }
1020 }
1021 
1035 void vpV4l2Grabber::setFormat()
1036 {
1037  fmt_me.width = width;
1038  fmt_me.height = height;
1039  // fmt_me.bytesperline = width; // bad (normally width * depth / 8), but
1040  // works
1041  // because initialized later by an ioctl call to VIDIOC_S_FMT
1042 
1043  switch (m_pixelformat) {
1044  case V4L2_GREY_FORMAT:
1045  fmt_me.pixelformat = V4L2_PIX_FMT_GREY;
1046  if (m_verbose)
1047  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_GREY)\n");
1048  break;
1049  case V4L2_RGB24_FORMAT:
1050  fmt_me.pixelformat = V4L2_PIX_FMT_RGB24;
1051  if (m_verbose)
1052  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_RGB24)\n");
1053  break;
1054  case V4L2_RGB32_FORMAT:
1055  fmt_me.pixelformat = V4L2_PIX_FMT_RGB32;
1056  if (m_verbose)
1057  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_RGB32)\n");
1058  break;
1059  case V4L2_BGR24_FORMAT:
1060  fmt_me.pixelformat = V4L2_PIX_FMT_BGR24;
1061  if (m_verbose)
1062  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_BGR24)\n");
1063  break;
1064  case V4L2_YUYV_FORMAT:
1065  fmt_me.pixelformat = V4L2_PIX_FMT_YUYV;
1066  if (m_verbose)
1067  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_YUYV)\n");
1068  break;
1069 
1070  default:
1071  close();
1072 
1073  throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Bad format, probably do to a wrong scale"));
1074  }
1075 
1076  /* Get Video Format */
1077  vpCLEAR(fmt_v4l2);
1078 
1079  fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1080 
1081  if (v4l2_ioctl(fd, VIDIOC_G_FMT, &fmt_v4l2) == -1) {
1082  close();
1083 
1084  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't get video format"));
1085  }
1086  fmt_v4l2.fmt.pix.pixelformat = fmt_me.pixelformat;
1087  fmt_v4l2.fmt.pix.width = fmt_me.width;
1088  fmt_v4l2.fmt.pix.height = fmt_me.height;
1089  // printf("1 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width,
1090  // fmt_v4l2.fmt.pix.height);
1091 
1092  switch (m_frameformat) {
1093  case V4L2_FRAME_FORMAT:
1094  fmt_v4l2.fmt.pix.field = V4L2_FIELD_ALTERNATE;
1095  if (m_verbose) {
1096  fprintf(stdout, "v4l2: new capture params (V4L2_FIELD_ALTERNATE)\n");
1097  }
1098  break;
1099  case V4L2_IMAGE_FORMAT:
1100  fmt_v4l2.fmt.pix.field = V4L2_FIELD_INTERLACED;
1101  if (m_verbose) {
1102  fprintf(stdout, "v4l2: new capture params (V4L2_FIELD_INTERLACED)\n");
1103  }
1104  break;
1105  default:
1106  close();
1107 
1108  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Unrecognized frame format"));
1109  }
1110 
1111  // height and width of the captured image or frame
1112  if (m_frameformat == V4L2_FRAME_FORMAT && height > FRAME_SIZE) {
1113  height = FRAME_SIZE;
1114  }
1115  // printf("2 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width,
1116  // fmt_v4l2.fmt.pix.height);
1117 
1118  if (v4l2_ioctl(fd, VIDIOC_S_FMT, &fmt_v4l2) == -1) {
1119  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't set video format"));
1120  }
1121 
1122  if (fmt_v4l2.fmt.pix.pixelformat != fmt_me.pixelformat) {
1124  }
1125 
1126  /* Buggy driver paranoia. */
1127  unsigned int min = fmt_v4l2.fmt.pix.width * 2;
1128  if (fmt_v4l2.fmt.pix.bytesperline < min)
1129  fmt_v4l2.fmt.pix.bytesperline = min;
1130  min = fmt_v4l2.fmt.pix.bytesperline * fmt_v4l2.fmt.pix.height;
1131  if (fmt_v4l2.fmt.pix.sizeimage < min)
1132  fmt_v4l2.fmt.pix.sizeimage = min;
1133 
1134  fmt_me.width = fmt_v4l2.fmt.pix.width;
1135  fmt_me.height = fmt_v4l2.fmt.pix.height;
1136  fmt_me.bytesperline = fmt_v4l2.fmt.pix.bytesperline;
1137 
1138  if (m_verbose) {
1139  fprintf(stdout,
1140  "v4l2: new capture params (%ux%u, %c%c%c%c, %d byte, %d bytes "
1141  "per line)\n",
1142  fmt_me.width, fmt_me.height, fmt_v4l2.fmt.pix.pixelformat & 0xff,
1143  (fmt_v4l2.fmt.pix.pixelformat >> 8) & 0xff, (fmt_v4l2.fmt.pix.pixelformat >> 16) & 0xff,
1144  (fmt_v4l2.fmt.pix.pixelformat >> 24) & 0xff, fmt_v4l2.fmt.pix.sizeimage, fmt_v4l2.fmt.pix.bytesperline);
1145  }
1146 }
1155 void vpV4l2Grabber::startStreaming()
1156 {
1157  if (streaming == true) { // Acquisition in process.
1158  stopStreaming();
1159  streaming = false;
1160  }
1161 
1162  /* setup buffers */
1163  memset(&(reqbufs), 0, sizeof(reqbufs));
1164  reqbufs.count = m_nbuffers;
1165  reqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1166  reqbufs.memory = V4L2_MEMORY_MMAP;
1167 
1168  if (v4l2_ioctl(fd, VIDIOC_REQBUFS, &reqbufs) == -1) {
1169  if (EINVAL == errno) {
1170  fprintf(stderr,
1171  "%s does not support "
1172  "memory mapping\n",
1173  device.c_str());
1174  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Does not support memory mapping"));
1175  }
1176  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't require video buffers"));
1177  }
1178 
1179  for (unsigned i = 0; i < reqbufs.count; i++) {
1180  // Clear the buffer
1181  memset(&(buf_v4l2[i]), 0, sizeof(buf_v4l2[i]));
1182  buf_v4l2[i].index = i;
1183  buf_v4l2[i].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1184  buf_v4l2[i].memory = V4L2_MEMORY_MMAP;
1185  buf_v4l2[i].length = 0;
1186  if (v4l2_ioctl(fd, VIDIOC_QUERYBUF, &buf_v4l2[i]) == -1) {
1187  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't query video buffers"));
1188  }
1189  memcpy(&buf_me[i].fmt, &fmt_me, sizeof(ng_video_fmt));
1190  buf_me[i].size = buf_me[i].fmt.bytesperline * buf_me[i].fmt.height;
1191 
1192  // if (m_verbose)
1193  // std::cout << "1: buf_v4l2[" << i << "].length: " <<
1194  // buf_v4l2[i].length
1195  // << " buf_v4l2[" << i << "].offset: " << buf_v4l2[i].m.offset
1196  // << std::endl;
1197 
1198  buf_me[i].data = (unsigned char *)v4l2_mmap(NULL, buf_v4l2[i].length, PROT_READ | PROT_WRITE, MAP_SHARED, fd,
1199  (off_t)buf_v4l2[i].m.offset);
1200 
1201  if (buf_me[i].data == MAP_FAILED) {
1203  }
1204 
1205  buf_me[i].refcount = 0;
1206 
1207  // if (m_verbose)
1208  // {
1209  // std::cout << "2: buf_v4l2[" << i << "].length: " <<
1210  // buf_v4l2[i].length
1211  // << " buf_v4l2[" << i << "].offset: " << buf_v4l2[i].m.offset
1212  // << std::endl;
1213  // std::cout << "2: buf_me[" << i << "].size: " << buf_me[i].size <<
1214  // std::endl;
1215  // }
1216 
1217  if (m_verbose)
1218  printBufInfo(buf_v4l2[i]);
1219  }
1220 
1221  /* queue up all buffers */
1222  queueAll();
1223 
1224  /* Set video stream capture on */
1225  if (v4l2_ioctl(fd, VIDIOC_STREAMON, &fmt_v4l2.type) < 0) {
1226  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't start streaming"));
1227  }
1228 
1229  streaming = true;
1230 }
1231 
1238 void vpV4l2Grabber::stopStreaming()
1239 {
1240  // nothing to do if (fd < 0) or if (streaming == false)
1241  if ((fd >= 0) && (streaming == true)) {
1242 
1243  // vpTRACE(" Stop the streaming...");
1244  /* stop capture */
1245  fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1246  if (v4l2_ioctl(fd, VIDIOC_STREAMOFF, &fmt_v4l2.type)) {
1247  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't stop streaming"));
1248  }
1249  /* free buffers */
1250  for (unsigned int i = 0; i < reqbufs.count; i++) {
1251  if (m_verbose)
1252  printBufInfo(buf_v4l2[i]);
1253  // vpTRACE("v4l2_munmap()");
1254 
1255  if (-1 == v4l2_munmap(buf_me[i].data, buf_me[i].size)) {
1256  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't unmap memory"));
1257  }
1258  }
1259  queue = 0;
1260  waiton_cpt = 0;
1261  streaming = false;
1262  }
1263 }
1264 
1278 unsigned char *vpV4l2Grabber::waiton(__u32 &index, struct timeval &timestamp)
1279 {
1280  struct v4l2_buffer buf;
1281  struct timeval tv;
1282  fd_set rdset;
1283 
1284 /* wait for the next frame */
1285 again:
1286 
1287  tv.tv_sec = 30;
1288  tv.tv_usec = 0;
1289  FD_ZERO(&rdset);
1290  FD_SET(static_cast<unsigned int>(fd), &rdset);
1291  switch (select(fd + 1, &rdset, NULL, NULL, &tv)) {
1292  case -1:
1293  if (EINTR == errno)
1294  goto again;
1295  index = 0;
1296  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't access to the frame"));
1297  return NULL;
1298  case 0:
1299  index = 0;
1300  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't access to the frame: timeout"));
1301  return NULL;
1302  }
1303 
1304  /* get it */
1305  memset(&buf, 0, sizeof(buf));
1306  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1307  buf.memory = V4L2_MEMORY_MMAP; // Fabien manquait
1308  if (-1 == v4l2_ioctl(fd, VIDIOC_DQBUF, &buf)) {
1309  index = 0;
1310  switch (errno) {
1311  case EAGAIN:
1312  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: EAGAIN"));
1313  break;
1314  case EINVAL:
1315  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: EINVAL"));
1316  break;
1317  case ENOMEM:
1318  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: ENOMEM"));
1319  break;
1320  default:
1322  break;
1323  }
1324  return NULL;
1325  }
1326 
1327  waiton_cpt++;
1328  buf_v4l2[buf.index] = buf;
1329 
1330  index = buf.index;
1331 
1332  field = buf_v4l2[index].field;
1333 
1334  timestamp = buf_v4l2[index].timestamp;
1335 
1336  // if(m_verbose)
1337  // {
1338  // vpERROR_TRACE("field: %d\n", buf_v4l2[index].field);
1339 
1340  // vpERROR_TRACE("data adress : 0x%p\n", buf_me[buf.index].data);
1341  // }
1342  return buf_me[buf.index].data;
1343 }
1344 
1350 int vpV4l2Grabber::queueBuffer()
1351 {
1352  unsigned int frame = queue % reqbufs.count;
1353  int rc;
1354 
1355  if (0 != buf_me[frame].refcount) {
1356  if (0 != queue - waiton_cpt)
1357  return -1;
1358  fprintf(stderr, "v4l2: waiting for a free buffer..............\n");
1359  // ng_waiton_video_buf(h->buf_me+frame);
1360  std::cout << "Normalement call ng_waiton_video_buf(buf_me+frame); --------\n";
1361  }
1362 
1363  // std::cout << "frame: " << frame << std::endl;
1364  rc = v4l2_ioctl(fd, VIDIOC_QBUF, &buf_v4l2[frame]);
1365  if (0 == rc)
1366  queue++;
1367  else {
1368  switch (errno) {
1369  case EAGAIN:
1370  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: EAGAIN"));
1371  break;
1372  case EINVAL:
1373  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: EINVAL"));
1374  break;
1375  case ENOMEM:
1376  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: ENOMEM"));
1377  break;
1378  default:
1380  break;
1381  }
1382  }
1383  return rc;
1384 }
1385 
1391 void vpV4l2Grabber::queueAll()
1392 {
1393  for (;;) {
1394  if (queue - waiton_cpt >= reqbufs.count) {
1395  return;
1396  }
1397  if (0 != queueBuffer()) {
1398  return;
1399  }
1400  }
1401 }
1402 
1408 void vpV4l2Grabber::printBufInfo(struct v4l2_buffer buf)
1409 {
1410  std::string type;
1411 
1412  switch (buf.type) {
1413  case V4L2_BUF_TYPE_VIDEO_CAPTURE:
1414  type = "video-cap";
1415  break;
1416  case V4L2_BUF_TYPE_VIDEO_OVERLAY:
1417  type = "video-over";
1418  break;
1419  case V4L2_BUF_TYPE_VIDEO_OUTPUT:
1420  type = "video-out";
1421  break;
1422  case V4L2_BUF_TYPE_VBI_CAPTURE:
1423  type = "vbi-cap";
1424  break;
1425  case V4L2_BUF_TYPE_VBI_OUTPUT:
1426  type = "vbi-out";
1427  break;
1428  default:
1429  type = "unknown";
1430  break;
1431  }
1432 
1433  fprintf(stdout, "v4l2: buf %d: type %d (%s) ad: 0x%lx offset 0x%x+%d (=0x%x),used %d\n", buf.index, buf.type,
1434  type.c_str(), buf.m.userptr, buf.m.offset, buf.length, buf.length, buf.bytesused);
1435 }
1436 
1454 {
1455  this->acquire(I);
1456  return *this;
1457 }
1458 
1476 {
1477  this->acquire(I);
1478  return *this;
1479 }
1480 
1481 #elif !defined(VISP_BUILD_SHARED_LIBS)
1482 // Work around to avoid warning: libvisp_sensor.a(vpV4l2Grabber.cpp.o) has no
1483 // symbols
1484 void dummy_vpV4l2Grabber(){};
1485 #endif
Error that can be emited by the vpFrameGrabber class and its derivates.
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:306
unsigned int getWidth() const
Definition: vpImage.h:247
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:800
Type * bitmap
points toward the bitmap
Definition: vpImage.h:144
unsigned int getHeight() const
Definition: vpImage.h:189
Definition: vpRGBa.h:67
Defines a rectangle in the plane.
Definition: vpRect.h:80
double getWidth() const
Definition: vpRect.h:228
double getLeft() const
Definition: vpRect.h:174
double getHeight() const
Definition: vpRect.h:167
double getTop() const
Definition: vpRect.h:193
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:393