Panda3D

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