usb_cam.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, Robert Bosch LLC.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Robert Bosch nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  *********************************************************************/
36 #define __STDC_CONSTANT_MACROS
37 #include <stdio.h>
38 #include <stdlib.h>
39 #include <string.h>
40 #include <assert.h>
41 #include <fcntl.h> /* low-level i/o */
42 #include <unistd.h>
43 #include <errno.h>
44 #include <malloc.h>
45 #include <sys/stat.h>
46 #include <sys/types.h>
47 #include <sys/time.h>
48 #include <sys/mman.h>
49 #include <sys/ioctl.h>
50 
51 #include <ros/ros.h>
52 #include <boost/lexical_cast.hpp>
53 #include <sensor_msgs/fill_image.h>
54 
55 #include <usb_cam/usb_cam.h>
56 
57 #define CLEAR(x) memset (&(x), 0, sizeof (x))
58 
59 namespace usb_cam {
60 
61 static void errno_exit(const char * s)
62 {
63  ROS_ERROR("%s error %d, %s", s, errno, strerror(errno));
64  exit(EXIT_FAILURE);
65 }
66 
67 static int xioctl(int fd, int request, void * arg)
68 {
69  int r;
70 
71  do
72  r = ioctl(fd, request, arg);
73  while (-1 == r && EINTR == errno);
74 
75  return r;
76 }
77 
78 const unsigned char uchar_clipping_table[] = {
79  0,
80  0,
81  0,
82  0,
83  0,
84  0,
85  0,
86  0, // -128 - -121
87  0,
88  0,
89  0,
90  0,
91  0,
92  0,
93  0,
94  0, // -120 - -113
95  0,
96  0,
97  0,
98  0,
99  0,
100  0,
101  0,
102  0, // -112 - -105
103  0,
104  0,
105  0,
106  0,
107  0,
108  0,
109  0,
110  0, // -104 - -97
111  0,
112  0,
113  0,
114  0,
115  0,
116  0,
117  0,
118  0, // -96 - -89
119  0,
120  0,
121  0,
122  0,
123  0,
124  0,
125  0,
126  0, // -88 - -81
127  0,
128  0,
129  0,
130  0,
131  0,
132  0,
133  0,
134  0, // -80 - -73
135  0,
136  0,
137  0,
138  0,
139  0,
140  0,
141  0,
142  0, // -72 - -65
143  0,
144  0,
145  0,
146  0,
147  0,
148  0,
149  0,
150  0, // -64 - -57
151  0,
152  0,
153  0,
154  0,
155  0,
156  0,
157  0,
158  0, // -56 - -49
159  0,
160  0,
161  0,
162  0,
163  0,
164  0,
165  0,
166  0, // -48 - -41
167  0,
168  0,
169  0,
170  0,
171  0,
172  0,
173  0,
174  0, // -40 - -33
175  0,
176  0,
177  0,
178  0,
179  0,
180  0,
181  0,
182  0, // -32 - -25
183  0,
184  0,
185  0,
186  0,
187  0,
188  0,
189  0,
190  0, // -24 - -17
191  0,
192  0,
193  0,
194  0,
195  0,
196  0,
197  0,
198  0, // -16 - -9
199  0,
200  0,
201  0,
202  0,
203  0,
204  0,
205  0,
206  0, // -8 - -1
207  0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30,
208  31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59,
209  60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88,
210  89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113,
211  114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136,
212  137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159,
213  160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182,
214  183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205,
215  206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228,
216  229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251,
217  252, 253, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, // 256-263
218  255, 255, 255, 255, 255, 255, 255, 255, // 264-271
219  255, 255, 255, 255, 255, 255, 255, 255, // 272-279
220  255, 255, 255, 255, 255, 255, 255, 255, // 280-287
221  255, 255, 255, 255, 255, 255, 255, 255, // 288-295
222  255, 255, 255, 255, 255, 255, 255, 255, // 296-303
223  255, 255, 255, 255, 255, 255, 255, 255, // 304-311
224  255, 255, 255, 255, 255, 255, 255, 255, // 312-319
225  255, 255, 255, 255, 255, 255, 255, 255, // 320-327
226  255, 255, 255, 255, 255, 255, 255, 255, // 328-335
227  255, 255, 255, 255, 255, 255, 255, 255, // 336-343
228  255, 255, 255, 255, 255, 255, 255, 255, // 344-351
229  255, 255, 255, 255, 255, 255, 255, 255, // 352-359
230  255, 255, 255, 255, 255, 255, 255, 255, // 360-367
231  255, 255, 255, 255, 255, 255, 255, 255, // 368-375
232  255, 255, 255, 255, 255, 255, 255, 255, // 376-383
233  };
234 const int clipping_table_offset = 128;
235 
239 static unsigned char CLIPVALUE(int val)
240 {
241  // Old method (if)
242  /* val = val < 0 ? 0 : val; */
243  /* return val > 255 ? 255 : val; */
244 
245  // New method (array)
246  return uchar_clipping_table[val + clipping_table_offset];
247 }
248 
270 static void YUV2RGB(const unsigned char y, const unsigned char u, const unsigned char v, unsigned char* r,
271  unsigned char* g, unsigned char* b)
272 {
273  const int y2 = (int)y;
274  const int u2 = (int)u - 128;
275  const int v2 = (int)v - 128;
276  //std::cerr << "YUV=("<<y2<<","<<u2<<","<<v2<<")"<<std::endl;
277 
278  // This is the normal YUV conversion, but
279  // appears to be incorrect for the firewire cameras
280  // int r2 = y2 + ( (v2*91947) >> 16);
281  // int g2 = y2 - ( ((u2*22544) + (v2*46793)) >> 16 );
282  // int b2 = y2 + ( (u2*115999) >> 16);
283  // This is an adjusted version (UV spread out a bit)
284  int r2 = y2 + ((v2 * 37221) >> 15);
285  int g2 = y2 - (((u2 * 12975) + (v2 * 18949)) >> 15);
286  int b2 = y2 + ((u2 * 66883) >> 15);
287  //std::cerr << " RGB=("<<r2<<","<<g2<<","<<b2<<")"<<std::endl;
288 
289  // Cap the values.
290  *r = CLIPVALUE(r2);
291  *g = CLIPVALUE(g2);
292  *b = CLIPVALUE(b2);
293 }
294 
295 void uyvy2rgb(char *YUV, char *RGB, int NumPixels)
296 {
297  int i, j;
298  unsigned char y0, y1, u, v;
299  unsigned char r, g, b;
300  for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6)
301  {
302  u = (unsigned char)YUV[i + 0];
303  y0 = (unsigned char)YUV[i + 1];
304  v = (unsigned char)YUV[i + 2];
305  y1 = (unsigned char)YUV[i + 3];
306  YUV2RGB(y0, u, v, &r, &g, &b);
307  RGB[j + 0] = r;
308  RGB[j + 1] = g;
309  RGB[j + 2] = b;
310  YUV2RGB(y1, u, v, &r, &g, &b);
311  RGB[j + 3] = r;
312  RGB[j + 4] = g;
313  RGB[j + 5] = b;
314  }
315 }
316 
317 static void mono102mono8(char *RAW, char *MONO, int NumPixels)
318 {
319  int i, j;
320  for (i = 0, j = 0; i < (NumPixels << 1); i += 2, j += 1)
321  {
322  //first byte is low byte, second byte is high byte; smash together and convert to 8-bit
323  MONO[j] = (unsigned char)(((RAW[i + 0] >> 2) & 0x3F) | ((RAW[i + 1] << 6) & 0xC0));
324  }
325 }
326 
327 static void yuyv2rgb(char *YUV, char *RGB, int NumPixels)
328 {
329  int i, j;
330  unsigned char y0, y1, u, v;
331  unsigned char r, g, b;
332 
333  for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6)
334  {
335  y0 = (unsigned char)YUV[i + 0];
336  u = (unsigned char)YUV[i + 1];
337  y1 = (unsigned char)YUV[i + 2];
338  v = (unsigned char)YUV[i + 3];
339  YUV2RGB(y0, u, v, &r, &g, &b);
340  RGB[j + 0] = r;
341  RGB[j + 1] = g;
342  RGB[j + 2] = b;
343  YUV2RGB(y1, u, v, &r, &g, &b);
344  RGB[j + 3] = r;
345  RGB[j + 4] = g;
346  RGB[j + 5] = b;
347  }
348 }
349 
350 void rgb242rgb(char *YUV, char *RGB, int NumPixels)
351 {
352  memcpy(RGB, YUV, NumPixels * 3);
353 }
354 
355 
357  : io_(IO_METHOD_MMAP), fd_(-1), buffers_(NULL), n_buffers_(0), avframe_camera_(NULL),
358  avframe_rgb_(NULL), avcodec_(NULL), avoptions_(NULL), avcodec_context_(NULL),
359  avframe_camera_size_(0), avframe_rgb_size_(0), video_sws_(NULL), image_(NULL), is_capturing_(false) {
360 }
362 {
363  shutdown();
364 }
365 
366 int UsbCam::init_mjpeg_decoder(int image_width, int image_height)
367 {
368  avcodec_register_all();
369 
370  avcodec_ = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
371  if (!avcodec_)
372  {
373  ROS_ERROR("Could not find MJPEG decoder");
374  return 0;
375  }
376 
377  avcodec_context_ = avcodec_alloc_context3(avcodec_);
378 #if LIBAVCODEC_VERSION_MAJOR < 55
379  avframe_camera_ = avcodec_alloc_frame();
380  avframe_rgb_ = avcodec_alloc_frame();
381 #else
382  avframe_camera_ = av_frame_alloc();
383  avframe_rgb_ = av_frame_alloc();
384 #endif
385 
386  avpicture_alloc((AVPicture *)avframe_rgb_, AV_PIX_FMT_RGB24, image_width, image_height);
387 
389  avcodec_context_->width = image_width;
390  avcodec_context_->height = image_height;
391 
392 #if LIBAVCODEC_VERSION_MAJOR > 52
393  avcodec_context_->pix_fmt = AV_PIX_FMT_YUV422P;
394  avcodec_context_->codec_type = AVMEDIA_TYPE_VIDEO;
395 #endif
396 
397  avframe_camera_size_ = avpicture_get_size(AV_PIX_FMT_YUV422P, image_width, image_height);
398  avframe_rgb_size_ = avpicture_get_size(AV_PIX_FMT_RGB24, image_width, image_height);
399 
400  /* open it */
401  if (avcodec_open2(avcodec_context_, avcodec_, &avoptions_) < 0)
402  {
403  ROS_ERROR("Could not open MJPEG Decoder");
404  return 0;
405  }
406  return 1;
407 }
408 
409 void UsbCam::mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels)
410 {
411  int got_picture;
412 
413  memset(RGB, 0, avframe_rgb_size_);
414 
415 #if LIBAVCODEC_VERSION_MAJOR > 52
416  int decoded_len;
417  AVPacket avpkt;
418  av_init_packet(&avpkt);
419 
420  avpkt.size = len;
421  avpkt.data = (unsigned char*)MJPEG;
422  decoded_len = avcodec_decode_video2(avcodec_context_, avframe_camera_, &got_picture, &avpkt);
423 
424  if (decoded_len < 0)
425  {
426  ROS_ERROR("Error while decoding frame.");
427  return;
428  }
429 #else
430  avcodec_decode_video(avcodec_context_, avframe_camera_, &got_picture, (uint8_t *) MJPEG, len);
431 #endif
432 
433  if (!got_picture)
434  {
435  ROS_ERROR("Webcam: expected picture but didn't get it...");
436  return;
437  }
438 
439  int xsize = avcodec_context_->width;
440  int ysize = avcodec_context_->height;
441  int pic_size = avpicture_get_size(avcodec_context_->pix_fmt, xsize, ysize);
442  if (pic_size != avframe_camera_size_)
443  {
444  ROS_ERROR("outbuf size mismatch. pic_size: %d bufsize: %d", pic_size, avframe_camera_size_);
445  return;
446  }
447 
448  video_sws_ = sws_getContext(xsize, ysize, avcodec_context_->pix_fmt, xsize, ysize, AV_PIX_FMT_RGB24, SWS_BILINEAR, NULL,
449  NULL, NULL);
450  sws_scale(video_sws_, avframe_camera_->data, avframe_camera_->linesize, 0, ysize, avframe_rgb_->data,
451  avframe_rgb_->linesize);
452  sws_freeContext(video_sws_);
453 
454  int size = avpicture_layout((AVPicture *)avframe_rgb_, AV_PIX_FMT_RGB24, xsize, ysize, (uint8_t *)RGB, avframe_rgb_size_);
455  if (size != avframe_rgb_size_)
456  {
457  ROS_ERROR("webcam: avpicture_layout error: %d", size);
458  return;
459  }
460 }
461 
462 void UsbCam::process_image(const void * src, int len, camera_image_t *dest)
463 {
464  if (pixelformat_ == V4L2_PIX_FMT_YUYV)
465  {
466  if (monochrome_)
467  { //actually format V4L2_PIX_FMT_Y16, but xioctl gets unhappy if you don't use the advertised type (yuyv)
468  mono102mono8((char*)src, dest->image, dest->width * dest->height);
469  }
470  else
471  {
472  yuyv2rgb((char*)src, dest->image, dest->width * dest->height);
473  }
474  }
475  else if (pixelformat_ == V4L2_PIX_FMT_UYVY)
476  uyvy2rgb((char*)src, dest->image, dest->width * dest->height);
477  else if (pixelformat_ == V4L2_PIX_FMT_MJPEG)
478  mjpeg2rgb((char*)src, len, dest->image, dest->width * dest->height);
479  else if (pixelformat_ == V4L2_PIX_FMT_RGB24)
480  rgb242rgb((char*)src, dest->image, dest->width * dest->height);
481  else if (pixelformat_ == V4L2_PIX_FMT_GREY)
482  memcpy(dest->image, (char*)src, dest->width * dest->height);
483 }
484 
486 {
487  struct v4l2_buffer buf;
488  unsigned int i;
489  int len;
490 
491  switch (io_)
492  {
493  case IO_METHOD_READ:
494  len = read(fd_, buffers_[0].start, buffers_[0].length);
495  if (len == -1)
496  {
497  switch (errno)
498  {
499  case EAGAIN:
500  return 0;
501 
502  case EIO:
503  /* Could ignore EIO, see spec. */
504 
505  /* fall through */
506 
507  default:
508  errno_exit("read");
509  }
510  }
511 
512  process_image(buffers_[0].start, len, image_);
513 
514  break;
515 
516  case IO_METHOD_MMAP:
517  CLEAR(buf);
518 
519  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
520  buf.memory = V4L2_MEMORY_MMAP;
521 
522  if (-1 == xioctl(fd_, VIDIOC_DQBUF, &buf))
523  {
524  switch (errno)
525  {
526  case EAGAIN:
527  return 0;
528 
529  case EIO:
530  /* Could ignore EIO, see spec. */
531 
532  /* fall through */
533 
534  default:
535  errno_exit("VIDIOC_DQBUF");
536  }
537  }
538 
539  assert(buf.index < n_buffers_);
540  len = buf.bytesused;
541  process_image(buffers_[buf.index].start, len, image_);
542 
543  if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
544  errno_exit("VIDIOC_QBUF");
545 
546  break;
547 
548  case IO_METHOD_USERPTR:
549  CLEAR(buf);
550 
551  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
552  buf.memory = V4L2_MEMORY_USERPTR;
553 
554  if (-1 == xioctl(fd_, VIDIOC_DQBUF, &buf))
555  {
556  switch (errno)
557  {
558  case EAGAIN:
559  return 0;
560 
561  case EIO:
562  /* Could ignore EIO, see spec. */
563 
564  /* fall through */
565 
566  default:
567  errno_exit("VIDIOC_DQBUF");
568  }
569  }
570 
571  for (i = 0; i < n_buffers_; ++i)
572  if (buf.m.userptr == (unsigned long)buffers_[i].start && buf.length == buffers_[i].length)
573  break;
574 
575  assert(i < n_buffers_);
576  len = buf.bytesused;
577  process_image((void *)buf.m.userptr, len, image_);
578 
579  if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
580  errno_exit("VIDIOC_QBUF");
581 
582  break;
583  }
584 
585  return 1;
586 }
587 
589  return is_capturing_;
590 }
591 
593 {
594  if(!is_capturing_) return;
595 
596  is_capturing_ = false;
597  enum v4l2_buf_type type;
598 
599  switch (io_)
600  {
601  case IO_METHOD_READ:
602  /* Nothing to do. */
603  break;
604 
605  case IO_METHOD_MMAP:
606  case IO_METHOD_USERPTR:
607  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
608 
609  if (-1 == xioctl(fd_, VIDIOC_STREAMOFF, &type))
610  errno_exit("VIDIOC_STREAMOFF");
611 
612  break;
613  }
614 }
615 
617 {
618 
619  if(is_capturing_) return;
620 
621  unsigned int i;
622  enum v4l2_buf_type type;
623 
624  switch (io_)
625  {
626  case IO_METHOD_READ:
627  /* Nothing to do. */
628  break;
629 
630  case IO_METHOD_MMAP:
631  for (i = 0; i < n_buffers_; ++i)
632  {
633  struct v4l2_buffer buf;
634 
635  CLEAR(buf);
636 
637  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
638  buf.memory = V4L2_MEMORY_MMAP;
639  buf.index = i;
640 
641  if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
642  errno_exit("VIDIOC_QBUF");
643  }
644 
645  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
646 
647  if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type))
648  errno_exit("VIDIOC_STREAMON");
649 
650  break;
651 
652  case IO_METHOD_USERPTR:
653  for (i = 0; i < n_buffers_; ++i)
654  {
655  struct v4l2_buffer buf;
656 
657  CLEAR(buf);
658 
659  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
660  buf.memory = V4L2_MEMORY_USERPTR;
661  buf.index = i;
662  buf.m.userptr = (unsigned long)buffers_[i].start;
663  buf.length = buffers_[i].length;
664 
665  if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
666  errno_exit("VIDIOC_QBUF");
667  }
668 
669  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
670 
671  if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type))
672  errno_exit("VIDIOC_STREAMON");
673 
674  break;
675  }
676  is_capturing_ = true;
677 }
678 
680 {
681  unsigned int i;
682 
683  switch (io_)
684  {
685  case IO_METHOD_READ:
686  free(buffers_[0].start);
687  break;
688 
689  case IO_METHOD_MMAP:
690  for (i = 0; i < n_buffers_; ++i)
691  if (-1 == munmap(buffers_[i].start, buffers_[i].length))
692  errno_exit("munmap");
693  break;
694 
695  case IO_METHOD_USERPTR:
696  for (i = 0; i < n_buffers_; ++i)
697  free(buffers_[i].start);
698  break;
699  }
700 
701  free(buffers_);
702 }
703 
704 void UsbCam::init_read(unsigned int buffer_size)
705 {
706  buffers_ = (buffer*)calloc(1, sizeof(*buffers_));
707 
708  if (!buffers_)
709  {
710  ROS_ERROR("Out of memory");
711  exit(EXIT_FAILURE);
712  }
713 
714  buffers_[0].length = buffer_size;
715  buffers_[0].start = malloc(buffer_size);
716 
717  if (!buffers_[0].start)
718  {
719  ROS_ERROR("Out of memory");
720  exit(EXIT_FAILURE);
721  }
722 }
723 
725 {
726  struct v4l2_requestbuffers req;
727 
728  CLEAR(req);
729 
730  req.count = 4;
731  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
732  req.memory = V4L2_MEMORY_MMAP;
733 
734  if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &req))
735  {
736  if (EINVAL == errno)
737  {
738  ROS_ERROR_STREAM(camera_dev_ << " does not support memory mapping");
739  exit(EXIT_FAILURE);
740  }
741  else
742  {
743  errno_exit("VIDIOC_REQBUFS");
744  }
745  }
746 
747  if (req.count < 2)
748  {
749  ROS_ERROR_STREAM("Insufficient buffer memory on " << camera_dev_);
750  exit(EXIT_FAILURE);
751  }
752 
753  buffers_ = (buffer*)calloc(req.count, sizeof(*buffers_));
754 
755  if (!buffers_)
756  {
757  ROS_ERROR("Out of memory");
758  exit(EXIT_FAILURE);
759  }
760 
761  for (n_buffers_ = 0; n_buffers_ < req.count; ++n_buffers_)
762  {
763  struct v4l2_buffer buf;
764 
765  CLEAR(buf);
766 
767  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
768  buf.memory = V4L2_MEMORY_MMAP;
769  buf.index = n_buffers_;
770 
771  if (-1 == xioctl(fd_, VIDIOC_QUERYBUF, &buf))
772  errno_exit("VIDIOC_QUERYBUF");
773 
774  buffers_[n_buffers_].length = buf.length;
775  buffers_[n_buffers_].start = mmap(NULL /* start anywhere */, buf.length, PROT_READ | PROT_WRITE /* required */,
776  MAP_SHARED /* recommended */,
777  fd_, buf.m.offset);
778 
779  if (MAP_FAILED == buffers_[n_buffers_].start)
780  errno_exit("mmap");
781  }
782 }
783 
784 void UsbCam::init_userp(unsigned int buffer_size)
785 {
786  struct v4l2_requestbuffers req;
787  unsigned int page_size;
788 
789  page_size = getpagesize();
790  buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
791 
792  CLEAR(req);
793 
794  req.count = 4;
795  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
796  req.memory = V4L2_MEMORY_USERPTR;
797 
798  if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &req))
799  {
800  if (EINVAL == errno)
801  {
802  ROS_ERROR_STREAM(camera_dev_ << " does not support "
803  "user pointer i/o");
804  exit(EXIT_FAILURE);
805  }
806  else
807  {
808  errno_exit("VIDIOC_REQBUFS");
809  }
810  }
811 
812  buffers_ = (buffer*)calloc(4, sizeof(*buffers_));
813 
814  if (!buffers_)
815  {
816  ROS_ERROR("Out of memory");
817  exit(EXIT_FAILURE);
818  }
819 
820  for (n_buffers_ = 0; n_buffers_ < 4; ++n_buffers_)
821  {
822  buffers_[n_buffers_].length = buffer_size;
823  buffers_[n_buffers_].start = memalign(/* boundary */page_size, buffer_size);
824 
825  if (!buffers_[n_buffers_].start)
826  {
827  ROS_ERROR("Out of memory");
828  exit(EXIT_FAILURE);
829  }
830  }
831 }
832 
833 void UsbCam::init_device(int image_width, int image_height, int framerate)
834 {
835  struct v4l2_capability cap;
836  struct v4l2_cropcap cropcap;
837  struct v4l2_crop crop;
838  struct v4l2_format fmt;
839  unsigned int min;
840 
841  if (-1 == xioctl(fd_, VIDIOC_QUERYCAP, &cap))
842  {
843  if (EINVAL == errno)
844  {
845  ROS_ERROR_STREAM(camera_dev_ << " is no V4L2 device");
846  exit(EXIT_FAILURE);
847  }
848  else
849  {
850  errno_exit("VIDIOC_QUERYCAP");
851  }
852  }
853 
854  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
855  {
856  ROS_ERROR_STREAM(camera_dev_ << " is no video capture device");
857  exit(EXIT_FAILURE);
858  }
859 
860  switch (io_)
861  {
862  case IO_METHOD_READ:
863  if (!(cap.capabilities & V4L2_CAP_READWRITE))
864  {
865  ROS_ERROR_STREAM(camera_dev_ << " does not support read i/o");
866  exit(EXIT_FAILURE);
867  }
868 
869  break;
870 
871  case IO_METHOD_MMAP:
872  case IO_METHOD_USERPTR:
873  if (!(cap.capabilities & V4L2_CAP_STREAMING))
874  {
875  ROS_ERROR_STREAM(camera_dev_ << " does not support streaming i/o");
876  exit(EXIT_FAILURE);
877  }
878 
879  break;
880  }
881 
882  /* Select video input, video standard and tune here. */
883 
884  CLEAR(cropcap);
885 
886  cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
887 
888  if (0 == xioctl(fd_, VIDIOC_CROPCAP, &cropcap))
889  {
890  crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
891  crop.c = cropcap.defrect; /* reset to default */
892 
893  if (-1 == xioctl(fd_, VIDIOC_S_CROP, &crop))
894  {
895  switch (errno)
896  {
897  case EINVAL:
898  /* Cropping not supported. */
899  break;
900  default:
901  /* Errors ignored. */
902  break;
903  }
904  }
905  }
906  else
907  {
908  /* Errors ignored. */
909  }
910 
911  CLEAR(fmt);
912 
913 // fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
914 // fmt.fmt.pix.width = 640;
915 // fmt.fmt.pix.height = 480;
916 // fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
917 // fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
918 
919  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
920  fmt.fmt.pix.width = image_width;
921  fmt.fmt.pix.height = image_height;
922  fmt.fmt.pix.pixelformat = pixelformat_;
923  fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
924 
925  if (-1 == xioctl(fd_, VIDIOC_S_FMT, &fmt))
926  errno_exit("VIDIOC_S_FMT");
927 
928  /* Note VIDIOC_S_FMT may change width and height. */
929 
930  /* Buggy driver paranoia. */
931  min = fmt.fmt.pix.width * 2;
932  if (fmt.fmt.pix.bytesperline < min)
933  fmt.fmt.pix.bytesperline = min;
934  min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
935  if (fmt.fmt.pix.sizeimage < min)
936  fmt.fmt.pix.sizeimage = min;
937 
938  image_width = fmt.fmt.pix.width;
939  image_height = fmt.fmt.pix.height;
940 
941  struct v4l2_streamparm stream_params;
942  memset(&stream_params, 0, sizeof(stream_params));
943  stream_params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
944  if (xioctl(fd_, VIDIOC_G_PARM, &stream_params) < 0)
945  errno_exit("Couldn't query v4l fps!");
946 
947  ROS_DEBUG("Capability flag: 0x%x", stream_params.parm.capture.capability);
948 
949  stream_params.parm.capture.timeperframe.numerator = 1;
950  stream_params.parm.capture.timeperframe.denominator = framerate;
951  if (xioctl(fd_, VIDIOC_S_PARM, &stream_params) < 0)
952  ROS_WARN("Couldn't set camera framerate");
953  else
954  ROS_DEBUG("Set framerate to be %i", framerate);
955 
956  switch (io_)
957  {
958  case IO_METHOD_READ:
959  init_read(fmt.fmt.pix.sizeimage);
960  break;
961 
962  case IO_METHOD_MMAP:
963  init_mmap();
964  break;
965 
966  case IO_METHOD_USERPTR:
967  init_userp(fmt.fmt.pix.sizeimage);
968  break;
969  }
970 }
971 
973 {
974  if (-1 == close(fd_))
975  errno_exit("close");
976 
977  fd_ = -1;
978 }
979 
981 {
982  struct stat st;
983 
984  if (-1 == stat(camera_dev_.c_str(), &st))
985  {
986  ROS_ERROR_STREAM("Cannot identify '" << camera_dev_ << "': " << errno << ", " << strerror(errno));
987  exit(EXIT_FAILURE);
988  }
989 
990  if (!S_ISCHR(st.st_mode))
991  {
992  ROS_ERROR_STREAM(camera_dev_ << " is no device");
993  exit(EXIT_FAILURE);
994  }
995 
996  fd_ = open(camera_dev_.c_str(), O_RDWR /* required */| O_NONBLOCK, 0);
997 
998  if (-1 == fd_)
999  {
1000  ROS_ERROR_STREAM("Cannot open '" << camera_dev_ << "': " << errno << ", " << strerror(errno));
1001  exit(EXIT_FAILURE);
1002  }
1003 }
1004 
1005 void UsbCam::start(const std::string& dev, io_method io_method,
1006  pixel_format pixel_format, int image_width, int image_height,
1007  int framerate)
1008 {
1009  camera_dev_ = dev;
1010 
1011  io_ = io_method;
1012  monochrome_ = false;
1013  if (pixel_format == PIXEL_FORMAT_YUYV)
1014  pixelformat_ = V4L2_PIX_FMT_YUYV;
1015  else if (pixel_format == PIXEL_FORMAT_UYVY)
1016  pixelformat_ = V4L2_PIX_FMT_UYVY;
1017  else if (pixel_format == PIXEL_FORMAT_MJPEG)
1018  {
1019  pixelformat_ = V4L2_PIX_FMT_MJPEG;
1020  init_mjpeg_decoder(image_width, image_height);
1021  }
1022  else if (pixel_format == PIXEL_FORMAT_YUVMONO10)
1023  {
1024  //actually format V4L2_PIX_FMT_Y16 (10-bit mono expresed as 16-bit pixels), but we need to use the advertised type (yuyv)
1025  pixelformat_ = V4L2_PIX_FMT_YUYV;
1026  monochrome_ = true;
1027  }
1028  else if (pixel_format == PIXEL_FORMAT_RGB24)
1029  {
1030  pixelformat_ = V4L2_PIX_FMT_RGB24;
1031  }
1032  else if (pixel_format == PIXEL_FORMAT_GREY)
1033  {
1034  pixelformat_ = V4L2_PIX_FMT_GREY;
1035  monochrome_ = true;
1036  }
1037  else
1038  {
1039  ROS_ERROR("Unknown pixel format.");
1040  exit(EXIT_FAILURE);
1041  }
1042 
1043  open_device();
1044  init_device(image_width, image_height, framerate);
1045  start_capturing();
1046 
1047  image_ = (camera_image_t *)calloc(1, sizeof(camera_image_t));
1048 
1049  image_->width = image_width;
1050  image_->height = image_height;
1051  image_->bytes_per_pixel = 3; //corrected 11/10/15 (BYTES not BITS per pixel)
1052 
1054  image_->is_new = 0;
1055  image_->image = (char *)calloc(image_->image_size, sizeof(char));
1056  memset(image_->image, 0, image_->image_size * sizeof(char));
1057 }
1058 
1060 {
1061  stop_capturing();
1062  uninit_device();
1063  close_device();
1064 
1065  if (avcodec_context_)
1066  {
1067  avcodec_close(avcodec_context_);
1068  av_free(avcodec_context_);
1069  avcodec_context_ = NULL;
1070  }
1071  if (avframe_camera_)
1072  av_free(avframe_camera_);
1073  avframe_camera_ = NULL;
1074  if (avframe_rgb_)
1075  av_free(avframe_rgb_);
1076  avframe_rgb_ = NULL;
1077  if(image_)
1078  free(image_);
1079  image_ = NULL;
1080 }
1081 
1082 void UsbCam::grab_image(sensor_msgs::Image* msg)
1083 {
1084  // grab the image
1085  grab_image();
1086  // stamp the image
1087  msg->header.stamp = ros::Time::now();
1088  // fill the info
1089  if (monochrome_)
1090  {
1091  fillImage(*msg, "mono8", image_->height, image_->width, image_->width,
1092  image_->image);
1093  }
1094  else
1095  {
1096  fillImage(*msg, "rgb8", image_->height, image_->width, 3 * image_->width,
1097  image_->image);
1098  }
1099 }
1100 
1102 {
1103  fd_set fds;
1104  struct timeval tv;
1105  int r;
1106 
1107  FD_ZERO(&fds);
1108  FD_SET(fd_, &fds);
1109 
1110  /* Timeout. */
1111  tv.tv_sec = 5;
1112  tv.tv_usec = 0;
1113 
1114  r = select(fd_ + 1, &fds, NULL, NULL, &tv);
1115 
1116  if (-1 == r)
1117  {
1118  if (EINTR == errno)
1119  return;
1120 
1121  errno_exit("select");
1122  }
1123 
1124  if (0 == r)
1125  {
1126  ROS_ERROR("select timeout");
1127  exit(EXIT_FAILURE);
1128  }
1129 
1130  read_frame();
1131  image_->is_new = 1;
1132 }
1133 
1134 // enables/disables auto focus
1135 void UsbCam::set_auto_focus(int value)
1136 {
1137  struct v4l2_queryctrl queryctrl;
1138  struct v4l2_ext_control control;
1139 
1140  memset(&queryctrl, 0, sizeof(queryctrl));
1141  queryctrl.id = V4L2_CID_FOCUS_AUTO;
1142 
1143  if (-1 == xioctl(fd_, VIDIOC_QUERYCTRL, &queryctrl))
1144  {
1145  if (errno != EINVAL)
1146  {
1147  perror("VIDIOC_QUERYCTRL");
1148  return;
1149  }
1150  else
1151  {
1152  ROS_INFO("V4L2_CID_FOCUS_AUTO is not supported");
1153  return;
1154  }
1155  }
1156  else if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
1157  {
1158  ROS_INFO("V4L2_CID_FOCUS_AUTO is not supported");
1159  return;
1160  }
1161  else
1162  {
1163  memset(&control, 0, sizeof(control));
1164  control.id = V4L2_CID_FOCUS_AUTO;
1165  control.value = value;
1166 
1167  if (-1 == xioctl(fd_, VIDIOC_S_CTRL, &control))
1168  {
1169  perror("VIDIOC_S_CTRL");
1170  return;
1171  }
1172  }
1173 }
1174 
1181 void UsbCam::set_v4l_parameter(const std::string& param, int value)
1182 {
1183  set_v4l_parameter(param, boost::lexical_cast<std::string>(value));
1184 }
1191 void UsbCam::set_v4l_parameter(const std::string& param, const std::string& value)
1192 {
1193  // build the command
1194  std::stringstream ss;
1195  ss << "v4l2-ctl --device=" << camera_dev_ << " -c " << param << "=" << value << " 2>&1";
1196  std::string cmd = ss.str();
1197 
1198  // capture the output
1199  std::string output;
1200  int buffer_size = 256;
1201  char buffer[buffer_size];
1202  FILE *stream = popen(cmd.c_str(), "r");
1203  if (stream)
1204  {
1205  while (!feof(stream))
1206  if (fgets(buffer, buffer_size, stream) != NULL)
1207  output.append(buffer);
1208  pclose(stream);
1209  // any output should be an error
1210  if (output.length() > 0)
1211  ROS_WARN("%s", output.c_str());
1212  }
1213  else
1214  ROS_WARN("usb_cam_node could not run '%s'", cmd.c_str());
1215 }
1216 
1218 {
1219  if (str == "mmap")
1220  return IO_METHOD_MMAP;
1221  else if (str == "read")
1222  return IO_METHOD_READ;
1223  else if (str == "userptr")
1224  return IO_METHOD_USERPTR;
1225  else
1226  return IO_METHOD_UNKNOWN;
1227 }
1228 
1230 {
1231  if (str == "yuyv")
1232  return PIXEL_FORMAT_YUYV;
1233  else if (str == "uyvy")
1234  return PIXEL_FORMAT_UYVY;
1235  else if (str == "mjpeg")
1236  return PIXEL_FORMAT_MJPEG;
1237  else if (str == "yuvmono10")
1238  return PIXEL_FORMAT_YUVMONO10;
1239  else if (str == "rgb24")
1240  return PIXEL_FORMAT_RGB24;
1241  else if (str == "grey")
1242  return PIXEL_FORMAT_GREY;
1243  else
1244  return PIXEL_FORMAT_UNKNOWN;
1245 }
1246 
1247 }
static void mono102mono8(char *RAW, char *MONO, int NumPixels)
Definition: usb_cam.cpp:317
int init_mjpeg_decoder(int image_width, int image_height)
Definition: usb_cam.cpp:366
bool is_capturing_
Definition: usb_cam.h:130
void start_capturing(void)
Definition: usb_cam.cpp:616
string cmd
void rgb242rgb(char *YUV, char *RGB, int NumPixels)
Definition: usb_cam.cpp:350
static int xioctl(int fd, int request, void *arg)
Definition: usb_cam.cpp:67
unsigned int n_buffers_
Definition: usb_cam.h:139
AVFrame * avframe_camera_
Definition: usb_cam.h:140
void set_auto_focus(int value)
Definition: usb_cam.cpp:1135
void uyvy2rgb(char *YUV, char *RGB, int NumPixels)
Definition: usb_cam.cpp:295
void open_device(void)
Definition: usb_cam.cpp:980
void uninit_device(void)
Definition: usb_cam.cpp:679
void shutdown(void)
Definition: usb_cam.cpp:1059
struct SwsContext * video_sws_
Definition: usb_cam.h:147
#define ROS_WARN(...)
void stop_capturing(void)
Definition: usb_cam.cpp:592
void process_image(const void *src, int len, camera_image_t *dest)
Definition: usb_cam.cpp:462
const int clipping_table_offset
Definition: usb_cam.cpp:234
AVCodec * avcodec_
Definition: usb_cam.h:142
static pixel_format pixel_format_from_string(const std::string &str)
Definition: usb_cam.cpp:1229
void close_device(void)
Definition: usb_cam.cpp:972
io_method io_
Definition: usb_cam.h:136
bool is_capturing()
Definition: usb_cam.cpp:588
#define ROS_INFO(...)
void mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels)
Definition: usb_cam.cpp:409
int avframe_camera_size_
Definition: usb_cam.h:145
unsigned int pixelformat_
Definition: usb_cam.h:134
std::string camera_dev_
Definition: usb_cam.h:133
bool monochrome_
Definition: usb_cam.h:135
void set_v4l_parameter(const std::string &param, int value)
Definition: usb_cam.cpp:1181
static io_method io_method_from_string(const std::string &str)
Definition: usb_cam.cpp:1217
void grab_image()
Definition: usb_cam.cpp:1101
void init_userp(unsigned int buffer_size)
Definition: usb_cam.cpp:784
AVDictionary * avoptions_
Definition: usb_cam.h:143
void init_device(int image_width, int image_height, int framerate)
Definition: usb_cam.cpp:833
buffer * buffers_
Definition: usb_cam.h:138
static void YUV2RGB(const unsigned char y, const unsigned char u, const unsigned char v, unsigned char *r, unsigned char *g, unsigned char *b)
Definition: usb_cam.cpp:270
void init_mmap(void)
Definition: usb_cam.cpp:724
camera_image_t * image_
Definition: usb_cam.h:148
AVCodecContext * avcodec_context_
Definition: usb_cam.h:144
#define AV_CODEC_ID_MJPEG
Definition: usb_cam.h:52
static Time now()
const unsigned char uchar_clipping_table[]
Definition: usb_cam.cpp:78
int avframe_rgb_size_
Definition: usb_cam.h:146
void init_read(unsigned int buffer_size)
Definition: usb_cam.cpp:704
#define ROS_ERROR_STREAM(args)
AVFrame * avframe_rgb_
Definition: usb_cam.h:141
int read_frame()
Definition: usb_cam.cpp:485
static void errno_exit(const char *s)
Definition: usb_cam.cpp:61
#define ROS_ERROR(...)
#define CLEAR(x)
Definition: usb_cam.cpp:57
void start(const std::string &dev, io_method io, pixel_format pf, int image_width, int image_height, int framerate)
Definition: usb_cam.cpp:1005
static unsigned char CLIPVALUE(int val)
Definition: usb_cam.cpp:239
#define ROS_DEBUG(...)
static void yuyv2rgb(char *YUV, char *RGB, int NumPixels)
Definition: usb_cam.cpp:327


usb_cam
Author(s): Benjamin Pitzer
autogenerated on Fri Jun 7 2019 21:46:25