Visual Servoing Platform  version 3.4.0
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), 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 {
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 {
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
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:305
unsigned int getWidth() const
Definition: vpImage.h:246
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: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