Panda3D
 All Classes Functions Variables Enumerations
webcamVideoCursorV4L.cxx
00001 // Filename: webcamVideoCursorV4L.cxx
00002 // Created by: rdb (11Jun2010)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
00012 //
00013 ////////////////////////////////////////////////////////////////////
00014 
00015 #include "webcamVideoV4L.h"
00016 
00017 #ifdef HAVE_VIDEO4LINUX
00018 
00019 #include <fcntl.h>
00020 #include <sys/mman.h>
00021 #include <sys/ioctl.h>
00022 #include <linux/videodev2.h>
00023 
00024 #ifdef HAVE_JPEG
00025 extern "C" {
00026   #include <jpeglib.h>
00027   #include <jerror.h>
00028 }
00029 
00030 #include <setjmp.h>
00031 #endif
00032 
00033 // This is supposed to be defined in jpegint.h,
00034 // but not all implementations of JPEG provide that file.
00035 #ifndef DSTATE_READY
00036 #define DSTATE_READY 202
00037 #endif
00038 
00039 TypeHandle WebcamVideoCursorV4L::_type_handle;
00040 
00041 #define clamp(x) min(max(x, 0.0), 255.0)
00042 
00043 INLINE static void yuv_to_rgb(unsigned char *dest, const unsigned char *src) {
00044   double y1 = (255 / 219.0) * (src[0] - 16);
00045   double pb = (255 / 224.0) * (src[1] - 128);
00046   double pr = (255 / 224.0) * (src[2] - 128);
00047   dest[2] = clamp(1.0 * y1 + 0     * pb + 1.402 * pr);
00048   dest[1] = clamp(1.0 * y1 - 0.344 * pb - 0.714 * pr);
00049   dest[0] = clamp(1.0 * y1 + 1.772 * pb + 0     * pr);
00050 }
00051 
00052 INLINE static void yuyv_to_rgbrgb(unsigned char *dest, const unsigned char *src) {
00053   unsigned char yuv[] = {src[0], src[1], src[3]};
00054   yuv_to_rgb(dest, yuv);
00055   yuv[0] = src[2];
00056   yuv_to_rgb(dest + 3, yuv);
00057 }
00058 
00059 INLINE static void yuyv_to_rgbargba(unsigned char *dest, const unsigned char *src) {
00060   unsigned char yuv[] = {src[0], src[1], src[3]};
00061   yuv_to_rgb(dest, yuv);
00062   yuv[0] = src[2];
00063   yuv_to_rgb(dest + 4, yuv);
00064   dest[3] = (unsigned char) -1;
00065   dest[7] = (unsigned char) -1;
00066 }
00067 
00068 #if defined(HAVE_JPEG) && !defined(CPPPARSER)
00069 
00070 struct my_error_mgr {
00071   struct jpeg_error_mgr pub;
00072   jmp_buf setjmp_buffer;
00073 };
00074 
00075 typedef struct my_error_mgr *my_error_ptr;
00076 
00077 static void my_error_exit (j_common_ptr cinfo) {
00078   // Output the error message
00079   char buffer[JMSG_LENGTH_MAX];
00080   (*cinfo->err->format_message) (cinfo, buffer);
00081   vision_cat.error() << buffer << "\n";
00082   // cinfo->err really points to a my_error_mgr struct, so coerce pointer
00083   my_error_ptr myerr = (my_error_ptr) cinfo->err;
00084   // Return control to the setjmp point
00085   longjmp(myerr->setjmp_buffer, 1);
00086 }
00087 
00088 static void my_output_message (j_common_ptr cinfo){
00089   char buffer[JMSG_LENGTH_MAX];
00090   (*cinfo->err->format_message) (cinfo, buffer);
00091   vision_cat.warning() << buffer << "\n";
00092 }
00093 
00094 static void my_init_source(j_decompress_ptr cinfo) {
00095 }
00096 
00097 static boolean my_fill_input_buffer(j_decompress_ptr cinfo) {
00098   struct jpeg_source_mgr *src = cinfo->src;
00099   static JOCTET FakeEOI[] = {0xFF, JPEG_EOI};
00100 
00101   WARNMS(cinfo, JWRN_JPEG_EOF);
00102 
00103   src->next_input_byte = FakeEOI;
00104   src->bytes_in_buffer = 2;
00105 
00106   return TRUE;
00107 }
00108 
00109 static void my_skip_input_data(j_decompress_ptr cinfo, long num_bytes) {
00110   struct jpeg_source_mgr *src = cinfo->src;
00111 
00112   if (num_bytes >= (long) src->bytes_in_buffer) {
00113     my_fill_input_buffer(cinfo);
00114     return;
00115   }
00116 
00117   src->bytes_in_buffer -= num_bytes;
00118   src->next_input_byte += num_bytes;
00119 }
00120 
00121 static void my_term_source(j_decompress_ptr cinfo) {
00122 }
00123 
00124 #endif
00125 
00126 ////////////////////////////////////////////////////////////////////
00127 //     Function: WebcamVideoCursorV4L::Constructor
00128 //       Access: Published
00129 //  Description:
00130 ////////////////////////////////////////////////////////////////////
00131 WebcamVideoCursorV4L::
00132 WebcamVideoCursorV4L(WebcamVideoV4L *src) : MovieVideoCursor(src) {
00133   _size_x = src->_size_x;
00134   _size_y = src->_size_y;
00135   _num_components = 3;
00136   _length = 1.0E10;
00137   _can_seek = false;
00138   _can_seek_fast = false;
00139   _aborted = false;
00140   _streaming = true;
00141   _ready = false;
00142   _format = (struct v4l2_format *) malloc(sizeof(struct v4l2_format));
00143   memset(_format, 0, sizeof(struct v4l2_format));
00144 #ifdef HAVE_JPEG
00145   _cinfo = NULL;
00146 #endif
00147   _buffers = NULL;
00148   _buflens = NULL;
00149   _fd = open(src->_device.c_str(), O_RDWR);
00150   if (-1 == _fd) {
00151     vision_cat.error() << "Failed to open " << src->_device.c_str() << "\n";
00152     return;
00153   }
00154 
00155   // Find the best format in our _pformats vector.
00156   // MJPEG is preferred over YUYV, as it's much smaller.
00157   _format->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00158   pvector<uint32_t>::iterator it;
00159   for (it = src->_pformats.begin(); it != src->_pformats.end(); ++it) {
00160 #ifdef HAVE_JPEG
00161     if (*it == V4L2_PIX_FMT_MJPEG) {
00162       _format->fmt.pix.pixelformat = *it;
00163       break;
00164     } else
00165 #endif
00166     if (*it == V4L2_PIX_FMT_YUYV) {
00167       _format->fmt.pix.pixelformat = *it;
00168       break;
00169     }
00170   }
00171   if (it == src->_pformats.end()) {
00172     vision_cat.error() << "Failed to find a suitable pixel format!\n";
00173     _ready = false;
00174     close(_fd);
00175     _fd = -1;
00176     return;
00177   }
00178 
00179   // Request a format of this size, and no interlacing
00180   _format->fmt.pix.width = _size_x;
00181   _format->fmt.pix.height = _size_y;
00182   _format->fmt.pix.field = V4L2_FIELD_NONE;
00183 
00184   // Now politely ask the driver to switch to this format
00185   if (-1 == ioctl(_fd, VIDIOC_S_FMT, _format)) {
00186     vision_cat.error() << "Driver rejected format!\n";
00187     _ready = false;
00188     close(_fd);
00189     _fd = -1;
00190     return;
00191   }
00192 
00193   _size_x = _format->fmt.pix.width;
00194   _size_y = _format->fmt.pix.height;
00195 
00196   struct v4l2_streamparm streamparm;
00197   memset(&streamparm, 0, sizeof streamparm);
00198   streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00199   streamparm.parm.capture.timeperframe.numerator = 1;
00200   streamparm.parm.capture.timeperframe.denominator = src->_fps;
00201   if (ioctl(_fd, VIDIOC_S_PARM, &streamparm) < 0) {
00202     vision_cat.error() << "Driver rejected framerate!\n";
00203   }
00204 
00205   struct v4l2_requestbuffers req;
00206   memset(&req, 0, sizeof req);
00207   req.count = 4;
00208   req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00209   req.memory = V4L2_MEMORY_MMAP;
00210 
00211   if (-1 == ioctl (_fd, VIDIOC_REQBUFS, &req)) {
00212     vision_cat.error() << "Failed to request buffers from webcam!\n";
00213   }
00214 
00215   if (req.count < 2) {
00216     vision_cat.error() << "Insufficient buffer memory!\n";
00217   }
00218 
00219   _bufcount = req.count;
00220   _buffers = (void* *) calloc (req.count, sizeof (void*));
00221   _buflens = (size_t*) calloc (req.count, sizeof (size_t));
00222 
00223   if (!_buffers || !_buflens) {
00224     vision_cat.error() << "Not enough memory!\n";
00225   }
00226 
00227   // Set up the mmap buffers
00228   struct v4l2_buffer buf;
00229   for (int i = 0; i < _bufcount; ++i) {
00230     memset(&buf, 0, sizeof buf);
00231     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00232     buf.memory = V4L2_MEMORY_MMAP;
00233     buf.index = i;
00234 
00235     if (-1 == ioctl(_fd, VIDIOC_QUERYBUF, &buf)) {
00236       vision_cat.error() << "Failed to query buffer!\n";
00237     }
00238 
00239     _buflens[i] = buf.length;
00240     _buffers[i] = mmap (NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, _fd, buf.m.offset);
00241 
00242     if (_buffers[i] == MAP_FAILED) {
00243       vision_cat.error() << "Failed to map buffer!\n";
00244     }
00245 
00246     if (-1 == ioctl(_fd, VIDIOC_QBUF, &buf)) {
00247       vision_cat.error() << "Failed to exchange buffer with driver!\n";
00248     }
00249   }
00250 
00251   enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00252   if (-1 == ioctl(_fd, VIDIOC_STREAMON, &type)) {
00253     vision_cat.error() << "Failed to stream from buffer!\n";
00254   }
00255 
00256 #ifdef HAVE_JPEG
00257   // Initialize the JPEG library, if necessary
00258   if (_format->fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
00259     _cinfo = (struct jpeg_decompress_struct *) malloc(sizeof(struct jpeg_decompress_struct));
00260     jpeg_create_decompress(_cinfo);
00261 
00262     _cinfo->src = (struct jpeg_source_mgr *)
00263       (*_cinfo->mem->alloc_small) ((j_common_ptr) _cinfo, JPOOL_PERMANENT,
00264                                           sizeof(struct jpeg_source_mgr));
00265     // Set up function pointers
00266     _cinfo->src->init_source = my_init_source;
00267     _cinfo->src->fill_input_buffer = my_fill_input_buffer;
00268     _cinfo->src->skip_input_data = my_skip_input_data;
00269     _cinfo->src->resync_to_restart = jpeg_resync_to_restart;
00270     _cinfo->src->term_source = my_term_source;
00271   }
00272 #endif
00273   _ready = true;
00274 }
00275 
00276 ////////////////////////////////////////////////////////////////////
00277 //     Function: WebcamVideoCursorV4L::Destructor
00278 //       Access: Published, Virtual
00279 //  Description:
00280 ////////////////////////////////////////////////////////////////////
00281 WebcamVideoCursorV4L::
00282 ~WebcamVideoCursorV4L() {
00283 #ifdef HAVE_JPEG
00284   if (_cinfo != NULL) {
00285     jpeg_destroy_decompress(_cinfo);
00286     free(_cinfo);
00287   }
00288 #endif
00289   if (_format != NULL) {
00290     free(_format);
00291   }
00292   if (-1 != _fd) {
00293     enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00294     ioctl(_fd, VIDIOC_STREAMOFF, &type);
00295     close(_fd);
00296   }
00297   if (_buffers) {
00298     for (int i = 0; i < _bufcount; ++i) {
00299       munmap(_buffers[i], _buflens[i]);
00300     }
00301     free(_buffers);
00302   }
00303   if (_buflens) {
00304     free(_buflens);
00305   }
00306 }
00307 
00308 ////////////////////////////////////////////////////////////////////
00309 //     Function: WebcamVideoCursorV4L::fetch_buffer
00310 //       Access: Published, Virtual
00311 //  Description:
00312 ////////////////////////////////////////////////////////////////////
00313 PT(MovieVideoCursor::Buffer) WebcamVideoCursorV4L::
00314 fetch_buffer() {
00315   if (!_ready) {
00316     return NULL;
00317   }
00318 
00319   PT(Buffer) buffer = get_standard_buffer();
00320   unsigned char *block = buffer->_block;
00321   struct v4l2_buffer vbuf;
00322   memset(&vbuf, 0, sizeof vbuf);
00323   vbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00324   vbuf.memory = V4L2_MEMORY_MMAP;
00325   if (-1 == ioctl(_fd, VIDIOC_DQBUF, &vbuf) && errno != EIO) {
00326     vision_cat.error() << "Failed to dequeue buffer!\n";
00327     return NULL;
00328   }
00329   nassertr(vbuf.index < _bufcount, NULL);
00330   size_t bufsize = _buflens[vbuf.index];
00331   size_t old_bpl = _format->fmt.pix.bytesperline;
00332   size_t new_bpl = _size_x * 3;
00333   unsigned char *buf = (unsigned char *) _buffers[vbuf.index];
00334 
00335   if (_format->fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
00336 #ifdef HAVE_JPEG
00337     struct my_error_mgr jerr;
00338     _cinfo->err = jpeg_std_error(&jerr.pub);
00339     jerr.pub.error_exit = my_error_exit;
00340     jerr.pub.output_message = my_output_message;
00341 
00342     unsigned char *newbuf = (unsigned char*) malloc(new_bpl * _size_y);
00343 
00344     // Establish the setjmp return context for my_error_exit to use
00345     if (setjmp(jerr.setjmp_buffer)) {
00346       if (_cinfo->global_state > DSTATE_READY) {
00347         jpeg_abort_decompress(_cinfo);
00348       }
00349     } else {
00350       // Set up data pointer
00351       _cinfo->src->bytes_in_buffer = bufsize;
00352       _cinfo->src->next_input_byte = buf;
00353 
00354       if (jpeg_read_header(_cinfo, TRUE) == JPEG_HEADER_OK) {
00355         _cinfo->scale_num = 1;
00356         _cinfo->scale_denom = 1;
00357         _cinfo->out_color_space = JCS_RGB;
00358 
00359         if (jpeg_start_decompress(_cinfo) && _cinfo->output_components == 3
00360           && _size_x == _cinfo->output_width && _size_y == _cinfo->output_height) {
00361 
00362           JSAMPLE *buffer_end = newbuf + new_bpl * _cinfo->output_height;
00363           JSAMPLE *rowptr = newbuf;
00364           while (_cinfo->output_scanline < _cinfo->output_height) {
00365             nassertd(rowptr + new_bpl <= buffer_end) break;
00366             jpeg_read_scanlines(_cinfo, &rowptr, 1);
00367             rowptr += new_bpl;
00368           }
00369 
00370           if (_cinfo->output_scanline < _cinfo->output_height) {
00371             jpeg_abort_decompress(_cinfo);
00372           } else {
00373             jpeg_finish_decompress(_cinfo);
00374           }
00375         }
00376       }
00377     }
00378 
00379     // Flip the image vertically
00380     for (size_t row = 0; row < _size_y; ++row) {
00381       memcpy(block + (_size_y - row - 1) * new_bpl, newbuf + row * new_bpl, new_bpl);
00382     }
00383     free(newbuf);
00384 
00385     // Swap red / blue
00386     unsigned char ex;
00387     for (size_t i = 0; i < new_bpl * _size_y; i += 3) {
00388       ex = block[i];
00389       block[i] = block[i + 2];
00390       block[i + 2] = ex;
00391     }
00392 #else
00393     nassertr(false, NULL); // Not compiled with JPEG support
00394 #endif
00395   } else {
00396     for (size_t row = 0; row < _size_y; ++row) {
00397       size_t c = 0;
00398       for (size_t i = 0; i < old_bpl; i += 4) {
00399         yuyv_to_rgbrgb(block + (_size_y - row - 1) * new_bpl + c, buf + row * old_bpl + i);
00400         c += 6;
00401       }
00402     }
00403   }
00404 
00405   if (-1 == ioctl(_fd, VIDIOC_QBUF, &vbuf)) {
00406     vision_cat.error() << "Failed to exchange buffer with driver!\n";
00407   }
00408 
00409   return buffer;
00410 }
00411 
00412 #endif
 All Classes Functions Variables Enumerations