Panda3D
|
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