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