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