Panda3D

webcamVideoCursorV4L.cxx
Go to the documentation of this file.
1 /**
2 * PANDA 3D SOFTWARE
3 * Copyright (c) Carnegie Mellon University. All rights reserved.
4 *
5 * All use of this software is subject to the terms of the revised BSD
6 * license. You should have received a copy of this license along
7 * with this source code in a file named "LICENSE."
8 *
9 * @file webcamVideoCursorV4L.cxx
10 * @author rdb
11 * @date 2010-06-11
12 */
13
14 #include "webcamVideoCursorV4L.h"
15
16 #include "config_vision.h"
17 #include "webcamVideoV4L.h"
18
19 #include "movieVideoCursor.h"
20
21 #if defined(HAVE_VIDEO4LINUX) && !defined(CPPPARSER)
22
23 #include <fcntl.h>
24 #include <sys/mman.h>
25 #include <sys/ioctl.h>
26
27 #ifdef HAVE_JPEG
28 extern "C" {
29 #include <jpeglib.h>
30 #include <jerror.h>
31 }
32
33 #include <setjmp.h>
34 #endif
35
36 TypeHandle WebcamVideoCursorV4L::_type_handle;
37
38 #define clamp(x) std::min(std::max(x, 0.0), 255.0)
39
40 INLINE static void yuv_to_bgr(unsigned char *dest, const unsigned char *src) {
41 double y1 = (255 / 219.0) * (src[0] - 16);
42 double pb = (255 / 224.0) * (src[1] - 128);
43 double pr = (255 / 224.0) * (src[2] - 128);
44 dest[2] = clamp(1.0 * y1 + 0 * pb + 1.402 * pr);
45 dest[1] = clamp(1.0 * y1 - 0.344 * pb - 0.714 * pr);
46 dest[0] = clamp(1.0 * y1 + 1.772 * pb + 0 * pr);
47 }
48
49 INLINE static void yuyv_to_bgrbgr(unsigned char *dest, const unsigned char *src) {
50 unsigned char yuv[] = {src[0], src[1], src[3]};
51 yuv_to_bgr(dest, yuv);
52 yuv[0] = src[2];
53 yuv_to_bgr(dest + 3, yuv);
54 }
55
56 INLINE static void yuyv_to_bgrabgra(unsigned char *dest, const unsigned char *src) {
57 unsigned char yuv[] = {src[0], src[1], src[3]};
58 yuv_to_bgr(dest, yuv);
59 yuv[0] = src[2];
60 yuv_to_bgr(dest + 4, yuv);
61 dest[3] = 0xff;
62 dest[7] = 0xff;
63 }
64
65 INLINE static void rgb_to_bgr(unsigned char *dest, const unsigned char *src) {
66 dest[0] = src[2];
67 dest[1] = src[1];
68 dest[2] = src[0];
69 }
70
71 INLINE static void rgb_to_bgra(unsigned char *dest, const unsigned char *src) {
72 dest[0] = src[2];
73 dest[1] = src[1];
74 dest[2] = src[0];
75 dest[3] = 0xff;
76 }
77
78 #if defined(HAVE_JPEG) && !defined(CPPPARSER)
79
80 struct my_error_mgr {
81 struct jpeg_error_mgr pub;
82 jmp_buf setjmp_buffer;
83 };
84
85 typedef struct my_error_mgr *my_error_ptr;
86
87 static void my_error_exit (j_common_ptr cinfo) {
88 // Output the error message
89 char buffer[JMSG_LENGTH_MAX];
90 (*cinfo->err->format_message) (cinfo, buffer);
91 vision_cat.error() << buffer << "\n";
92 // cinfo->err really points to a my_error_mgr struct, so coerce pointer
93 my_error_ptr myerr = (my_error_ptr) cinfo->err;
94 // Return control to the setjmp point
95 longjmp(myerr->setjmp_buffer, 1);
96 }
97
98 static void my_output_message (j_common_ptr cinfo){
99 char buffer[JMSG_LENGTH_MAX];
100 (*cinfo->err->format_message) (cinfo, buffer);
101 vision_cat.warning() << buffer << "\n";
102 }
103
104 static void my_init_source(j_decompress_ptr cinfo) {
105 }
106
107 static boolean my_fill_input_buffer(j_decompress_ptr cinfo) {
108 struct jpeg_source_mgr *src = cinfo->src;
109 static JOCTET FakeEOI[] = {0xFF, JPEG_EOI};
110
111 WARNMS(cinfo, JWRN_JPEG_EOF);
112
113 src->next_input_byte = FakeEOI;
114 src->bytes_in_buffer = 2;
115
116 return TRUE;
117 }
118
119 static void my_skip_input_data(j_decompress_ptr cinfo, long num_bytes) {
120 struct jpeg_source_mgr *src = cinfo->src;
121
122 if (num_bytes >= (long) src->bytes_in_buffer) {
123 my_fill_input_buffer(cinfo);
124 return;
125 }
126
127 src->bytes_in_buffer -= num_bytes;
128 src->next_input_byte += num_bytes;
129 }
130
131 static void my_term_source(j_decompress_ptr cinfo) {
132 }
133
134 // Huffman tables used for MJPEG streams that omit them.
135 static JHUFF_TBL dc_luminance_tbl = {
136 {0, 0, 1, 5, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0},
137 {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11},
138 FALSE
139 };
140
141 static JHUFF_TBL dc_chrominance_tbl = {
142 {0, 0, 3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0},
143 {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11},
144 FALSE
145 };
146
147 static JHUFF_TBL ac_luminance_tbl = {
148 {0, 0, 2, 1, 3, 3, 2, 4, 3, 5, 5, 4, 4, 0, 0, 1, 0x7d},
149 {
150 0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12, 0x21,
151 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07, 0x22, 0x71,
152 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08, 0x23, 0x42, 0xb1,
153 0xc1, 0x15, 0x52, 0xd1, 0xf0, 0x24, 0x33, 0x62, 0x72,
154 0x82, 0x09, 0x0a, 0x16, 0x17, 0x18, 0x19, 0x1a, 0x25,
155 0x26, 0x27, 0x28, 0x29, 0x2a, 0x34, 0x35, 0x36, 0x37,
156 0x38, 0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48,
157 0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59,
158 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6a,
159 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7a, 0x83,
160 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8a, 0x92, 0x93,
161 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3,
162 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3,
163 0xb4, 0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3,
164 0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3,
165 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2,
166 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea, 0xf1,
167 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa
168 },
169 FALSE
170 };
171
172 static JHUFF_TBL ac_chrominance_tbl = {
173 {0, 0, 2, 1, 2, 4, 4, 3, 4, 7, 5, 4, 4, 0, 1, 2, 0x77},
174 {
175 0x00, 0x01, 0x02, 0x03, 0x11, 0x04, 0x05, 0x21, 0x31,
176 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71, 0x13, 0x22,
177 0x32, 0x81, 0x08, 0x14, 0x42, 0x91, 0xa1, 0xb1, 0xc1,
178 0x09, 0x23, 0x33, 0x52, 0xf0, 0x15, 0x62, 0x72, 0xd1,
179 0x0a, 0x16, 0x24, 0x34, 0xe1, 0x25, 0xf1, 0x17, 0x18,
180 0x19, 0x1a, 0x26, 0x27, 0x28, 0x29, 0x2a, 0x35, 0x36,
181 0x37, 0x38, 0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47,
182 0x48, 0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58,
183 0x59, 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69,
184 0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7a,
185 0x82, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8a,
186 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a,
187 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa,
188 0xb2, 0xb3, 0xb4, 0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba,
189 0xc2, 0xc3, 0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca,
190 0xd2, 0xd3, 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda,
191 0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea,
192 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa
193 },
194 FALSE
195 };
196
197 #endif
198
199 /**
200 *
201 */
202 WebcamVideoCursorV4L::
203 WebcamVideoCursorV4L(WebcamVideoV4L *src) : MovieVideoCursor(src) {
204 _size_x = src->_size_x;
205 _size_y = src->_size_y;
206 _num_components = 3;
207 _length = 1.0E10;
208 _can_seek = false;
209 _can_seek_fast = false;
210 _aborted = false;
211 _streaming = true;
212 _ready = false;
213 memset(&_format, 0, sizeof(struct v4l2_format));
214
215 _buffers = nullptr;
216 _buflens = nullptr;
217
218 int mode = O_RDWR;
219 if (!v4l_blocking) {
220 mode = O_NONBLOCK;
221 }
222
223 _fd = open(src->_device.c_str(), mode);
224 if (-1 == _fd) {
225 vision_cat.error() << "Failed to open " << src->_device.c_str() << "\n";
226 return;
227 }
228
229 // Find the best format in our _pformats vector. MJPEG is preferred over
230 // YUYV, as it's much smaller.
231 _format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
232 _format.fmt.pix.pixelformat = src->_pformat;
233
234 switch (_format.fmt.pix.pixelformat) {
235 #ifdef HAVE_JPEG
236 case V4L2_PIX_FMT_MJPEG:
237 _num_components = 3;
238 break;
239 #endif
240
241 case V4L2_PIX_FMT_YUYV:
242 _num_components = 3;
243 break;
244
245 case V4L2_PIX_FMT_BGR24:
246 _num_components = 3;
247 break;
248
249 case V4L2_PIX_FMT_BGR32:
250 _num_components = 4;
251 break;
252
253 case V4L2_PIX_FMT_RGB24:
254 _num_components = 3;
255 break;
256
257 case V4L2_PIX_FMT_RGB32:
258 _num_components = 4;
259 break;
260
261 case V4L2_PIX_FMT_GREY:
262 _num_components = 1;
263 break;
264
265 default:
266 vision_cat.error() << "Unsupported pixel format " << src->get_pixel_format() << "!\n";
267 _ready = false;
268 close(_fd);
269 _fd = -1;
270 return;
271 }
272
273 // Request a format of this size, and no interlacing
274 _format.fmt.pix.width = _size_x;
275 _format.fmt.pix.height = _size_y;
276 _format.fmt.pix.field = V4L2_FIELD_NONE;
277
278 // Now politely ask the driver to switch to this format
279 if (-1 == ioctl(_fd, VIDIOC_S_FMT, &_format)) {
280 vision_cat.error() << "Driver rejected format!\n";
281 _ready = false;
282 close(_fd);
283 _fd = -1;
284 return;
285 }
286
287 _size_x = _format.fmt.pix.width;
288 _size_y = _format.fmt.pix.height;
289
290 struct v4l2_streamparm streamparm;
291 memset(&streamparm, 0, sizeof streamparm);
292 streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
293 streamparm.parm.capture.timeperframe.numerator = 1;
294 streamparm.parm.capture.timeperframe.denominator = src->_fps;
295 if (ioctl(_fd, VIDIOC_S_PARM, &streamparm) < 0) {
296 vision_cat.error() << "Driver rejected framerate!\n";
297 }
298
299 struct v4l2_requestbuffers req;
300 memset(&req, 0, sizeof req);
301 req.count = 4;
302 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
303 req.memory = V4L2_MEMORY_MMAP;
304
305 if (-1 == ioctl (_fd, VIDIOC_REQBUFS, &req)) {
306 vision_cat.error() << "Failed to request buffers from webcam!\n";
307 }
308
309 if (req.count < 2) {
310 vision_cat.error() << "Insufficient buffer memory!\n";
311 }
312
313 _bufcount = req.count;
314 _buffers = (void **) calloc (req.count, sizeof (void*));
315 _buflens = (size_t*) calloc (req.count, sizeof (size_t));
316
317 if (!_buffers || !_buflens) {
318 vision_cat.error() << "Not enough memory!\n";
319 }
320
321 // Set up the mmap buffers
322 struct v4l2_buffer buf;
323 for (unsigned int i = 0; i < (unsigned int)_bufcount; ++i) {
324 memset(&buf, 0, sizeof buf);
325 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
326 buf.memory = V4L2_MEMORY_MMAP;
327 buf.index = i;
328
329 if (-1 == ioctl(_fd, VIDIOC_QUERYBUF, &buf)) {
330 vision_cat.error() << "Failed to query buffer!\n";
331 }
332
333 _buflens[i] = buf.length;
334 _buffers[i] = mmap (nullptr, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, _fd, buf.m.offset);
335
336 if (_buffers[i] == MAP_FAILED) {
337 vision_cat.error() << "Failed to map buffer!\n";
338 }
339
340 if (-1 == ioctl(_fd, VIDIOC_QBUF, &buf)) {
341 vision_cat.error() << "Failed to exchange buffer with driver!\n";
342 }
343 }
344
345 enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
346 if (-1 == ioctl(_fd, VIDIOC_STREAMON, &type)) {
347 vision_cat.error() << "Failed to stream from buffer!\n";
348 }
349
350 #ifdef HAVE_JPEG
351 // Initialize the JPEG library, if necessary
352 if (_format.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
353 jpeg_create_decompress(&_cinfo);
354
355 _cinfo.src = (struct jpeg_source_mgr *)
356 (*_cinfo.mem->alloc_small) ((j_common_ptr) &_cinfo, JPOOL_PERMANENT,
357 sizeof(struct jpeg_source_mgr));
358 // Set up function pointers
359 _cinfo.src->init_source = my_init_source;
360 _cinfo.src->fill_input_buffer = my_fill_input_buffer;
361 _cinfo.src->skip_input_data = my_skip_input_data;
362 _cinfo.src->resync_to_restart = jpeg_resync_to_restart;
363 _cinfo.src->term_source = my_term_source;
364 }
365 #endif
366 _ready = true;
367 }
368
369 /**
370 *
371 */
372 WebcamVideoCursorV4L::
373 ~WebcamVideoCursorV4L() {
374 #ifdef HAVE_JPEG
375 if (_format.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
376 jpeg_destroy_decompress(&_cinfo);
377 }
378 #endif
379 if (-1 != _fd) {
380 enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
381 ioctl(_fd, VIDIOC_STREAMOFF, &type);
382 close(_fd);
383 }
384 if (_buffers) {
385 for (unsigned int i = 0; i < (unsigned int)_bufcount; ++i) {
386 munmap(_buffers[i], _buflens[i]);
387 }
388 free(_buffers);
389 }
390 if (_buflens) {
391 free(_buflens);
392 }
393 }
394
395 /**
396 *
397 */
398 PT(MovieVideoCursor::Buffer) WebcamVideoCursorV4L::
399 fetch_buffer() {
400 if (!_ready) {
401 return nullptr;
402 }
403
404 PT(Buffer) buffer = get_standard_buffer();
405 unsigned char *block = buffer->_block;
406 struct v4l2_buffer vbuf;
407 memset(&vbuf, 0, sizeof vbuf);
408 vbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
409 vbuf.memory = V4L2_MEMORY_MMAP;
410 if (-1 == ioctl(_fd, VIDIOC_DQBUF, &vbuf) && errno != EIO) {
411 if (errno == EAGAIN) {
412 // Simply nothing is available yet.
413 return nullptr;
414 }
415 vision_cat.error() << "Failed to dequeue buffer!\n";
416 return nullptr;
417 }
418 nassertr(vbuf.index < _bufcount, nullptr);
419 size_t bufsize = _buflens[vbuf.index];
420 size_t old_bpl = _format.fmt.pix.bytesperline;
421 size_t new_bpl = _size_x * _num_components;
422 unsigned char *buf = (unsigned char *) _buffers[vbuf.index];
423
424 switch (_format.fmt.pix.pixelformat) {
425 case V4L2_PIX_FMT_MJPEG: {
426 #ifdef HAVE_JPEG
427 struct my_error_mgr jerr;
428 _cinfo.err = jpeg_std_error(&jerr.pub);
429 jerr.pub.error_exit = my_error_exit;
430 jerr.pub.output_message = my_output_message;
431
432 unsigned char *newbuf = (unsigned char*) malloc(new_bpl * _size_y);
433
434 // Establish the setjmp return context for my_error_exit to use
435 if (setjmp(jerr.setjmp_buffer)) {
436 jpeg_abort_decompress(&_cinfo);
437 } else {
438 // Set up data pointer
439 _cinfo.src->bytes_in_buffer = bufsize;
440 _cinfo.src->next_input_byte = buf;
441
442 if (jpeg_read_header(&_cinfo, TRUE) == JPEG_HEADER_OK) {
443 if (_cinfo.dc_huff_tbl_ptrs[0] == nullptr) {
444 // Many MJPEG streams do not include huffman tables. Remedy this.
445 _cinfo.dc_huff_tbl_ptrs[0] = &dc_luminance_tbl;
446 _cinfo.dc_huff_tbl_ptrs[1] = &dc_chrominance_tbl;
447 _cinfo.ac_huff_tbl_ptrs[0] = &ac_luminance_tbl;
448 _cinfo.ac_huff_tbl_ptrs[1] = &ac_chrominance_tbl;
449 }
450
451 _cinfo.scale_num = 1;
452 _cinfo.scale_denom = 1;
453 _cinfo.out_color_space = JCS_RGB;
454 _cinfo.dct_method = JDCT_IFAST;
455
456 if (jpeg_start_decompress(&_cinfo) && _cinfo.output_components == 3
457 && _size_x == _cinfo.output_width && _size_y == _cinfo.output_height) {
458
459 JSAMPLE *buffer_end = newbuf + new_bpl * _cinfo.output_height;
460 JSAMPLE *rowptr = newbuf;
461 while (_cinfo.output_scanline < _cinfo.output_height) {
462 nassertd(rowptr + new_bpl <= buffer_end) break;
463 jpeg_read_scanlines(&_cinfo, &rowptr, _cinfo.output_height);
464 rowptr += new_bpl;
465 }
466
467 if (_cinfo.output_scanline < _cinfo.output_height) {
468 jpeg_abort_decompress(&_cinfo);
469 } else {
470 jpeg_finish_decompress(&_cinfo);
471 }
472 }
473 }
474 }
475
476 // Flip the image vertically
477 for (int row = 0; row < _size_y; ++row) {
478 memcpy(block + (_size_y - row - 1) * new_bpl, newbuf + row * new_bpl, new_bpl);
479 }
480 free(newbuf);
481
482 // Swap red blue
483 unsigned char ex;
484 for (size_t i = 0; i < new_bpl * _size_y; i += 3) {
485 ex = block[i];
486 block[i] = block[i + 2];
487 block[i + 2] = ex;
488 }
489 #else
490 nassert_raise("JPEG support not compiled-in");
491 return nullptr;
492 #endif
493 break;
494 }
495 case V4L2_PIX_FMT_YUYV:
496 for (size_t row = 0; row < _size_y; ++row) {
497 size_t c = 0;
498 for (size_t i = 0; i < old_bpl; i += 4) {
499 yuyv_to_bgrbgr(block + (_size_y - row - 1) * new_bpl + c, buf + row * old_bpl + i);
500 c += 6;
501 }
502 }
503 break;
504
505 case V4L2_PIX_FMT_BGR24:
506 case V4L2_PIX_FMT_BGR32:
507 case V4L2_PIX_FMT_GREY:
508 // Simplest case: copying every row verbatim.
509 nassertr(old_bpl == new_bpl, nullptr);
510
511 for (size_t row = 0; row < _size_y; ++row) {
512 memcpy(block + (_size_y - row - 1) * new_bpl, buf + row * old_bpl, new_bpl);
513 }
514 break;
515
516 case V4L2_PIX_FMT_RGB24:
517 // Swap components.
518 nassertr(old_bpl == new_bpl, nullptr);
519
520 for (size_t row = 0; row < _size_y; ++row) {
521 for (size_t i = 0; i < old_bpl; i += 3) {
522 rgb_to_bgr(block + (_size_y - row - 1) * old_bpl + i, buf + row * old_bpl + i);
523 }
524 }
525 break;
526
527 case V4L2_PIX_FMT_RGB32:
528 // Swap components.
529 nassertr(old_bpl == new_bpl, nullptr);
530
531 for (size_t row = 0; row < _size_y; ++row) {
532 for (size_t i = 0; i < old_bpl; i += 4) {
533 rgb_to_bgra(block + (_size_y - row - 1) * old_bpl + i, buf + row * old_bpl + i + 1);
534 }
535 }
536 break;
537 }
538
539 if (-1 == ioctl(_fd, VIDIOC_QBUF, &vbuf)) {
540 vision_cat.error() << "Failed to exchange buffer with driver!\n";
541 }
542
543 return buffer;
544 }
545
546 #endif
Buffer
Definition: buffer.h:24
MovieVideoCursor::Buffer
Definition: movieVideoCursor.h:62
MovieVideoCursor
A MovieVideo is actually any source that provides a sequence of video frames.
Definition: movieVideoCursor.h:41
TypeHandle
TypeHandle is the identifier used to differentiate C++ class types.
Definition: typeHandle.h:81
config_vision.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
movieVideoCursor.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
webcamVideoCursorV4L.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
webcamVideoV4L.h
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.