Visual Servoing Platform  version 3.5.1 under development (2022-12-01)
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 {
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 {
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 {
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 {
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 {
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),
745  static_cast<void *>(bitmap + 1 + (unsigned int)(roi.getTop() * width + roi.getLeft())),
746  I.getWidth() * sizeof(vpRGBa) - 1);
747  I[i][I.getWidth() - 1].A = 0;
748  }
749  }
750  break;
751  case V4L2_BGR24_FORMAT: // tested
752  if (roi == vpRect())
753  vpImageConvert::BGRToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width, height, false);
754  else {
756  vpImageConvert::BGRToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width, height, false);
757  vpImageTools::crop(tmp, roi, I);
758  }
759  break;
760  case V4L2_YUYV_FORMAT: // tested
761  if (roi == vpRect())
762  vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width, height);
763  else {
765  vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width, height);
766  vpImageTools::crop(tmp, roi, I);
767  }
768  break;
769  default:
770  std::cout << "V4l2 conversion not handled" << std::endl;
771  break;
772  }
773 
774  queueAll();
775 }
793 {
794  if (field == 2)
795  return 0; // top field
796  else if (field == 3)
797  return 1; // bottom field;
798  else {
799  close();
800 
801  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "V4l2 returns a bad frame field"));
802  return false;
803  }
804 }
820 {
821  this->m_framerate = framerate;
822 
823  if (framerate == vpV4l2Grabber::framerate_25fps)
824  setFrameFormat(V4L2_IMAGE_FORMAT);
825  else
826  setFrameFormat(V4L2_FRAME_FORMAT);
827 }
828 
839 
844 {
845  stopStreaming();
846  streaming = false;
847 
848  if (fd >= 0) {
849  // vpTRACE("v4l2_close()");
850  v4l2_close(fd);
851  fd = -1;
852  }
853 
854  if (inp != NULL) {
855  delete[] inp;
856  inp = NULL;
857  }
858  if (std != NULL) {
859  delete[] std;
860  std = NULL;
861  }
862  if (fmt != NULL) {
863  delete[] fmt;
864  fmt = NULL;
865  }
866  if (ctl != NULL) {
867  delete[] ctl;
868  ctl = NULL;
869  }
870  if (buf_v4l2 != NULL) {
871  delete[] buf_v4l2;
872  buf_v4l2 = NULL;
873  }
874  if (buf_me != NULL) {
875  delete[] buf_me;
876  buf_me = NULL;
877  }
878 }
879 
891 void vpV4l2Grabber::open()
892 {
893  /* Open Video Device */
894  struct stat st;
895 
896  if (-1 == stat(device, &st)) {
897  fprintf(stderr, "Cannot identify '%s': %d, %s\n", device, errno, strerror(errno));
898  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Cannot identify video device"));
899  }
900 
901  if (!S_ISCHR(st.st_mode)) {
902  fprintf(stderr, "%s is no device\n", device);
904  }
905  fd = v4l2_open(device, O_RDWR | O_NONBLOCK, 0);
906  if (fd < 0) {
907  close();
908 
909  vpERROR_TRACE("No video device \"%s\"\n", device);
910  throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Can't access to video device"));
911  }
912 
913  if (inp != NULL) {
914  delete[] inp;
915  inp = NULL;
916  }
917  if (std != NULL) {
918  delete[] std;
919  std = NULL;
920  }
921  if (fmt != NULL) {
922  delete[] fmt;
923  fmt = NULL;
924  }
925  if (ctl != NULL) {
926  delete[] ctl;
927  ctl = NULL;
928  }
929  if (buf_v4l2 != NULL) {
930  delete[] buf_v4l2;
931  buf_v4l2 = NULL;
932  }
933  if (buf_me != NULL) {
934  delete[] buf_me;
935  buf_me = NULL;
936  }
937 
938  inp = new struct v4l2_input[vpV4l2Grabber::MAX_INPUTS];
939  std = new struct v4l2_standard[vpV4l2Grabber::MAX_NORM];
940  fmt = new struct v4l2_fmtdesc[vpV4l2Grabber::MAX_FORMAT];
941  ctl = new struct v4l2_queryctrl[vpV4l2Grabber::MAX_CTRL * 2];
942  buf_v4l2 = new struct v4l2_buffer[vpV4l2Grabber::MAX_BUFFERS];
943  buf_me = new struct ng_video_buf[vpV4l2Grabber::MAX_BUFFERS];
944 
945  /* Querry Video Device Capabilities */
946  if (v4l2_ioctl(fd, VIDIOC_QUERYCAP, &cap) == -1) {
947  close();
948  fprintf(stderr, "%s is no V4L2 device\n", device);
949  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Is not a V4L2 device"));
950  }
951  if (m_verbose) {
952  fprintf(stdout,
953  "v4l2 info:\n"
954  " device: %s\n"
955  " %s %d.%d.%d / %s @ %s\n",
956  device, cap.driver, (cap.version >> 16) & 0xff, (cap.version >> 8) & 0xff, cap.version & 0xff, cap.card,
957  cap.bus_info);
958  if (cap.capabilities & V4L2_CAP_VIDEO_OVERLAY)
959  fprintf(stdout, " Support overlay\n");
960  else
961  fprintf(stdout, " Does not support overlay\n");
962  if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)
963  fprintf(stdout, " Support capture\n");
964  else
965  fprintf(stdout, " Does not support capture\n");
966  if (cap.capabilities & V4L2_CAP_TUNER)
967  fprintf(stdout, " Support tuning\n");
968  else
969  fprintf(stdout, " Does not support tuning\n");
970  if (cap.capabilities & V4L2_CAP_STREAMING)
971  fprintf(stdout, " Support streaming capture.\n");
972  else
973  fprintf(stdout, " Does not support streaming capture\n");
974  if (cap.capabilities & V4L2_CAP_ASYNCIO)
975  fprintf(stdout, " Support asynchronous I/O methods\n");
976  else
977  fprintf(stdout, " Does not support asynchronous I/O methods\n");
978  if (cap.capabilities & V4L2_CAP_TIMEPERFRAME)
979  fprintf(stdout, " Support time per frame field\n");
980  else
981  fprintf(stdout, " Does not support time per frame field\n");
982  // Get framerate
983  streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
984  if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) != -1) {
985  fprintf(stdout, " Current acquisition framerate: %d fps\n", streamparm.parm.output.timeperframe.denominator);
986  }
987  }
988 
989  getCapabilities();
990 }
991 
998 void vpV4l2Grabber::getCapabilities()
999 {
1000  for (__u32 ninputs = 0; ninputs < MAX_INPUTS; ninputs++) {
1001  inp[ninputs].index = ninputs;
1002  if (v4l2_ioctl(fd, VIDIOC_ENUMINPUT, &inp[ninputs]))
1003  break;
1004  }
1005  for (__u32 nstds = 0; nstds < MAX_NORM; nstds++) {
1006  std[nstds].index = nstds;
1007  if (v4l2_ioctl(fd, VIDIOC_ENUMSTD, &std[nstds]))
1008  break;
1009  }
1010  for (__u32 nfmts = 0; nfmts < MAX_FORMAT; nfmts++) {
1011  fmt[nfmts].index = nfmts;
1012  fmt[nfmts].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1013  if (v4l2_ioctl(fd, VIDIOC_ENUM_FMT, &fmt[nfmts]))
1014  break;
1015  }
1016 
1017  streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1018  if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) == -1) {
1019  close();
1020 
1021  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't get video parameters"));
1022  }
1023 }
1024 
1038 void vpV4l2Grabber::setFormat()
1039 {
1040  fmt_me.width = width;
1041  fmt_me.height = height;
1042  // fmt_me.bytesperline = width; // bad (normally width * depth / 8), but
1043  // works
1044  // because initialized later by an ioctl call to VIDIOC_S_FMT
1045 
1046  switch (m_pixelformat) {
1047  case V4L2_GREY_FORMAT:
1048  fmt_me.pixelformat = V4L2_PIX_FMT_GREY;
1049  if (m_verbose)
1050  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_GREY)\n");
1051  break;
1052  case V4L2_RGB24_FORMAT:
1053  fmt_me.pixelformat = V4L2_PIX_FMT_RGB24;
1054  if (m_verbose)
1055  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_RGB24)\n");
1056  break;
1057  case V4L2_RGB32_FORMAT:
1058  fmt_me.pixelformat = V4L2_PIX_FMT_RGB32;
1059  if (m_verbose)
1060  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_RGB32)\n");
1061  break;
1062  case V4L2_BGR24_FORMAT:
1063  fmt_me.pixelformat = V4L2_PIX_FMT_BGR24;
1064  if (m_verbose)
1065  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_BGR24)\n");
1066  break;
1067  case V4L2_YUYV_FORMAT:
1068  fmt_me.pixelformat = V4L2_PIX_FMT_YUYV;
1069  if (m_verbose)
1070  fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_YUYV)\n");
1071  break;
1072 
1073  default:
1074  close();
1075 
1076  throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Bad format, probably do to a wrong scale"));
1077  }
1078 
1079  /* Get Video Format */
1080  vpCLEAR(fmt_v4l2);
1081 
1082  fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1083 
1084  if (v4l2_ioctl(fd, VIDIOC_G_FMT, &fmt_v4l2) == -1) {
1085  close();
1086 
1087  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't get video format"));
1088  }
1089  fmt_v4l2.fmt.pix.pixelformat = fmt_me.pixelformat;
1090  fmt_v4l2.fmt.pix.width = fmt_me.width;
1091  fmt_v4l2.fmt.pix.height = fmt_me.height;
1092  // printf("1 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width,
1093  // fmt_v4l2.fmt.pix.height);
1094 
1095  switch (m_frameformat) {
1096  case V4L2_FRAME_FORMAT:
1097  fmt_v4l2.fmt.pix.field = V4L2_FIELD_ALTERNATE;
1098  if (m_verbose) {
1099  fprintf(stdout, "v4l2: new capture params (V4L2_FIELD_ALTERNATE)\n");
1100  }
1101  break;
1102  case V4L2_IMAGE_FORMAT:
1103  fmt_v4l2.fmt.pix.field = V4L2_FIELD_INTERLACED;
1104  if (m_verbose) {
1105  fprintf(stdout, "v4l2: new capture params (V4L2_FIELD_INTERLACED)\n");
1106  }
1107  break;
1108  default:
1109  close();
1110 
1111  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Unrecognized frame format"));
1112  }
1113 
1114  // height and width of the captured image or frame
1115  if (m_frameformat == V4L2_FRAME_FORMAT && height > FRAME_SIZE) {
1116  height = FRAME_SIZE;
1117  }
1118  // printf("2 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width,
1119  // fmt_v4l2.fmt.pix.height);
1120 
1121  if (v4l2_ioctl(fd, VIDIOC_S_FMT, &fmt_v4l2) == -1) {
1122  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't set video format"));
1123  }
1124 
1125  if (fmt_v4l2.fmt.pix.pixelformat != fmt_me.pixelformat) {
1127  }
1128 
1129  /* Buggy driver paranoia. */
1130  unsigned int min = fmt_v4l2.fmt.pix.width * 2;
1131  if (fmt_v4l2.fmt.pix.bytesperline < min)
1132  fmt_v4l2.fmt.pix.bytesperline = min;
1133  min = fmt_v4l2.fmt.pix.bytesperline * fmt_v4l2.fmt.pix.height;
1134  if (fmt_v4l2.fmt.pix.sizeimage < min)
1135  fmt_v4l2.fmt.pix.sizeimage = min;
1136 
1137  fmt_me.width = fmt_v4l2.fmt.pix.width;
1138  fmt_me.height = fmt_v4l2.fmt.pix.height;
1139  fmt_me.bytesperline = fmt_v4l2.fmt.pix.bytesperline;
1140 
1141  if (m_verbose) {
1142  fprintf(stdout,
1143  "v4l2: new capture params (%ux%u, %c%c%c%c, %d byte, %d bytes "
1144  "per line)\n",
1145  fmt_me.width, fmt_me.height, fmt_v4l2.fmt.pix.pixelformat & 0xff,
1146  (fmt_v4l2.fmt.pix.pixelformat >> 8) & 0xff, (fmt_v4l2.fmt.pix.pixelformat >> 16) & 0xff,
1147  (fmt_v4l2.fmt.pix.pixelformat >> 24) & 0xff, fmt_v4l2.fmt.pix.sizeimage, fmt_v4l2.fmt.pix.bytesperline);
1148  }
1149 }
1158 void vpV4l2Grabber::startStreaming()
1159 {
1160  if (streaming == true) { // Acquisition in process.
1161  stopStreaming();
1162  streaming = false;
1163  }
1164 
1165  /* setup buffers */
1166  memset(&(reqbufs), 0, sizeof(reqbufs));
1167  reqbufs.count = m_nbuffers;
1168  reqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1169  reqbufs.memory = V4L2_MEMORY_MMAP;
1170 
1171  if (v4l2_ioctl(fd, VIDIOC_REQBUFS, &reqbufs) == -1) {
1172  if (EINVAL == errno) {
1173  fprintf(stderr,
1174  "%s does not support "
1175  "memory mapping\n",
1176  device);
1177  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Does not support memory mapping"));
1178  }
1179  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't require video buffers"));
1180  }
1181 
1182  for (unsigned i = 0; i < reqbufs.count; i++) {
1183  // Clear the buffer
1184  memset(&(buf_v4l2[i]), 0, sizeof(buf_v4l2[i]));
1185  buf_v4l2[i].index = i;
1186  buf_v4l2[i].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1187  buf_v4l2[i].memory = V4L2_MEMORY_MMAP;
1188  buf_v4l2[i].length = 0;
1189  if (v4l2_ioctl(fd, VIDIOC_QUERYBUF, &buf_v4l2[i]) == -1) {
1190  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't query video buffers"));
1191  }
1192  memcpy(&buf_me[i].fmt, &fmt_me, sizeof(ng_video_fmt));
1193  buf_me[i].size = buf_me[i].fmt.bytesperline * buf_me[i].fmt.height;
1194 
1195  // if (m_verbose)
1196  // std::cout << "1: buf_v4l2[" << i << "].length: " <<
1197  // buf_v4l2[i].length
1198  // << " buf_v4l2[" << i << "].offset: " << buf_v4l2[i].m.offset
1199  // << std::endl;
1200 
1201  buf_me[i].data = (unsigned char *)v4l2_mmap(NULL, buf_v4l2[i].length, PROT_READ | PROT_WRITE, MAP_SHARED, fd,
1202  (off_t)buf_v4l2[i].m.offset);
1203 
1204  if (buf_me[i].data == MAP_FAILED) {
1206  }
1207 
1208  buf_me[i].refcount = 0;
1209 
1210  // if (m_verbose)
1211  // {
1212  // std::cout << "2: buf_v4l2[" << i << "].length: " <<
1213  // buf_v4l2[i].length
1214  // << " buf_v4l2[" << i << "].offset: " << buf_v4l2[i].m.offset
1215  // << std::endl;
1216  // std::cout << "2: buf_me[" << i << "].size: " << buf_me[i].size <<
1217  // std::endl;
1218  // }
1219 
1220  if (m_verbose)
1221  printBufInfo(buf_v4l2[i]);
1222  }
1223 
1224  /* queue up all buffers */
1225  queueAll();
1226 
1227  /* Set video stream capture on */
1228  if (v4l2_ioctl(fd, VIDIOC_STREAMON, &fmt_v4l2.type) < 0) {
1229  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't start streaming"));
1230  }
1231 
1232  streaming = true;
1233 }
1234 
1241 void vpV4l2Grabber::stopStreaming()
1242 {
1243  // nothing to do if (fd < 0) or if (streaming == false)
1244  if ((fd >= 0) && (streaming == true)) {
1245 
1246  // vpTRACE(" Stop the streaming...");
1247  /* stop capture */
1248  fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1249  if (v4l2_ioctl(fd, VIDIOC_STREAMOFF, &fmt_v4l2.type)) {
1250  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't stop streaming"));
1251  }
1252  /* free buffers */
1253  for (unsigned int i = 0; i < reqbufs.count; i++) {
1254  if (m_verbose)
1255  printBufInfo(buf_v4l2[i]);
1256  // vpTRACE("v4l2_munmap()");
1257 
1258  if (-1 == v4l2_munmap(buf_me[i].data, buf_me[i].size)) {
1259  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't unmap memory"));
1260  }
1261  }
1262  queue = 0;
1263  waiton_cpt = 0;
1264  streaming = false;
1265  }
1266 }
1267 
1281 unsigned char *vpV4l2Grabber::waiton(__u32 &index, struct timeval &timestamp)
1282 {
1283  struct v4l2_buffer buf;
1284  struct timeval tv;
1285  fd_set rdset;
1286 
1287 /* wait for the next frame */
1288 again:
1289 
1290  tv.tv_sec = 30;
1291  tv.tv_usec = 0;
1292  FD_ZERO(&rdset);
1293  FD_SET(static_cast<unsigned int>(fd), &rdset);
1294  switch (select(fd + 1, &rdset, NULL, NULL, &tv)) {
1295  case -1:
1296  if (EINTR == errno)
1297  goto again;
1298  index = 0;
1299  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't access to the frame"));
1300  return NULL;
1301  case 0:
1302  index = 0;
1303  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't access to the frame: timeout"));
1304  return NULL;
1305  }
1306 
1307  /* get it */
1308  memset(&buf, 0, sizeof(buf));
1309  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1310  buf.memory = V4L2_MEMORY_MMAP; // Fabien manquait
1311  if (-1 == v4l2_ioctl(fd, VIDIOC_DQBUF, &buf)) {
1312  index = 0;
1313  switch (errno) {
1314  case EAGAIN:
1315  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: EAGAIN"));
1316  break;
1317  case EINVAL:
1318  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: EINVAL"));
1319  break;
1320  case ENOMEM:
1321  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: ENOMEM"));
1322  break;
1323  default:
1325  break;
1326  }
1327  return NULL;
1328  }
1329 
1330  waiton_cpt++;
1331  buf_v4l2[buf.index] = buf;
1332 
1333  index = buf.index;
1334 
1335  field = buf_v4l2[index].field;
1336 
1337  timestamp = buf_v4l2[index].timestamp;
1338 
1339  // if(m_verbose)
1340  // {
1341  // vpERROR_TRACE("field: %d\n", buf_v4l2[index].field);
1342 
1343  // vpERROR_TRACE("data adress : 0x%p\n", buf_me[buf.index].data);
1344  // }
1345  return buf_me[buf.index].data;
1346 }
1347 
1353 int vpV4l2Grabber::queueBuffer()
1354 {
1355  unsigned int frame = queue % reqbufs.count;
1356  int rc;
1357 
1358  if (0 != buf_me[frame].refcount) {
1359  if (0 != queue - waiton_cpt)
1360  return -1;
1361  fprintf(stderr, "v4l2: waiting for a free buffer..............\n");
1362  // ng_waiton_video_buf(h->buf_me+frame);
1363  std::cout << "Normalement call ng_waiton_video_buf(buf_me+frame); --------\n";
1364  }
1365 
1366  // std::cout << "frame: " << frame << std::endl;
1367  rc = v4l2_ioctl(fd, VIDIOC_QBUF, &buf_v4l2[frame]);
1368  if (0 == rc)
1369  queue++;
1370  else {
1371  switch (errno) {
1372  case EAGAIN:
1373  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: EAGAIN"));
1374  break;
1375  case EINVAL:
1376  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: EINVAL"));
1377  break;
1378  case ENOMEM:
1379  throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: ENOMEM"));
1380  break;
1381  default:
1383  break;
1384  }
1385  }
1386  return rc;
1387 }
1388 
1394 void vpV4l2Grabber::queueAll()
1395 {
1396  for (;;) {
1397  if (queue - waiton_cpt >= reqbufs.count) {
1398  return;
1399  }
1400  if (0 != queueBuffer()) {
1401  return;
1402  }
1403  }
1404 }
1405 
1411 void vpV4l2Grabber::printBufInfo(struct v4l2_buffer buf)
1412 {
1413  char type[40];
1414 
1415  switch (buf.type) {
1416  case V4L2_BUF_TYPE_VIDEO_CAPTURE:
1417  sprintf(type, "video-cap");
1418  break;
1419  case V4L2_BUF_TYPE_VIDEO_OVERLAY:
1420  sprintf(type, "video-over");
1421  break;
1422  case V4L2_BUF_TYPE_VIDEO_OUTPUT:
1423  sprintf(type, "video-out");
1424  break;
1425  case V4L2_BUF_TYPE_VBI_CAPTURE:
1426  sprintf(type, "vbi-cap");
1427  break;
1428  case V4L2_BUF_TYPE_VBI_OUTPUT:
1429  sprintf(type, "vbi-out");
1430  break;
1431  default:
1432  sprintf(type, "unknown");
1433  break;
1434  }
1435 
1436  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,
1437  buf.m.offset, buf.length, buf.length, buf.bytesused);
1438 }
1439 
1457 {
1458  this->acquire(I);
1459  return *this;
1460 }
1461 
1479 {
1480  this->acquire(I);
1481  return *this;
1482 }
1483 
1484 #elif !defined(VISP_BUILD_SHARED_LIBS)
1485 // Work around to avoid warning: libvisp_sensor.a(vpV4l2Grabber.cpp.o) has no
1486 // symbols
1487 void dummy_vpV4l2Grabber(){};
1488 #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:246
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:799
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
unsigned int getHeight() const
Definition: vpImage.h:188
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