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