Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
42#include <visp3/core/vpConfig.h>
43
44#ifdef VISP_HAVE_V4L2
45
46#include <errno.h>
47#include <fcntl.h>
48#include <iostream>
49#include <stdio.h>
50#include <stdlib.h>
51#include <sys/ioctl.h>
52#include <sys/mman.h>
53#include <sys/stat.h>
54#include <sys/time.h>
55#include <sys/types.h>
56#include <unistd.h>
57
58#include <visp3/core/vpFrameGrabberException.h>
59#include <visp3/sensor/vpV4l2Grabber.h>
60//#include <visp3/io/vpImageIo.h>
61#include <visp3/core/vpImageConvert.h>
62#include <visp3/core/vpImageTools.h>
63
64const unsigned int vpV4l2Grabber::DEFAULT_INPUT = 2;
65const unsigned int vpV4l2Grabber::DEFAULT_SCALE = 2;
66const __u32 vpV4l2Grabber::MAX_INPUTS = 16;
67const __u32 vpV4l2Grabber::MAX_NORM = 16;
68const __u32 vpV4l2Grabber::MAX_FORMAT = 32;
69const unsigned int vpV4l2Grabber::MAX_CTRL = 32;
70const unsigned int vpV4l2Grabber::MAX_BUFFERS = 32;
71const unsigned int vpV4l2Grabber::FRAME_SIZE = 288;
72#define vpCLEAR(x) memset(&(x), 0, sizeof(x))
73
147 : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
148 buf_v4l2(NULL), buf_me(NULL), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0),
149 streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
150 m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
151{
152 setDevice("/dev/video0");
153 setNBuffers(3);
158
159 init = false;
160}
161
203 : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
204 buf_v4l2(NULL), buf_me(NULL), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(verbose), m_nbuffers(3), field(0),
205 streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
206 m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
207{
208 setDevice("/dev/video0");
209 setNBuffers(3);
214
215 init = false;
216}
217
248vpV4l2Grabber::vpV4l2Grabber(unsigned input, unsigned scale)
249 : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
250 buf_v4l2(NULL), buf_me(NULL), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0),
251 streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
252 m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
253{
254 setDevice("/dev/video0");
255 setNBuffers(3);
257 setInput(input);
258 setScale(scale);
259
260 init = false;
261}
262
294vpV4l2Grabber::vpV4l2Grabber(vpImage<unsigned char> &I, unsigned input, unsigned scale)
295 : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
296 buf_v4l2(NULL), buf_me(NULL), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0),
297 streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
298 m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
299{
300 setDevice("/dev/video0");
301 setNBuffers(3);
303 setInput(input);
304 setScale(scale);
305
306 init = false;
307
308 open(I);
309}
310
343vpV4l2Grabber::vpV4l2Grabber(vpImage<vpRGBa> &I, unsigned input, unsigned scale)
344 : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL), fmt_v4l2(), fmt_me(), reqbufs(),
345 buf_v4l2(NULL), buf_me(NULL), queue(0), waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0),
346 streaming(false), m_input(vpV4l2Grabber::DEFAULT_INPUT), m_framerate(vpV4l2Grabber::framerate_25fps),
347 m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT), m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
348{
349 setDevice("/dev/video0");
350 setNBuffers(3);
352 setInput(input);
353 setScale(scale);
354
355 init = false;
356
357 open(I);
358}
359
366
370void vpV4l2Grabber::setInput(unsigned input) { this->m_input = input; }
371
384void vpV4l2Grabber::setScale(unsigned scale)
385{
386 if ((scale < 1) || (scale > 16)) {
387 close();
388
389 vpERROR_TRACE("Wrong scale %d, scale should be between 1 and 16", scale);
391 }
392
393 setWidth(640 / scale);
394 setHeight(480 / scale);
395}
396
408{
409 open();
410
411 if (v4l2_ioctl(fd, VIDIOC_S_INPUT, &m_input) == -1) {
412 std::cout << "Warning: cannot set input channel to " << m_input << std::endl;
413 }
414
415 vpV4l2PixelFormatType req_pixelformat = getPixelFormat();
416
417 try {
418 setFormat();
419
420 startStreaming();
421 }
422 catch (...) {
423 if (m_verbose) {
424 std::cout << "Requested pixel format [" << req_pixelformat << "] not compatible with camera" << std::endl;
425 std::cout << "Try to found a compatible pixel format..." << std::endl;
426 }
427
428 // try to fing a compatible format
429 for (int format = 0; format < (int)V4L2_MAX_FORMAT; format++) {
430 if (format == req_pixelformat) {
431 continue;
432 }
433 try {
435 setFormat();
436 startStreaming();
437 if (m_verbose)
438 std::cout << "This format [" << m_pixelformat << "] is compatible with camera" << std::endl;
439
440 break;
441 }
442 catch (...) {
443 if (m_verbose)
444 std::cout << "This format [" << m_pixelformat << "] is not compatible with camera" << std::endl;
445 if (format == (int)V4L2_MAX_FORMAT) {
446 std::cout << "No pixel format compatible with the camera was found" << std::endl;
447 close();
448
450 "No pixel format compatible with the camera was found"));
451 }
452 }
453 }
454 }
455
456 I.resize(height, width);
457
458 init = true;
459}
460
471{
472 open();
473
474 if (v4l2_ioctl(fd, VIDIOC_S_INPUT, &m_input) == -1) {
475 std::cout << "Warning: cannot set input channel to " << m_input << std::endl;
476 }
477
478 vpV4l2PixelFormatType req_pixelformat = getPixelFormat();
479
480 try {
481 setFormat();
482
483 startStreaming();
484 }
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 }
505 catch (...) {
506 if (m_verbose)
507 std::cout << "This format [" << m_pixelformat << "] is not compatible with camera" << std::endl;
508 }
509 }
510 }
511
512 I.resize(height, width);
513
514 init = true;
515}
516
528{
529 struct timeval timestamp;
530 vpRect roi;
531
532 acquire(I, timestamp, roi);
533}
534
547{
548 struct timeval timestamp;
549
550 acquire(I, timestamp, roi);
551}
552
572void vpV4l2Grabber::acquire(vpImage<unsigned char> &I, struct timeval &timestamp, const vpRect &roi)
573{
574 if (init == false) {
575 open(I);
576 }
577
578 if (init == false) {
579 close();
580
581 throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "V4l2 frame grabber not initialized"));
582 }
583
584 unsigned char *bitmap;
585 bitmap = waiton(index_buffer, timestamp);
586
587 if (roi == vpRect())
588 I.resize(height, width);
589 else
590 I.resize((unsigned int)roi.getHeight(), (unsigned int)roi.getWidth());
591 switch (m_pixelformat) {
592 case V4L2_GREY_FORMAT:
593 if (roi == vpRect())
594 memcpy(I.bitmap, bitmap, height * width * sizeof(unsigned char));
595 else
596 vpImageTools::crop(bitmap, width, height, roi, I);
597 break;
598 case V4L2_RGB24_FORMAT: // tested
599 if (roi == vpRect())
600 vpImageConvert::RGBToGrey((unsigned char *)bitmap, I.bitmap, width * height);
601 else {
603 vpImageConvert::RGBToGrey((unsigned char *)bitmap, tmp.bitmap, width * height);
604 vpImageTools::crop(tmp, roi, I);
605 }
606 break;
608 if (roi == vpRect())
609 vpImageConvert::RGBaToGrey((unsigned char *)bitmap, I.bitmap, width * height);
610 else {
612 vpImageConvert::RGBaToGrey((unsigned char *)bitmap, tmp.bitmap, width * height);
613 vpImageTools::crop(tmp, roi, I);
614 }
615
616 break;
617 case V4L2_BGR24_FORMAT: // tested
618 if (roi == vpRect())
619 vpImageConvert::BGRToGrey((unsigned char *)bitmap, I.bitmap, width, height, false);
620 else {
622 vpImageConvert::BGRToGrey((unsigned char *)bitmap, tmp.bitmap, width, height, false);
623 vpImageTools::crop(tmp, roi, I);
624 }
625 break;
626 case V4L2_YUYV_FORMAT: // tested
627 if (roi == vpRect())
628 vpImageConvert::YUYVToGrey((unsigned char *)bitmap, I.bitmap, width * height);
629 else {
631 vpImageConvert::YUYVToGrey((unsigned char *)bitmap, tmp.bitmap, width * height);
632 vpImageTools::crop(tmp, roi, I);
633 }
634 break;
635 default:
636 std::cout << "V4L2 conversion not handled" << std::endl;
637 break;
638 }
639
640 queueAll();
641}
642
654{
655 struct timeval timestamp;
656 vpRect roi;
657
658 acquire(I, timestamp, roi);
659}
660
673{
674 struct timeval timestamp;
675
676 acquire(I, timestamp, roi);
677}
678
698void vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, struct timeval &timestamp, const vpRect &roi)
699{
700 if (init == false) {
701 open(I);
702 }
703
704 if (init == false) {
705 close();
706
707 throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "V4l2 frame grabber not initialized"));
708 }
709
710 unsigned char *bitmap;
711 bitmap = waiton(index_buffer, timestamp);
712
713 if (roi == vpRect())
714 I.resize(height, width);
715 else
716 I.resize((unsigned int)roi.getHeight(), (unsigned int)roi.getWidth());
717
718 // The framegrabber acquire aRGB format. We just shift the data from 1 byte
719 // all the data and initialize the last byte
720
721 switch (m_pixelformat) {
722 case V4L2_GREY_FORMAT:
723 if (roi == vpRect())
724 vpImageConvert::GreyToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width * height);
725 else
726 vpImageTools::crop(bitmap, width, height, roi, I);
727 break;
728 case V4L2_RGB24_FORMAT: // tested
729 if (roi == vpRect())
730 vpImageConvert::RGBToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width * height);
731 else {
733 vpImageConvert::RGBToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width * height);
734 vpImageTools::crop(tmp, roi, I);
735 }
736 break;
738 if (roi == vpRect()) {
739 // The framegrabber acquire aRGB format. We just shift the data
740 // from 1 byte all the data and initialize the last byte
741 memcpy(static_cast<void *>(I.bitmap), static_cast<void *>(bitmap + 1), height * width * sizeof(vpRGBa) - 1);
742 I[height - 1][width - 1].A = 0;
743 }
744 else {
745 for (unsigned int i = 0; i < I.getHeight(); i++) {
746 memcpy(static_cast<void *>(I.bitmap),
747 static_cast<void *>(bitmap + 1 + (unsigned int)(roi.getTop() * width + roi.getLeft())),
748 I.getWidth() * sizeof(vpRGBa) - 1);
749 I[i][I.getWidth() - 1].A = 0;
750 }
751 }
752 break;
753 case V4L2_BGR24_FORMAT: // tested
754 if (roi == vpRect())
755 vpImageConvert::BGRToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width, height, false);
756 else {
758 vpImageConvert::BGRToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width, height, false);
759 vpImageTools::crop(tmp, roi, I);
760 }
761 break;
762 case V4L2_YUYV_FORMAT: // tested
763 if (roi == vpRect())
764 vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, (unsigned char *)I.bitmap, width, height);
765 else {
767 vpImageConvert::YUYVToRGBa((unsigned char *)bitmap, (unsigned char *)tmp.bitmap, width, height);
768 vpImageTools::crop(tmp, roi, I);
769 }
770 break;
771 default:
772 std::cout << "V4l2 conversion not handled" << std::endl;
773 break;
774 }
775
776 queueAll();
777}
795{
796 if (field == 2)
797 return 0; // top field
798 else if (field == 3)
799 return 1; // bottom field;
800 else {
801 close();
802
803 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "V4l2 returns a bad frame field"));
804 return false;
805 }
806}
822{
823 this->m_framerate = framerate;
824
825 if (framerate == vpV4l2Grabber::framerate_25fps)
826 setFrameFormat(V4L2_IMAGE_FORMAT);
827 else
828 setFrameFormat(V4L2_FRAME_FORMAT);
829}
830
841
846{
847 stopStreaming();
848 streaming = false;
849
850 if (fd >= 0) {
851 // vpTRACE("v4l2_close()");
852 v4l2_close(fd);
853 fd = -1;
854 }
855
856 if (inp != NULL) {
857 delete [] inp;
858 inp = NULL;
859 }
860 if (std != NULL) {
861 delete [] std;
862 std = NULL;
863 }
864 if (fmt != NULL) {
865 delete [] fmt;
866 fmt = NULL;
867 }
868 if (ctl != NULL) {
869 delete [] ctl;
870 ctl = NULL;
871 }
872 if (buf_v4l2 != NULL) {
873 delete [] buf_v4l2;
874 buf_v4l2 = NULL;
875 }
876 if (buf_me != NULL) {
877 delete [] buf_me;
878 buf_me = NULL;
879 }
880}
881
893void vpV4l2Grabber::open()
894{
895 /* Open Video Device */
896 struct stat st;
897
898 if (-1 == stat(device.c_str(), &st)) {
899 fprintf(stderr, "Cannot identify '%s': %d, %s\n", device.c_str(), errno, strerror(errno));
900 throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Cannot identify video device"));
901 }
902
903 if (!S_ISCHR(st.st_mode)) {
904 fprintf(stderr, "%s is no device\n", device.c_str());
906 }
907 fd = v4l2_open(device.c_str(), O_RDWR | O_NONBLOCK, 0);
908 if (fd < 0) {
909 close();
910
911 vpERROR_TRACE("No video device \"%s\"\n", device.c_str());
912 throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Can't access to video device"));
913 }
914
915 if (inp != NULL) {
916 delete [] inp;
917 inp = NULL;
918 }
919 if (std != NULL) {
920 delete [] std;
921 std = NULL;
922 }
923 if (fmt != NULL) {
924 delete [] fmt;
925 fmt = NULL;
926 }
927 if (ctl != NULL) {
928 delete [] ctl;
929 ctl = NULL;
930 }
931 if (buf_v4l2 != NULL) {
932 delete [] buf_v4l2;
933 buf_v4l2 = NULL;
934 }
935 if (buf_me != NULL) {
936 delete [] buf_me;
937 buf_me = NULL;
938 }
939
940 inp = new struct v4l2_input[vpV4l2Grabber::MAX_INPUTS];
941 std = new struct v4l2_standard[vpV4l2Grabber::MAX_NORM];
942 fmt = new struct v4l2_fmtdesc[vpV4l2Grabber::MAX_FORMAT];
943 ctl = new struct v4l2_queryctrl[vpV4l2Grabber::MAX_CTRL * 2];
944 buf_v4l2 = new struct v4l2_buffer[vpV4l2Grabber::MAX_BUFFERS];
945 buf_me = new struct ng_video_buf[vpV4l2Grabber::MAX_BUFFERS];
946
947 /* Querry Video Device Capabilities */
948 if (v4l2_ioctl(fd, VIDIOC_QUERYCAP, &cap) == -1) {
949 close();
950 fprintf(stderr, "%s is no V4L2 device\n", device.c_str());
951 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Is not a V4L2 device"));
952 }
953 if (m_verbose) {
954 fprintf(stdout,
955 "v4l2 info:\n"
956 " device: %s\n"
957 " %s %d.%d.%d / %s @ %s\n",
958 device.c_str(), cap.driver, (cap.version >> 16) & 0xff, (cap.version >> 8) & 0xff, cap.version & 0xff,
959 cap.card, cap.bus_info);
960 if (cap.capabilities & V4L2_CAP_VIDEO_OVERLAY)
961 fprintf(stdout, " Support overlay\n");
962 else
963 fprintf(stdout, " Does not support overlay\n");
964 if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)
965 fprintf(stdout, " Support capture\n");
966 else
967 fprintf(stdout, " Does not support capture\n");
968 if (cap.capabilities & V4L2_CAP_TUNER)
969 fprintf(stdout, " Support tuning\n");
970 else
971 fprintf(stdout, " Does not support tuning\n");
972 if (cap.capabilities & V4L2_CAP_STREAMING)
973 fprintf(stdout, " Support streaming capture.\n");
974 else
975 fprintf(stdout, " Does not support streaming capture\n");
976 if (cap.capabilities & V4L2_CAP_ASYNCIO)
977 fprintf(stdout, " Support asynchronous I/O methods\n");
978 else
979 fprintf(stdout, " Does not support asynchronous I/O methods\n");
980 if (cap.capabilities & V4L2_CAP_TIMEPERFRAME)
981 fprintf(stdout, " Support time per frame field\n");
982 else
983 fprintf(stdout, " Does not support time per frame field\n");
984 // Get framerate
985 streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
986 if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) != -1) {
987 fprintf(stdout, " Current acquisition framerate: %d fps\n", streamparm.parm.output.timeperframe.denominator);
988 }
989 }
990
991 getCapabilities();
992}
993
1000void vpV4l2Grabber::getCapabilities()
1001{
1002 for (__u32 ninputs = 0; ninputs < MAX_INPUTS; ninputs++) {
1003 inp[ninputs].index = ninputs;
1004 if (v4l2_ioctl(fd, VIDIOC_ENUMINPUT, &inp[ninputs]))
1005 break;
1006 }
1007 for (__u32 nstds = 0; nstds < MAX_NORM; nstds++) {
1008 std[nstds].index = nstds;
1009 if (v4l2_ioctl(fd, VIDIOC_ENUMSTD, &std[nstds]))
1010 break;
1011 }
1012 for (__u32 nfmts = 0; nfmts < MAX_FORMAT; nfmts++) {
1013 fmt[nfmts].index = nfmts;
1014 fmt[nfmts].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1015 if (v4l2_ioctl(fd, VIDIOC_ENUM_FMT, &fmt[nfmts]))
1016 break;
1017 }
1018
1019 streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1020 if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) == -1) {
1021 close();
1022
1023 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't get video parameters"));
1024 }
1025}
1026
1040void vpV4l2Grabber::setFormat()
1041{
1042 fmt_me.width = width;
1043 fmt_me.height = height;
1044 // fmt_me.bytesperline = width; // bad (normally width * depth / 8), but
1045 // works
1046 // because initialized later by an ioctl call to VIDIOC_S_FMT
1047
1048 switch (m_pixelformat) {
1049 case V4L2_GREY_FORMAT:
1050 fmt_me.pixelformat = V4L2_PIX_FMT_GREY;
1051 if (m_verbose)
1052 fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_GREY)\n");
1053 break;
1054 case V4L2_RGB24_FORMAT:
1055 fmt_me.pixelformat = V4L2_PIX_FMT_RGB24;
1056 if (m_verbose)
1057 fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_RGB24)\n");
1058 break;
1059 case V4L2_RGB32_FORMAT:
1060 fmt_me.pixelformat = V4L2_PIX_FMT_RGB32;
1061 if (m_verbose)
1062 fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_RGB32)\n");
1063 break;
1064 case V4L2_BGR24_FORMAT:
1065 fmt_me.pixelformat = V4L2_PIX_FMT_BGR24;
1066 if (m_verbose)
1067 fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_BGR24)\n");
1068 break;
1069 case V4L2_YUYV_FORMAT:
1070 fmt_me.pixelformat = V4L2_PIX_FMT_YUYV;
1071 if (m_verbose)
1072 fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_YUYV)\n");
1073 break;
1074
1075 default:
1076 close();
1077
1078 throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Bad format, probably do to a wrong scale"));
1079 }
1080
1081 /* Get Video Format */
1082 vpCLEAR(fmt_v4l2);
1083
1084 fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1085
1086 if (v4l2_ioctl(fd, VIDIOC_G_FMT, &fmt_v4l2) == -1) {
1087 close();
1088
1089 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't get video format"));
1090 }
1091 fmt_v4l2.fmt.pix.pixelformat = fmt_me.pixelformat;
1092 fmt_v4l2.fmt.pix.width = fmt_me.width;
1093 fmt_v4l2.fmt.pix.height = fmt_me.height;
1094 // printf("1 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width,
1095 // fmt_v4l2.fmt.pix.height);
1096
1097 switch (m_frameformat) {
1098 case V4L2_FRAME_FORMAT:
1099 fmt_v4l2.fmt.pix.field = V4L2_FIELD_ALTERNATE;
1100 if (m_verbose) {
1101 fprintf(stdout, "v4l2: new capture params (V4L2_FIELD_ALTERNATE)\n");
1102 }
1103 break;
1104 case V4L2_IMAGE_FORMAT:
1105 fmt_v4l2.fmt.pix.field = V4L2_FIELD_INTERLACED;
1106 if (m_verbose) {
1107 fprintf(stdout, "v4l2: new capture params (V4L2_FIELD_INTERLACED)\n");
1108 }
1109 break;
1110 default:
1111 close();
1112
1113 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Unrecognized frame format"));
1114 }
1115
1116 // height and width of the captured image or frame
1117 if (m_frameformat == V4L2_FRAME_FORMAT && height > FRAME_SIZE) {
1119 }
1120 // printf("2 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width,
1121 // fmt_v4l2.fmt.pix.height);
1122
1123 if (v4l2_ioctl(fd, VIDIOC_S_FMT, &fmt_v4l2) == -1) {
1124 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't set video format"));
1125 }
1126
1127 if (fmt_v4l2.fmt.pix.pixelformat != fmt_me.pixelformat) {
1129 }
1130
1131 /* Buggy driver paranoia. */
1132 unsigned int min = fmt_v4l2.fmt.pix.width * 2;
1133 if (fmt_v4l2.fmt.pix.bytesperline < min)
1134 fmt_v4l2.fmt.pix.bytesperline = min;
1135 min = fmt_v4l2.fmt.pix.bytesperline * fmt_v4l2.fmt.pix.height;
1136 if (fmt_v4l2.fmt.pix.sizeimage < min)
1137 fmt_v4l2.fmt.pix.sizeimage = min;
1138
1139 fmt_me.width = fmt_v4l2.fmt.pix.width;
1140 fmt_me.height = fmt_v4l2.fmt.pix.height;
1141 fmt_me.bytesperline = fmt_v4l2.fmt.pix.bytesperline;
1142
1143 if (m_verbose) {
1144 fprintf(stdout,
1145 "v4l2: new capture params (%ux%u, %c%c%c%c, %d byte, %d bytes "
1146 "per line)\n",
1147 fmt_me.width, fmt_me.height, fmt_v4l2.fmt.pix.pixelformat & 0xff,
1148 (fmt_v4l2.fmt.pix.pixelformat >> 8) & 0xff, (fmt_v4l2.fmt.pix.pixelformat >> 16) & 0xff,
1149 (fmt_v4l2.fmt.pix.pixelformat >> 24) & 0xff, fmt_v4l2.fmt.pix.sizeimage, fmt_v4l2.fmt.pix.bytesperline);
1150 }
1151}
1160void vpV4l2Grabber::startStreaming()
1161{
1162 if (streaming == true) { // Acquisition in process.
1163 stopStreaming();
1164 streaming = false;
1165 }
1166
1167 /* setup buffers */
1168 memset(&(reqbufs), 0, sizeof(reqbufs));
1169 reqbufs.count = m_nbuffers;
1170 reqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1171 reqbufs.memory = V4L2_MEMORY_MMAP;
1172
1173 if (v4l2_ioctl(fd, VIDIOC_REQBUFS, &reqbufs) == -1) {
1174 if (EINVAL == errno) {
1175 fprintf(stderr,
1176 "%s does not support "
1177 "memory mapping\n",
1178 device.c_str());
1179 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Does not support memory mapping"));
1180 }
1181 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't require video buffers"));
1182 }
1183
1184 for (unsigned i = 0; i < reqbufs.count; i++) {
1185 // Clear the buffer
1186 memset(&(buf_v4l2[i]), 0, sizeof(buf_v4l2[i]));
1187 buf_v4l2[i].index = i;
1188 buf_v4l2[i].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1189 buf_v4l2[i].memory = V4L2_MEMORY_MMAP;
1190 buf_v4l2[i].length = 0;
1191 if (v4l2_ioctl(fd, VIDIOC_QUERYBUF, &buf_v4l2[i]) == -1) {
1192 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't query video buffers"));
1193 }
1194 memcpy(&buf_me[i].fmt, &fmt_me, sizeof(ng_video_fmt));
1195 buf_me[i].size = buf_me[i].fmt.bytesperline * buf_me[i].fmt.height;
1196
1197 buf_me[i].data = (unsigned char *)v4l2_mmap(NULL, buf_v4l2[i].length, PROT_READ | PROT_WRITE, MAP_SHARED, fd,
1198 (off_t)buf_v4l2[i].m.offset);
1199
1200 if (buf_me[i].data == MAP_FAILED) {
1202 }
1203
1204 buf_me[i].refcount = 0;
1205
1206 if (m_verbose)
1207 printBufInfo(buf_v4l2[i]);
1208 }
1209
1210 /* queue up all buffers */
1211 queueAll();
1212
1213 /* Set video stream capture on */
1214 if (v4l2_ioctl(fd, VIDIOC_STREAMON, &fmt_v4l2.type) < 0) {
1215 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't start streaming"));
1216 }
1217
1218 streaming = true;
1219}
1220
1227void vpV4l2Grabber::stopStreaming()
1228{
1229 // nothing to do if (fd < 0) or if (streaming == false)
1230 if ((fd >= 0) && (streaming == true)) {
1231
1232 // vpTRACE(" Stop the streaming...");
1233 /* stop capture */
1234 fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1235 if (v4l2_ioctl(fd, VIDIOC_STREAMOFF, &fmt_v4l2.type)) {
1236 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't stop streaming"));
1237 }
1238 /* free buffers */
1239 for (unsigned int i = 0; i < reqbufs.count; i++) {
1240 if (m_verbose)
1241 printBufInfo(buf_v4l2[i]);
1242 // vpTRACE("v4l2_munmap()");
1243
1244 if (-1 == v4l2_munmap(buf_me[i].data, buf_me[i].size)) {
1246 }
1247 }
1248 queue = 0;
1249 waiton_cpt = 0;
1250 streaming = false;
1251 }
1252}
1253
1267unsigned char *vpV4l2Grabber::waiton(__u32 &index, struct timeval &timestamp)
1268{
1269 struct v4l2_buffer buf;
1270 struct timeval tv;
1271 fd_set rdset;
1272
1273 /* wait for the next frame */
1274again:
1275
1276 tv.tv_sec = 30;
1277 tv.tv_usec = 0;
1278 FD_ZERO(&rdset);
1279 FD_SET(static_cast<unsigned int>(fd), &rdset);
1280 switch (select(fd + 1, &rdset, NULL, NULL, &tv)) {
1281 case -1:
1282 if (EINTR == errno)
1283 goto again;
1284 index = 0;
1285 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't access to the frame"));
1286 return NULL;
1287 case 0:
1288 index = 0;
1289 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't access to the frame: timeout"));
1290 return NULL;
1291 }
1292
1293 /* get it */
1294 memset(&buf, 0, sizeof(buf));
1295 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1296 buf.memory = V4L2_MEMORY_MMAP;
1297 if (-1 == v4l2_ioctl(fd, VIDIOC_DQBUF, &buf)) {
1298 index = 0;
1299 switch (errno) {
1300 case EAGAIN:
1301 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: EAGAIN"));
1302 break;
1303 case EINVAL:
1304 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: EINVAL"));
1305 break;
1306 case ENOMEM:
1307 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_DQBUF: ENOMEM"));
1308 break;
1309 default:
1311 break;
1312 }
1313 return NULL;
1314 }
1315
1316 waiton_cpt++;
1317 buf_v4l2[buf.index] = buf;
1318
1319 index = buf.index;
1320
1321 field = buf_v4l2[index].field;
1322
1323 timestamp = buf_v4l2[index].timestamp;
1324
1325 // if(m_verbose)
1326 // {
1327 // vpERROR_TRACE("field: %d\n", buf_v4l2[index].field);
1328
1329 // vpERROR_TRACE("data adress : 0x%p\n", buf_me[buf.index].data);
1330 // }
1331 return buf_me[buf.index].data;
1332}
1333
1339int vpV4l2Grabber::queueBuffer()
1340{
1341 unsigned int frame = queue % reqbufs.count;
1342 int rc;
1343
1344 if (0 != buf_me[frame].refcount) {
1345 if (0 != queue - waiton_cpt)
1346 return -1;
1347 fprintf(stderr, "v4l2: waiting for a free buffer..............\n");
1348 // ng_waiton_video_buf(h->buf_me+frame);
1349 std::cout << "Normalement call ng_waiton_video_buf(buf_me+frame); --------\n";
1350 }
1351
1352 // std::cout << "frame: " << frame << std::endl;
1353 rc = v4l2_ioctl(fd, VIDIOC_QBUF, &buf_v4l2[frame]);
1354 if (0 == rc)
1355 queue++;
1356 else {
1357 switch (errno) {
1358 case EAGAIN:
1359 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: EAGAIN"));
1360 break;
1361 case EINVAL:
1362 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: EINVAL"));
1363 break;
1364 case ENOMEM:
1365 throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "VIDIOC_QBUF: ENOMEM"));
1366 break;
1367 default:
1369 break;
1370 }
1371 }
1372 return rc;
1373}
1374
1380void vpV4l2Grabber::queueAll()
1381{
1382 for (;;) {
1383 if (queue - waiton_cpt >= reqbufs.count) {
1384 return;
1385 }
1386 if (0 != queueBuffer()) {
1387 return;
1388 }
1389 }
1390}
1391
1397void vpV4l2Grabber::printBufInfo(struct v4l2_buffer buf)
1398{
1399 std::string type;
1400
1401 switch (buf.type) {
1402 case V4L2_BUF_TYPE_VIDEO_CAPTURE:
1403 type = "video-cap";
1404 break;
1405 case V4L2_BUF_TYPE_VIDEO_OVERLAY:
1406 type = "video-over";
1407 break;
1408 case V4L2_BUF_TYPE_VIDEO_OUTPUT:
1409 type = "video-out";
1410 break;
1411 case V4L2_BUF_TYPE_VBI_CAPTURE:
1412 type = "vbi-cap";
1413 break;
1414 case V4L2_BUF_TYPE_VBI_OUTPUT:
1415 type = "vbi-out";
1416 break;
1417 default:
1418 type = "unknown";
1419 break;
1420 }
1421
1422 fprintf(stdout, "v4l2: buf %d: type %d (%s) ad: 0x%lx offset 0x%x+%d (=0x%x),used %d\n", buf.index, buf.type,
1423 type.c_str(), buf.m.userptr, buf.m.offset, buf.length, buf.length, buf.bytesused);
1424}
1425
1443{
1444 this->acquire(I);
1445 return *this;
1446}
1447
1465{
1466 this->acquire(I);
1467 return *this;
1468}
1469
1470#elif !defined(VISP_BUILD_SHARED_LIBS)
1471// Work around to avoid warning: libvisp_sensor.a(vpV4l2Grabber.cpp.o) has no
1472// symbols
1473void dummy_vpV4l2Grabber() { };
1474#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 of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition vpImage.h:795
Type * bitmap
points toward the bitmap
Definition vpImage.h:139
unsigned int getHeight() const
Definition vpImage.h:184
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:388