Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
vpV4l2Grabber.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
45 #include <visp3/core/vpConfig.h>
46 
47 #ifdef VISP_HAVE_V4L2
48 
49 #include <errno.h>
50 #include <fcntl.h>
51 #include <iostream>
52 #include <stdio.h>
53 #include <stdlib.h>
54 #include <sys/ioctl.h>
55 #include <sys/mman.h>
56 #include <sys/stat.h>
57 #include <sys/time.h>
58 #include <sys/types.h>
59 #include <unistd.h>
60 
61 #include <visp3/core/vpFrameGrabberException.h>
62 #include <visp3/sensor/vpV4l2Grabber.h>
63 //#include <visp3/io/vpImageIo.h>
64 #include <visp3/core/vpImageConvert.h>
65 #include <visp3/core/vpImageTools.h>
66 
67 const unsigned int vpV4l2Grabber::DEFAULT_INPUT = 2;
68 const unsigned int vpV4l2Grabber::DEFAULT_SCALE = 2;
69 const __u32 vpV4l2Grabber::MAX_INPUTS = 16;
70 const __u32 vpV4l2Grabber::MAX_NORM = 16;
71 const __u32 vpV4l2Grabber::MAX_FORMAT = 32;
72 const unsigned int vpV4l2Grabber::MAX_CTRL = 32;
73 const unsigned int vpV4l2Grabber::MAX_BUFFERS = 32;
74 const unsigned int vpV4l2Grabber::FRAME_SIZE = 288;
75 #define vpCLEAR(x) memset(&(x), 0, sizeof(x))
76 
150  : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
151  buf_v4l2(NULL), buf_me(NULL), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0),
152  streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
153  m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
154 {
155  setDevice("/dev/video0");
156  setNBuffers(3);
161 
162  init = false;
163 }
164 
206  : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
207  buf_v4l2(NULL), buf_me(NULL), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(verbose), m_nbuffers(3), field(0),
208  streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
209  m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
210 {
211  setDevice("/dev/video0");
212  setNBuffers(3);
217 
218  init = false;
219 }
220 
251 vpV4l2Grabber::vpV4l2Grabber(unsigned input, unsigned scale)
252  : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
253  buf_v4l2(NULL), buf_me(NULL), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0),
254  streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
255  m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
256 {
257  setDevice("/dev/video0");
258  setNBuffers(3);
260  setInput(input);
261  setScale(scale);
262 
263  init = false;
264 }
265 
297 vpV4l2Grabber::vpV4l2Grabber(vpImage<unsigned char> &I, unsigned input, unsigned scale)
298  : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
299  buf_v4l2(NULL), buf_me(NULL), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0),
300  streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
301  m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
302 {
303  setDevice("/dev/video0");
304  setNBuffers(3);
306  setInput(input);
307  setScale(scale);
308 
309  init = false;
310 
311  open(I);
312 }
313 
346 vpV4l2Grabber::vpV4l2Grabber(vpImage<vpRGBa> &I, unsigned input, unsigned scale)
347  : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
348  buf_v4l2(NULL), buf_me(NULL), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0),
349  streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
350  m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
351 {
352  setDevice("/dev/video0");
353  setNBuffers(3);
355  setInput(input);
356  setScale(scale);
357 
358  init = false;
359 
360  open(I);
361 }
362 
369 
373 void vpV4l2Grabber::setInput(unsigned input) { this->m_input = input; }
374 
387 void vpV4l2Grabber::setScale(unsigned scale)
388 {
389  if ((scale < 1) || (scale > 16)) {
390  close();
391 
392  vpERROR_TRACE("Wrong scale %d, scale should be between 1 and 16", scale);
394  }
395 
396  setWidth(640 / scale);
397  setHeight(480 / scale);
398 }
399 
411 {
412  open();
413 
414  if (v4l2_ioctl(fd, VIDIOC_S_INPUT, &m_input) == -1) {
415  std::cout << "Warning: cannot set input channel to " << m_input << std::endl;
416  }
417 
418  vpV4l2PixelFormatType req_pixelformat = getPixelFormat();
419 
420  try {
421  setFormat();
422 
423  startStreaming();
424  } catch (...) {
425  if (m_verbose) {
426  std::cout << "Requested pixel format [" << req_pixelformat << "] not compatible with camera" << std::endl;
427  std::cout << "Try to found a compatible pixel format..." << std::endl;
428  }
429 
430  // try to fing a compatible format
431  for (int format = 0; format < (int)V4L2_MAX_FORMAT; format++) {
432  if (format == req_pixelformat) {
433  continue;
434  }
435  try {
437  setFormat();
438  startStreaming();
439  if (m_verbose)
440  std::cout << "This format [" << m_pixelformat << "] is compatible with camera" << std::endl;
441 
442  break;
443  } catch (...) {
444  if (m_verbose)
445  std::cout << "This format [" << m_pixelformat << "] is not compatible with camera" << std::endl;
446  if (format == (int)V4L2_MAX_FORMAT) {
447  std::cout << "No pixel format compatible with the camera was found" << std::endl;
448  close();
449 
451  "No pixel format compatible with the camera was found"));
452  }
453  }
454  }
455  }
456 
457  I.resize(height, width);
458 
459  init = true;
460 }
461 
472 {
473  open();
474 
475  if (v4l2_ioctl(fd, VIDIOC_S_INPUT, &m_input) == -1) {
476  std::cout << "Warning: cannot set input channel to " << m_input << std::endl;
477  }
478 
479  vpV4l2PixelFormatType req_pixelformat = getPixelFormat();
480 
481  try {
482  setFormat();
483 
484  startStreaming();
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  } catch (...) {
505  if (m_verbose)
506  std::cout << "This format [" << m_pixelformat << "] is not compatible with camera" << std::endl;
507  }
508  }
509  }
510 
511  I.resize(height, width);
512 
513  init = true;
514 }
515 
527 {
528  struct timeval timestamp;
529  vpRect roi;
530 
531  acquire(I, timestamp, roi);
532 }
533 
546 {
547  struct timeval timestamp;
548 
549  acquire(I, timestamp, roi);
550 }
551 
571 void vpV4l2Grabber::acquire(vpImage<unsigned char> &I, struct timeval &timestamp, const vpRect &roi)
572 {
573  if (init == false) {
574  open(I);
575  }
576 
577  if (init == false) {
578  close();
579 
580  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "V4l2 frame grabber not initialized"));
581  }
582 
583  unsigned char *bitmap;
584  bitmap = waiton(index_buffer, timestamp);
585 
586  if (roi == vpRect())
587  I.resize(height, width);
588  else
589  I.resize((unsigned int)roi.getHeight(), (unsigned int)roi.getWidth());
590  switch (m_pixelformat) {
591  case V4L2_GREY_FORMAT:
592  if (roi == vpRect())
593  memcpy(I.bitmap, bitmap, height * width * sizeof(unsigned char));
594  else
595  vpImageTools::crop(bitmap, width, height, roi, I);
596  break;
597  case V4L2_RGB24_FORMAT: // tested
598  if (roi == vpRect())
599  vpImageConvert::RGBToGrey((unsigned char *)bitmap, I.bitmap, width * height);
600  else {
601  vpImage<unsigned char> tmp(height, width);
602  vpImageConvert::RGBToGrey((unsigned char *)bitmap, tmp.bitmap, width * height);
603  vpImageTools::crop(tmp, roi, I);
604  }
605  break;
606  case V4L2_RGB32_FORMAT:
607  if (roi == vpRect())
608  vpImageConvert::RGBaToGrey((unsigned char *)bitmap, I.bitmap, width * height);
609  else {
610  vpImage<unsigned char> tmp(height, width);
611  vpImageConvert::RGBaToGrey((unsigned char *)bitmap, tmp.bitmap, width * height);
612  vpImageTools::crop(tmp, roi, I);
613  }
614 
615  break;
616  case V4L2_BGR24_FORMAT: // tested
617  if (roi == vpRect())
618  vpImageConvert::BGRToGrey((unsigned char *)bitmap, I.bitmap, width, height, false);
619  else {
620  vpImage<unsigned char> tmp(height, width);
621  vpImageConvert::BGRToGrey((unsigned char *)bitmap, tmp.bitmap, width, height, false);
622  vpImageTools::crop(tmp, roi, I);
623  }
624  break;
625  case V4L2_YUYV_FORMAT: // tested
626  if (roi == vpRect())
627  vpImageConvert::YUYVToGrey((unsigned char *)bitmap, I.bitmap, width * height);
628  else {
629  vpImage<unsigned char> tmp(height, width);
630  vpImageConvert::YUYVToGrey((unsigned char *)bitmap, tmp.bitmap, width * height);
631  vpImageTools::crop(tmp, roi, I);
632  }
633  break;
634  default:
635  std::cout << "V4L2 conversion not handled" << std::endl;
636  break;
637  }
638 
639  queueAll();
640 }
641 
653 {
654  struct timeval timestamp;
655  vpRect roi;
656 
657  acquire(I, timestamp, roi);
658 }
659 
672 {
673  struct timeval timestamp;
674 
675  acquire(I, timestamp, roi);
676 }
677 
697 void vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, struct timeval &timestamp, const vpRect &roi)
698 {
699  if (init == false) {
700  open(I);
701  }
702 
703  if (init == false) {
704  close();
705 
706  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "V4l2 frame grabber not initialized"));
707  }
708 
709  unsigned char *bitmap;
710  bitmap = waiton(index_buffer, timestamp);
711 
712  if (roi == vpRect())
713  I.resize(height, width);
714  else
715  I.resize((unsigned int)roi.getHeight(), (unsigned int)roi.getWidth());
716 
717  // The framegrabber acquire aRGB format. We just shift the data from 1 byte
718  // all the data and initialize the last byte
719 
720  switch (m_pixelformat) {
721  case V4L2_GREY_FORMAT:
722  if (roi == vpRect())
723  vpImageConvert::GreyToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width * height);
724  else
725  vpImageTools::crop(bitmap, width, height, roi, I);
726  break;
727  case V4L2_RGB24_FORMAT: // tested
728  if (roi == vpRect())
729  vpImageConvert::RGBToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width * height);
730  else {
731  vpImage<vpRGBa> tmp(height, width);
732  vpImageConvert::RGBToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width * height);
733  vpImageTools::crop(tmp, roi, I);
734  }
735  break;
736  case V4L2_RGB32_FORMAT:
737  if (roi == vpRect()) {
738  // The framegrabber acquire aRGB format. We just shift the data
739  // from 1 byte all the data and initialize the last byte
740  memcpy(static_cast<void*>(I.bitmap), static_cast<void*>(bitmap + 1), height * width * sizeof(vpRGBa) - 1);
741  I[height - 1][width - 1].A = 0;
742  } else {
743  for (unsigned int i = 0; i < I.getHeight(); i++) {
744  memcpy(static_cast<void*>(I.bitmap), static_cast<void*>(bitmap + 1 + (unsigned int)(roi.getTop() * width + roi.getLeft())),
745  I.getWidth() * sizeof(vpRGBa) - 1);
746  I[i][I.getWidth() - 1].A = 0;
747  }
748  }
749  break;
750  case V4L2_BGR24_FORMAT: // tested
751  if (roi == vpRect())
752  vpImageConvert::BGRToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width, height, false);
753  else {
754  vpImage<vpRGBa> tmp(height, width);
755  vpImageConvert::BGRToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width, height, false);
756  vpImageTools::crop(tmp, roi, I);
757  }
758  break;
759  case V4L2_YUYV_FORMAT: // tested
760  if (roi == vpRect())
761  vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width, height);
762  else {
763  vpImage<vpRGBa> tmp(height, width);
764  vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width, height);
765  vpImageTools::crop(tmp, roi, I);
766  }
767  break;
768  default:
769  std::cout << "V4l2 conversion not handled" << std::endl;
770  break;
771  }
772 
773  queueAll();
774 }
792 {
793  if (field == 2)
794  return 0; // top field
795  else if (field == 3)
796  return 1; // bottom field;
797  else {
798  close();
799 
800  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "V4l2 returns a bad frame field"));
801  return false;
802  }
803 }
819 {
820  this->m_framerate = framerate;
821 
822  if (framerate == vpV4l2Grabber::framerate_25fps)
823  setFrameFormat(V4L2_IMAGE_FORMAT);
824  else
825  setFrameFormat(V4L2_FRAME_FORMAT);
826 }
827 
838 
843 {
844  stopStreaming();
845  streaming = false;
846 
847  if (fd >= 0) {
848  // vpTRACE("v4l2_close()");
849  v4l2_close(fd);
850  fd = -1;
851  }
852 
853  if (inp != NULL) {
854  delete[] inp;
855  inp = NULL;
856  }
857  if (std != NULL) {
858  delete[] std;
859  std = NULL;
860  }
861  if (fmt != NULL) {
862  delete[] fmt;
863  fmt = NULL;
864  }
865  if (ctl != NULL) {
866  delete[] ctl;
867  ctl = NULL;
868  }
869  if (buf_v4l2 != NULL) {
870  delete[] buf_v4l2;
871  buf_v4l2 = NULL;
872  }
873  if (buf_me != NULL) {
874  delete[] buf_me;
875  buf_me = NULL;
876  }
877 }
878 
890 void vpV4l2Grabber::open()
891 {
892  /* Open Video Device */
893  struct stat st;
894 
895  if (-1 == stat(device, &st)) {
896  fprintf(stderr, "Cannot identify '%s': %d, %s\n", device, errno, strerror(errno));
897  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Cannot identify video device"));
898  }
899 
900  if (!S_ISCHR(st.st_mode)) {
901  fprintf(stderr, "%s is no device\n", device);
903  }
904  fd = v4l2_open(device, O_RDWR | O_NONBLOCK, 0);
905  if (fd < 0) {
906  close();
907 
908  vpERROR_TRACE("No video device \"%s\"\n", device);
909  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Can't access to video device"));
910  }
911 
912  if (inp != NULL) {
913  delete[] inp;
914  inp = NULL;
915  }
916  if (std != NULL) {
917  delete[] std;
918  std = NULL;
919  }
920  if (fmt != NULL) {
921  delete[] fmt;
922  fmt = NULL;
923  }
924  if (ctl != NULL) {
925  delete[] ctl;
926  ctl = NULL;
927  }
928  if (buf_v4l2 != NULL) {
929  delete[] buf_v4l2;
930  buf_v4l2 = NULL;
931  }
932  if (buf_me != NULL) {
933  delete[] buf_me;
934  buf_me = NULL;
935  }
936 
937  inp = new struct v4l2_input[vpV4l2Grabber::MAX_INPUTS];
938  std = new struct v4l2_standard[vpV4l2Grabber::MAX_NORM];
939  fmt = new struct v4l2_fmtdesc[vpV4l2Grabber::MAX_FORMAT];
940  ctl = new struct v4l2_queryctrl[vpV4l2Grabber::MAX_CTRL * 2];
941  buf_v4l2 = new struct v4l2_buffer[vpV4l2Grabber::MAX_BUFFERS];
942  buf_me = new struct ng_video_buf[vpV4l2Grabber::MAX_BUFFERS];
943 
944  /* Querry Video Device Capabilities */
945  if (v4l2_ioctl(fd, VIDIOC_QUERYCAP, &cap) == -1) {
946  close();
947  fprintf(stderr, "%s is no V4L2 device\n", device);
948  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Is not a V4L2 device"));
949  }
950  if (m_verbose) {
951  fprintf(stdout,
952  "v4l2 info:\n"
953  " device: %s\n"
954  " %s %d.%d.%d / %s @ %s\n",
955  device, cap.driver, (cap.version >> 16) & 0xff, (cap.version >> 8) & 0xff, cap.version & 0xff, cap.card,
956  cap.bus_info);
957  if (cap.capabilities & V4L2_CAP_VIDEO_OVERLAY)
958  fprintf(stdout, " Support overlay\n");
959  else
960  fprintf(stdout, " Does not support overlay\n");
961  if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)
962  fprintf(stdout, " Support capture\n");
963  else
964  fprintf(stdout, " Does not support capture\n");
965  if (cap.capabilities & V4L2_CAP_TUNER)
966  fprintf(stdout, " Support tuning\n");
967  else
968  fprintf(stdout, " Does not support tuning\n");
969  if (cap.capabilities & V4L2_CAP_STREAMING)
970  fprintf(stdout, " Support streaming capture.\n");
971  else
972  fprintf(stdout, " Does not support streaming capture\n");
973  if (cap.capabilities & V4L2_CAP_ASYNCIO)
974  fprintf(stdout, " Support asynchronous I/O methods\n");
975  else
976  fprintf(stdout, " Does not support asynchronous I/O methods\n");
977  if (cap.capabilities & V4L2_CAP_TIMEPERFRAME)
978  fprintf(stdout, " Support time per frame field\n");
979  else
980  fprintf(stdout, " Does not support time per frame field\n");
981  // Get framerate
982  streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
983  if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) != -1) {
984  fprintf(stdout, " Current acquisition framerate: %d fps\n", streamparm.parm.output.timeperframe.denominator);
985  }
986  }
987 
988  getCapabilities();
989 }
990 
997 void vpV4l2Grabber::getCapabilities()
998 {
999  for (__u32 ninputs = 0; ninputs < MAX_INPUTS; ninputs++) {
1000  inp[ninputs].index = ninputs;
1001  if (v4l2_ioctl(fd, VIDIOC_ENUMINPUT, &inp[ninputs]))
1002  break;
1003  }
1004  for (__u32 nstds = 0; nstds < MAX_NORM; nstds++) {
1005  std[nstds].index = nstds;
1006  if (v4l2_ioctl(fd, VIDIOC_ENUMSTD, &std[nstds]))
1007  break;
1008  }
1009  for (__u32 nfmts = 0; nfmts < MAX_FORMAT; nfmts++) {
1010  fmt[nfmts].index = nfmts;
1011  fmt[nfmts].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1012  if (v4l2_ioctl(fd, VIDIOC_ENUM_FMT, &fmt[nfmts]))
1013  break;
1014  }
1015 
1016  streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1017  if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) == -1) {
1018  close();
1019 
1020  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't get video parameters"));
1021  }
1022 }
1023 
1037 void vpV4l2Grabber::setFormat()
1038 {
1039  fmt_me.width = width;
1040  fmt_me.height = height;
1041  // fmt_me.bytesperline = width; // bad (normally width * depth / 8), but
1042  // works
1043  // because initialized later by an ioctl call to VIDIOC_S_FMT
1044 
1045  switch (m_pixelformat) {
1046  case V4L2_GREY_FORMAT:
1047  fmt_me.pixelformat = V4L2_PIX_FMT_GREY;
1048  if (m_verbose)
1049  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_GREY)\n");
1050  break;
1051  case V4L2_RGB24_FORMAT:
1052  fmt_me.pixelformat = V4L2_PIX_FMT_RGB24;
1053  if (m_verbose)
1054  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_RGB24)\n");
1055  break;
1056  case V4L2_RGB32_FORMAT:
1057  fmt_me.pixelformat = V4L2_PIX_FMT_RGB32;
1058  if (m_verbose)
1059  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_RGB32)\n");
1060  break;
1061  case V4L2_BGR24_FORMAT:
1062  fmt_me.pixelformat = V4L2_PIX_FMT_BGR24;
1063  if (m_verbose)
1064  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_BGR24)\n");
1065  break;
1066  case V4L2_YUYV_FORMAT:
1067  fmt_me.pixelformat = V4L2_PIX_FMT_YUYV;
1068  if (m_verbose)
1069  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_YUYV)\n");
1070  break;
1071 
1072  default:
1073  close();
1074 
1075  throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Bad format, probably do to a wrong scale"));
1076  }
1077 
1078  /* Get Video Format */
1079  vpCLEAR(fmt_v4l2);
1080 
1081  fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1082 
1083  if (v4l2_ioctl(fd, VIDIOC_G_FMT, &fmt_v4l2) == -1) {
1084  close();
1085 
1086  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't get video format"));
1087  }
1088  fmt_v4l2.fmt.pix.pixelformat = fmt_me.pixelformat;
1089  fmt_v4l2.fmt.pix.width = fmt_me.width;
1090  fmt_v4l2.fmt.pix.height = fmt_me.height;
1091  // printf("1 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width,
1092  // fmt_v4l2.fmt.pix.height);
1093 
1094  switch (m_frameformat) {
1095  case V4L2_FRAME_FORMAT:
1096  fmt_v4l2.fmt.pix.field = V4L2_FIELD_ALTERNATE;
1097  if (m_verbose) {
1098  fprintf(stdout, "v4l2: new capture params (V4L2_FIELD_ALTERNATE)\n");
1099  }
1100  break;
1101  case V4L2_IMAGE_FORMAT:
1102  fmt_v4l2.fmt.pix.field = V4L2_FIELD_INTERLACED;
1103  if (m_verbose) {
1104  fprintf(stdout, "v4l2: new capture params (V4L2_FIELD_INTERLACED)\n");
1105  }
1106  break;
1107  default:
1108  close();
1109 
1110  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Unrecognized frame format"));
1111  }
1112 
1113  // height and width of the captured image or frame
1114  if (m_frameformat == V4L2_FRAME_FORMAT && height > FRAME_SIZE) {
1115  height = FRAME_SIZE;
1116  }
1117  // printf("2 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width,
1118  // fmt_v4l2.fmt.pix.height);
1119 
1120  if (v4l2_ioctl(fd, VIDIOC_S_FMT, &fmt_v4l2) == -1) {
1121  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't set video format"));
1122  }
1123 
1124  if (fmt_v4l2.fmt.pix.pixelformat != fmt_me.pixelformat) {
1126  }
1127 
1128  /* Buggy driver paranoia. */
1129  unsigned int min = fmt_v4l2.fmt.pix.width * 2;
1130  if (fmt_v4l2.fmt.pix.bytesperline < min)
1131  fmt_v4l2.fmt.pix.bytesperline = min;
1132  min = fmt_v4l2.fmt.pix.bytesperline * fmt_v4l2.fmt.pix.height;
1133  if (fmt_v4l2.fmt.pix.sizeimage < min)
1134  fmt_v4l2.fmt.pix.sizeimage = min;
1135 
1136  fmt_me.width = fmt_v4l2.fmt.pix.width;
1137  fmt_me.height = fmt_v4l2.fmt.pix.height;
1138  fmt_me.bytesperline = fmt_v4l2.fmt.pix.bytesperline;
1139 
1140  if (m_verbose) {
1141  fprintf(stdout,
1142  "v4l2: new capture params (%ux%u, %c%c%c%c, %d byte, %d bytes "
1143  "per line)\n",
1144  fmt_me.width, fmt_me.height, fmt_v4l2.fmt.pix.pixelformat & 0xff,
1145  (fmt_v4l2.fmt.pix.pixelformat >> 8) & 0xff, (fmt_v4l2.fmt.pix.pixelformat >> 16) & 0xff,
1146  (fmt_v4l2.fmt.pix.pixelformat >> 24) & 0xff, fmt_v4l2.fmt.pix.sizeimage, fmt_v4l2.fmt.pix.bytesperline);
1147  }
1148 }
1157 void vpV4l2Grabber::startStreaming()
1158 {
1159  if (streaming == true) { // Acquisition in process.
1160  stopStreaming();
1161  streaming = false;
1162  }
1163 
1164  /* setup buffers */
1165  memset(&(reqbufs), 0, sizeof(reqbufs));
1166  reqbufs.count = m_nbuffers;
1167  reqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1168  reqbufs.memory = V4L2_MEMORY_MMAP;
1169 
1170  if (v4l2_ioctl(fd, VIDIOC_REQBUFS, &reqbufs) == -1) {
1171  if (EINVAL == errno) {
1172  fprintf(stderr,
1173  "%s does not support "
1174  "memory mapping\n",
1175  device);
1176  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Does not support memory mapping"));
1177  }
1178  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't require video buffers"));
1179  }
1180 
1181  for (unsigned i = 0; i < reqbufs.count; i++) {
1182  // Clear the buffer
1183  memset(&(buf_v4l2[i]), 0, sizeof(buf_v4l2[i]));
1184  buf_v4l2[i].index = i;
1185  buf_v4l2[i].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1186  buf_v4l2[i].memory = V4L2_MEMORY_MMAP;
1187  buf_v4l2[i].length = 0;
1188  if (v4l2_ioctl(fd, VIDIOC_QUERYBUF, &buf_v4l2[i]) == -1) {
1189  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't query video buffers"));
1190  }
1191  memcpy(&buf_me[i].fmt, &fmt_me, sizeof(ng_video_fmt));
1192  buf_me[i].size = buf_me[i].fmt.bytesperline * buf_me[i].fmt.height;
1193 
1194  // if (m_verbose)
1195  // std::cout << "1: buf_v4l2[" << i << "].length: " <<
1196  // buf_v4l2[i].length
1197  // << " buf_v4l2[" << i << "].offset: " << buf_v4l2[i].m.offset
1198  // << std::endl;
1199 
1200  buf_me[i].data = (unsigned char *)v4l2_mmap(NULL, buf_v4l2[i].length, PROT_READ | PROT_WRITE, MAP_SHARED, fd,
1201  (off_t)buf_v4l2[i].m.offset);
1202 
1203  if (buf_me[i].data == MAP_FAILED) {
1205  }
1206 
1207  buf_me[i].refcount = 0;
1208 
1209  // if (m_verbose)
1210  // {
1211  // std::cout << "2: buf_v4l2[" << i << "].length: " <<
1212  // buf_v4l2[i].length
1213  // << " buf_v4l2[" << i << "].offset: " << buf_v4l2[i].m.offset
1214  // << std::endl;
1215  // std::cout << "2: buf_me[" << i << "].size: " << buf_me[i].size <<
1216  // std::endl;
1217  // }
1218 
1219  if (m_verbose)
1220  printBufInfo(buf_v4l2[i]);
1221  }
1222 
1223  /* queue up all buffers */
1224  queueAll();
1225 
1226  /* Set video stream capture on */
1227  if (v4l2_ioctl(fd, VIDIOC_STREAMON, &fmt_v4l2.type) < 0) {
1228  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't start streaming"));
1229  }
1230 
1231  streaming = true;
1232 }
1233 
1240 void vpV4l2Grabber::stopStreaming()
1241 {
1242  // nothing to do if (fd < 0) or if (streaming == false)
1243  if ((fd >= 0) && (streaming == true)) {
1244 
1245  // vpTRACE(" Stop the streaming...");
1246  /* stop capture */
1247  fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1248  if (v4l2_ioctl(fd, VIDIOC_STREAMOFF, &fmt_v4l2.type)) {
1249  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't stop streaming"));
1250  }
1251  /* free buffers */
1252  for (unsigned int i = 0; i < reqbufs.count; i++) {
1253  if (m_verbose)
1254  printBufInfo(buf_v4l2[i]);
1255  // vpTRACE("v4l2_munmap()");
1256 
1257  if (-1 == v4l2_munmap(buf_me[i].data, buf_me[i].size)) {
1258  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't unmap memory"));
1259  }
1260  }
1261  queue = 0;
1262  waiton_cpt = 0;
1263  streaming = false;
1264  }
1265 }
1266 
1280 unsigned char *vpV4l2Grabber::waiton(__u32 &index, struct timeval &timestamp)
1281 {
1282  struct v4l2_buffer buf;
1283  struct timeval tv;
1284  fd_set rdset;
1285 
1286 /* wait for the next frame */
1287 again:
1288 
1289  tv.tv_sec = 30;
1290  tv.tv_usec = 0;
1291  FD_ZERO(&rdset);
1292  FD_SET(static_cast<unsigned int>(fd), &rdset);
1293  switch (select(fd + 1, &rdset, NULL, NULL, &tv)) {
1294  case -1:
1295  if (EINTR == errno)
1296  goto again;
1297  index = 0;
1298  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't access to the frame"));
1299  return NULL;
1300  case 0:
1301  index = 0;
1302  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't access to the frame: timeout"));
1303  return NULL;
1304  }
1305 
1306  /* get it */
1307  memset(&buf, 0, sizeof(buf));
1308  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1309  buf.memory = V4L2_MEMORY_MMAP; // Fabien manquait
1310  if (-1 == v4l2_ioctl(fd, VIDIOC_DQBUF, &buf)) {
1311  index = 0;
1312  switch (errno) {
1313  case EAGAIN:
1314  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: EAGAIN"));
1315  break;
1316  case EINVAL:
1317  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: EINVAL"));
1318  break;
1319  case ENOMEM:
1320  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: ENOMEM"));
1321  break;
1322  default:
1324  break;
1325  }
1326  return NULL;
1327  }
1328 
1329  waiton_cpt++;
1330  buf_v4l2[buf.index] = buf;
1331 
1332  index = buf.index;
1333 
1334  field = buf_v4l2[index].field;
1335 
1336  timestamp = buf_v4l2[index].timestamp;
1337 
1338  // if(m_verbose)
1339  // {
1340  // vpERROR_TRACE("field: %d\n", buf_v4l2[index].field);
1341 
1342  // vpERROR_TRACE("data adress : 0x%p\n", buf_me[buf.index].data);
1343  // }
1344  return buf_me[buf.index].data;
1345 }
1346 
1352 int vpV4l2Grabber::queueBuffer()
1353 {
1354  unsigned int frame = queue % reqbufs.count;
1355  int rc;
1356 
1357  if (0 != buf_me[frame].refcount) {
1358  if (0 != queue - waiton_cpt)
1359  return -1;
1360  fprintf(stderr, "v4l2: waiting for a free buffer..............\n");
1361  // ng_waiton_video_buf(h->buf_me+frame);
1362  std::cout << "Normalement call ng_waiton_video_buf(buf_me+frame); --------\n";
1363  }
1364 
1365  // std::cout << "frame: " << frame << std::endl;
1366  rc = v4l2_ioctl(fd, VIDIOC_QBUF, &buf_v4l2[frame]);
1367  if (0 == rc)
1368  queue++;
1369  else {
1370  switch (errno) {
1371  case EAGAIN:
1372  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: EAGAIN"));
1373  break;
1374  case EINVAL:
1375  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: EINVAL"));
1376  break;
1377  case ENOMEM:
1378  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: ENOMEM"));
1379  break;
1380  default:
1382  break;
1383  }
1384  }
1385  return rc;
1386 }
1387 
1393 void vpV4l2Grabber::queueAll()
1394 {
1395  for (;;) {
1396  if (queue - waiton_cpt >= reqbufs.count) {
1397  return;
1398  }
1399  if (0 != queueBuffer()) {
1400  return;
1401  }
1402  }
1403 }
1404 
1410 void vpV4l2Grabber::printBufInfo(struct v4l2_buffer buf)
1411 {
1412  char type[40];
1413 
1414  switch (buf.type) {
1415  case V4L2_BUF_TYPE_VIDEO_CAPTURE:
1416  sprintf(type, "video-cap");
1417  break;
1418  case V4L2_BUF_TYPE_VIDEO_OVERLAY:
1419  sprintf(type, "video-over");
1420  break;
1421  case V4L2_BUF_TYPE_VIDEO_OUTPUT:
1422  sprintf(type, "video-out");
1423  break;
1424  case V4L2_BUF_TYPE_VBI_CAPTURE:
1425  sprintf(type, "vbi-cap");
1426  break;
1427  case V4L2_BUF_TYPE_VBI_OUTPUT:
1428  sprintf(type, "vbi-out");
1429  break;
1430  default:
1431  sprintf(type, "unknown");
1432  break;
1433  }
1434 
1435  fprintf(stdout, "v4l2: buf %d: %d ad: 0x%lx offset 0x%x+%d (=0x%x),used %d\n", buf.index, buf.type, buf.m.userptr,
1436  buf.m.offset, buf.length, buf.length, buf.bytesused);
1437 }
1438 
1456 {
1457  this->acquire(I);
1458  return *this;
1459 }
1460 
1478 {
1479  this->acquire(I);
1480  return *this;
1481 }
1482 
1483 #elif !defined(VISP_BUILD_SHARED_LIBS)
1484 // Work arround to avoid warning: libvisp_sensor.a(vpV4l2Grabber.cpp.o) has no
1485 // symbols
1486 void dummy_vpV4l2Grabber(){};
1487 #endif
static void BGRToRGBa(unsigned char *bgr, unsigned char *rgba, unsigned int width, unsigned int height, bool flip=false)
static const unsigned int MAX_CTRL
double getTop() const
Definition: vpRect.h:192
void acquire(vpImage< unsigned char > &I)
static void YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, unsigned int width, unsigned int height)
static void RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int size)
void open(vpImage< unsigned char > &I)
static const __u32 MAX_FORMAT
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:879
static void RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned int size)
vpV4l2FramerateType getFramerate()
static void BGRToGrey(unsigned char *bgr, unsigned char *grey, unsigned int width, unsigned int height, bool flip=false)
Type * bitmap
points toward the bitmap
Definition: vpImage.h:141
#define vpERROR_TRACE
Definition: vpDebug.h:393
void setDevice(const std::string &devname)
static void GreyToRGBa(unsigned char *grey, unsigned char *rgba, unsigned int size)
vpV4l2PixelFormatType getPixelFormat()
static const unsigned int DEFAULT_INPUT
Error that can be emited by the vpFrameGrabber class and its derivates.
Definition: vpRGBa.h:66
double getWidth() const
Definition: vpRect.h:227
virtual ~vpV4l2Grabber()
vpV4l2Grabber & operator>>(vpImage< unsigned char > &I)
static const unsigned int FRAME_SIZE
void setHeight(unsigned h)
unsigned int height
Number of rows in the image.
static void YUYVToGrey(unsigned char *yuyv, unsigned char *grey, unsigned int size)
static const __u32 MAX_INPUTS
void setWidth(unsigned w)
static const __u32 MAX_NORM
void setScale(unsigned scale=vpV4l2Grabber::DEFAULT_SCALE)
double getLeft() const
Definition: vpRect.h:173
void setInput(unsigned input=vpV4l2Grabber::DEFAULT_INPUT)
bool init
Set to true if the frame grabber has been initialized.
Class that is a wrapper over the Video4Linux2 (V4L2) driver.
unsigned int getHeight() const
Definition: vpImage.h:186
25 frames per second
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:296
double getHeight() const
Definition: vpRect.h:166
Defines a rectangle in the plane.
Definition: vpRect.h:78
void setNBuffers(unsigned nbuffers)
unsigned int getWidth() const
Definition: vpImage.h:244
unsigned int width
Number of columns in the image.
static void RGBaToGrey(unsigned char *rgba, unsigned char *grey, unsigned int size)
void setPixelFormat(vpV4l2PixelFormatType pixelformat)
static const unsigned int DEFAULT_SCALE
static const unsigned int MAX_BUFFERS
void setFramerate(vpV4l2FramerateType framerate)