Visual Servoing Platform version 3.5.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
67const unsigned int vpV4l2Grabber::DEFAULT_INPUT = 2;
68const unsigned int vpV4l2Grabber::DEFAULT_SCALE = 2;
69const __u32 vpV4l2Grabber::MAX_INPUTS = 16;
70const __u32 vpV4l2Grabber::MAX_NORM = 16;
71const __u32 vpV4l2Grabber::MAX_FORMAT = 32;
72const unsigned int vpV4l2Grabber::MAX_CTRL = 32;
73const unsigned int vpV4l2Grabber::MAX_BUFFERS = 32;
74const 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
251vpV4l2Grabber::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
297vpV4l2Grabber::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
346vpV4l2Grabber::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
373void vpV4l2Grabber::setInput(unsigned input) { this->m_input = input; }
374
387void 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
571void 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;
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
697void 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;
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
890void 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
997void 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
1037void 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) {
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}
1157void 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
1240void 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)) {
1259 }
1260 }
1261 queue = 0;
1262 waiton_cpt = 0;
1263 streaming = false;
1264 }
1265}
1266
1280unsigned 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 */
1287again:
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
1352int 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
1393void 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
1410void 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
1486void 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