usb_cam.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, Robert Bosch LLC.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Robert Bosch nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *********************************************************************/
00036 #define __STDC_CONSTANT_MACROS
00037 #include <stdio.h>
00038 #include <stdlib.h>
00039 #include <string.h>
00040 #include <assert.h>
00041 #include <fcntl.h>              /* low-level i/o */
00042 #include <unistd.h>
00043 #include <errno.h>
00044 #include <malloc.h>
00045 #include <sys/stat.h>
00046 #include <sys/types.h>
00047 #include <sys/time.h>
00048 #include <sys/mman.h>
00049 #include <sys/ioctl.h>
00050 
00051 #include <ros/ros.h>
00052 #include <boost/lexical_cast.hpp>
00053 #include <sensor_msgs/fill_image.h>
00054 
00055 #include <usb_cam/usb_cam.h>
00056 
00057 #define CLEAR(x) memset (&(x), 0, sizeof (x))
00058 
00059 namespace usb_cam {
00060 
00061 static void errno_exit(const char * s)
00062 {
00063   ROS_ERROR("%s error %d, %s", s, errno, strerror(errno));
00064   exit(EXIT_FAILURE);
00065 }
00066 
00067 static int xioctl(int fd, int request, void * arg)
00068 {
00069   int r;
00070 
00071   do
00072     r = ioctl(fd, request, arg);
00073   while (-1 == r && EINTR == errno);
00074 
00075   return r;
00076 }
00077 
00078 const unsigned char uchar_clipping_table[] = {
00079     0,
00080     0,
00081     0,
00082     0,
00083     0,
00084     0,
00085     0,
00086     0, // -128 - -121
00087     0,
00088     0,
00089     0,
00090     0,
00091     0,
00092     0,
00093     0,
00094     0, // -120 - -113
00095     0,
00096     0,
00097     0,
00098     0,
00099     0,
00100     0,
00101     0,
00102     0, // -112 - -105
00103     0,
00104     0,
00105     0,
00106     0,
00107     0,
00108     0,
00109     0,
00110     0, // -104 -  -97
00111     0,
00112     0,
00113     0,
00114     0,
00115     0,
00116     0,
00117     0,
00118     0, //  -96 -  -89
00119     0,
00120     0,
00121     0,
00122     0,
00123     0,
00124     0,
00125     0,
00126     0, //  -88 -  -81
00127     0,
00128     0,
00129     0,
00130     0,
00131     0,
00132     0,
00133     0,
00134     0, //  -80 -  -73
00135     0,
00136     0,
00137     0,
00138     0,
00139     0,
00140     0,
00141     0,
00142     0, //  -72 -  -65
00143     0,
00144     0,
00145     0,
00146     0,
00147     0,
00148     0,
00149     0,
00150     0, //  -64 -  -57
00151     0,
00152     0,
00153     0,
00154     0,
00155     0,
00156     0,
00157     0,
00158     0, //  -56 -  -49
00159     0,
00160     0,
00161     0,
00162     0,
00163     0,
00164     0,
00165     0,
00166     0, //  -48 -  -41
00167     0,
00168     0,
00169     0,
00170     0,
00171     0,
00172     0,
00173     0,
00174     0, //  -40 -  -33
00175     0,
00176     0,
00177     0,
00178     0,
00179     0,
00180     0,
00181     0,
00182     0, //  -32 -  -25
00183     0,
00184     0,
00185     0,
00186     0,
00187     0,
00188     0,
00189     0,
00190     0, //  -24 -  -17
00191     0,
00192     0,
00193     0,
00194     0,
00195     0,
00196     0,
00197     0,
00198     0, //  -16 -   -9
00199     0,
00200     0,
00201     0,
00202     0,
00203     0,
00204     0,
00205     0,
00206     0, //   -8 -   -1
00207     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,
00208     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,
00209     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,
00210     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,
00211     114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136,
00212     137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159,
00213     160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182,
00214     183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205,
00215     206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228,
00216     229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251,
00217     252, 253, 254, 255, 255, 255, 255, 255, 255, 255, 255, 255, // 256-263
00218     255, 255, 255, 255, 255, 255, 255, 255, // 264-271
00219     255, 255, 255, 255, 255, 255, 255, 255, // 272-279
00220     255, 255, 255, 255, 255, 255, 255, 255, // 280-287
00221     255, 255, 255, 255, 255, 255, 255, 255, // 288-295
00222     255, 255, 255, 255, 255, 255, 255, 255, // 296-303
00223     255, 255, 255, 255, 255, 255, 255, 255, // 304-311
00224     255, 255, 255, 255, 255, 255, 255, 255, // 312-319
00225     255, 255, 255, 255, 255, 255, 255, 255, // 320-327
00226     255, 255, 255, 255, 255, 255, 255, 255, // 328-335
00227     255, 255, 255, 255, 255, 255, 255, 255, // 336-343
00228     255, 255, 255, 255, 255, 255, 255, 255, // 344-351
00229     255, 255, 255, 255, 255, 255, 255, 255, // 352-359
00230     255, 255, 255, 255, 255, 255, 255, 255, // 360-367
00231     255, 255, 255, 255, 255, 255, 255, 255, // 368-375
00232     255, 255, 255, 255, 255, 255, 255, 255, // 376-383
00233     };
00234 const int clipping_table_offset = 128;
00235 
00239 static unsigned char CLIPVALUE(int val)
00240 {
00241   // Old method (if)
00242   /*   val = val < 0 ? 0 : val; */
00243   /*   return val > 255 ? 255 : val; */
00244 
00245   // New method (array)
00246   return uchar_clipping_table[val + clipping_table_offset];
00247 }
00248 
00270 static void YUV2RGB(const unsigned char y, const unsigned char u, const unsigned char v, unsigned char* r,
00271                     unsigned char* g, unsigned char* b)
00272 {
00273   const int y2 = (int)y;
00274   const int u2 = (int)u - 128;
00275   const int v2 = (int)v - 128;
00276   //std::cerr << "YUV=("<<y2<<","<<u2<<","<<v2<<")"<<std::endl;
00277 
00278   // This is the normal YUV conversion, but
00279   // appears to be incorrect for the firewire cameras
00280   //   int r2 = y2 + ( (v2*91947) >> 16);
00281   //   int g2 = y2 - ( ((u2*22544) + (v2*46793)) >> 16 );
00282   //   int b2 = y2 + ( (u2*115999) >> 16);
00283   // This is an adjusted version (UV spread out a bit)
00284   int r2 = y2 + ((v2 * 37221) >> 15);
00285   int g2 = y2 - (((u2 * 12975) + (v2 * 18949)) >> 15);
00286   int b2 = y2 + ((u2 * 66883) >> 15);
00287   //std::cerr << "   RGB=("<<r2<<","<<g2<<","<<b2<<")"<<std::endl;
00288 
00289   // Cap the values.
00290   *r = CLIPVALUE(r2);
00291   *g = CLIPVALUE(g2);
00292   *b = CLIPVALUE(b2);
00293 }
00294 
00295 void uyvy2rgb(char *YUV, char *RGB, int NumPixels)
00296 {
00297   int i, j;
00298   unsigned char y0, y1, u, v;
00299   unsigned char r, g, b;
00300   for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6)
00301   {
00302     u = (unsigned char)YUV[i + 0];
00303     y0 = (unsigned char)YUV[i + 1];
00304     v = (unsigned char)YUV[i + 2];
00305     y1 = (unsigned char)YUV[i + 3];
00306     YUV2RGB(y0, u, v, &r, &g, &b);
00307     RGB[j + 0] = r;
00308     RGB[j + 1] = g;
00309     RGB[j + 2] = b;
00310     YUV2RGB(y1, u, v, &r, &g, &b);
00311     RGB[j + 3] = r;
00312     RGB[j + 4] = g;
00313     RGB[j + 5] = b;
00314   }
00315 }
00316 
00317 static void mono102mono8(char *RAW, char *MONO, int NumPixels)
00318 {
00319   int i, j;
00320   for (i = 0, j = 0; i < (NumPixels << 1); i += 2, j += 1)
00321   {
00322     //first byte is low byte, second byte is high byte; smash together and convert to 8-bit
00323     MONO[j] = (unsigned char)(((RAW[i + 0] >> 2) & 0x3F) | ((RAW[i + 1] << 6) & 0xC0));
00324   }
00325 }
00326 
00327 static void yuyv2rgb(char *YUV, char *RGB, int NumPixels)
00328 {
00329   int i, j;
00330   unsigned char y0, y1, u, v;
00331   unsigned char r, g, b;
00332 
00333   for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6)
00334   {
00335     y0 = (unsigned char)YUV[i + 0];
00336     u = (unsigned char)YUV[i + 1];
00337     y1 = (unsigned char)YUV[i + 2];
00338     v = (unsigned char)YUV[i + 3];
00339     YUV2RGB(y0, u, v, &r, &g, &b);
00340     RGB[j + 0] = r;
00341     RGB[j + 1] = g;
00342     RGB[j + 2] = b;
00343     YUV2RGB(y1, u, v, &r, &g, &b);
00344     RGB[j + 3] = r;
00345     RGB[j + 4] = g;
00346     RGB[j + 5] = b;
00347   }
00348 }
00349 
00350 void rgb242rgb(char *YUV, char *RGB, int NumPixels)
00351 {
00352   memcpy(RGB, YUV, NumPixels * 3);
00353 }
00354 
00355 
00356 UsbCam::UsbCam()
00357   : io_(IO_METHOD_MMAP), fd_(-1), buffers_(NULL), n_buffers_(0), avframe_camera_(NULL),
00358     avframe_rgb_(NULL), avcodec_(NULL), avoptions_(NULL), avcodec_context_(NULL),
00359     avframe_camera_size_(0), avframe_rgb_size_(0), video_sws_(NULL), image_(NULL) {
00360 }
00361 UsbCam::~UsbCam()
00362 {
00363   shutdown();
00364 }
00365 
00366 int UsbCam::init_mjpeg_decoder(int image_width, int image_height)
00367 {
00368   avcodec_register_all();
00369 
00370   avcodec_ = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
00371   if (!avcodec_)
00372   {
00373     ROS_ERROR("Could not find MJPEG decoder");
00374     return 0;
00375   }
00376 
00377   avcodec_context_ = avcodec_alloc_context3(avcodec_);
00378   avframe_camera_ = avcodec_alloc_frame();
00379   avframe_rgb_ = avcodec_alloc_frame();
00380 
00381   avpicture_alloc((AVPicture *)avframe_rgb_, PIX_FMT_RGB24, image_width, image_height);
00382 
00383   avcodec_context_->codec_id = AV_CODEC_ID_MJPEG;
00384   avcodec_context_->width = image_width;
00385   avcodec_context_->height = image_height;
00386 
00387 #if LIBAVCODEC_VERSION_MAJOR > 52
00388   avcodec_context_->pix_fmt = PIX_FMT_YUV422P;
00389   avcodec_context_->codec_type = AVMEDIA_TYPE_VIDEO;
00390 #endif
00391 
00392   avframe_camera_size_ = avpicture_get_size(PIX_FMT_YUV422P, image_width, image_height);
00393   avframe_rgb_size_ = avpicture_get_size(PIX_FMT_RGB24, image_width, image_height);
00394 
00395   /* open it */
00396   if (avcodec_open2(avcodec_context_, avcodec_, &avoptions_) < 0)
00397   {
00398     ROS_ERROR("Could not open MJPEG Decoder");
00399     return 0;
00400   }
00401   return 1;
00402 }
00403 
00404 void UsbCam::mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels)
00405 {
00406   int got_picture;
00407 
00408   memset(RGB, 0, avframe_rgb_size_);
00409 
00410 #if LIBAVCODEC_VERSION_MAJOR > 52
00411   int decoded_len;
00412   AVPacket avpkt;
00413   av_init_packet(&avpkt);
00414 
00415   avpkt.size = len;
00416   avpkt.data = (unsigned char*)MJPEG;
00417   decoded_len = avcodec_decode_video2(avcodec_context_, avframe_camera_, &got_picture, &avpkt);
00418 
00419   if (decoded_len < 0)
00420   {
00421     ROS_ERROR("Error while decoding frame.");
00422     return;
00423   }
00424 #else
00425   avcodec_decode_video(avcodec_context_, avframe_camera_, &got_picture, (uint8_t *) MJPEG, len);
00426 #endif
00427 
00428   if (!got_picture)
00429   {
00430     ROS_ERROR("Webcam: expected picture but didn't get it...");
00431     return;
00432   }
00433 
00434   int xsize = avcodec_context_->width;
00435   int ysize = avcodec_context_->height;
00436   int pic_size = avpicture_get_size(avcodec_context_->pix_fmt, xsize, ysize);
00437   if (pic_size != avframe_camera_size_)
00438   {
00439     ROS_ERROR("outbuf size mismatch.  pic_size: %d bufsize: %d", pic_size, avframe_camera_size_);
00440     return;
00441   }
00442 
00443   video_sws_ = sws_getContext(xsize, ysize, avcodec_context_->pix_fmt, xsize, ysize, PIX_FMT_RGB24, SWS_BILINEAR, NULL,
00444                               NULL,  NULL);
00445   sws_scale(video_sws_, avframe_camera_->data, avframe_camera_->linesize, 0, ysize, avframe_rgb_->data,
00446             avframe_rgb_->linesize);
00447   sws_freeContext(video_sws_);
00448 
00449   int size = avpicture_layout((AVPicture *)avframe_rgb_, PIX_FMT_RGB24, xsize, ysize, (uint8_t *)RGB, avframe_rgb_size_);
00450   if (size != avframe_rgb_size_)
00451   {
00452     ROS_ERROR("webcam: avpicture_layout error: %d", size);
00453     return;
00454   }
00455 }
00456 
00457 void UsbCam::process_image(const void * src, int len, camera_image_t *dest)
00458 {
00459   if (pixelformat_ == V4L2_PIX_FMT_YUYV)
00460   {
00461     if (monochrome_)
00462     { //actually format V4L2_PIX_FMT_Y16, but xioctl gets unhappy if you don't use the advertised type (yuyv)
00463       mono102mono8((char*)src, dest->image, dest->width * dest->height);
00464     }
00465     else
00466     {
00467       yuyv2rgb((char*)src, dest->image, dest->width * dest->height);
00468     }
00469   }
00470   else if (pixelformat_ == V4L2_PIX_FMT_UYVY)
00471     uyvy2rgb((char*)src, dest->image, dest->width * dest->height);
00472   else if (pixelformat_ == V4L2_PIX_FMT_MJPEG)
00473     mjpeg2rgb((char*)src, len, dest->image, dest->width * dest->height);
00474   else if (pixelformat_ == V4L2_PIX_FMT_RGB24)
00475     rgb242rgb((char*)src, dest->image, dest->width * dest->height);
00476 }
00477 
00478 int UsbCam::read_frame()
00479 {
00480   struct v4l2_buffer buf;
00481   unsigned int i;
00482   int len;
00483 
00484   switch (io_)
00485   {
00486     case IO_METHOD_READ:
00487       len = read(fd_, buffers_[0].start, buffers_[0].length);
00488       if (len == -1)
00489       {
00490         switch (errno)
00491         {
00492           case EAGAIN:
00493             return 0;
00494 
00495           case EIO:
00496             /* Could ignore EIO, see spec. */
00497 
00498             /* fall through */
00499 
00500           default:
00501             errno_exit("read");
00502         }
00503       }
00504 
00505       process_image(buffers_[0].start, len, image_);
00506 
00507       break;
00508 
00509     case IO_METHOD_MMAP:
00510       CLEAR(buf);
00511 
00512       buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00513       buf.memory = V4L2_MEMORY_MMAP;
00514 
00515       if (-1 == xioctl(fd_, VIDIOC_DQBUF, &buf))
00516       {
00517         switch (errno)
00518         {
00519           case EAGAIN:
00520             return 0;
00521 
00522           case EIO:
00523             /* Could ignore EIO, see spec. */
00524 
00525             /* fall through */
00526 
00527           default:
00528             errno_exit("VIDIOC_DQBUF");
00529         }
00530       }
00531 
00532       assert(buf.index < n_buffers_);
00533       len = buf.bytesused;
00534       process_image(buffers_[buf.index].start, len, image_);
00535 
00536       if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
00537         errno_exit("VIDIOC_QBUF");
00538 
00539       break;
00540 
00541     case IO_METHOD_USERPTR:
00542       CLEAR(buf);
00543 
00544       buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00545       buf.memory = V4L2_MEMORY_USERPTR;
00546 
00547       if (-1 == xioctl(fd_, VIDIOC_DQBUF, &buf))
00548       {
00549         switch (errno)
00550         {
00551           case EAGAIN:
00552             return 0;
00553 
00554           case EIO:
00555             /* Could ignore EIO, see spec. */
00556 
00557             /* fall through */
00558 
00559           default:
00560             errno_exit("VIDIOC_DQBUF");
00561         }
00562       }
00563 
00564       for (i = 0; i < n_buffers_; ++i)
00565         if (buf.m.userptr == (unsigned long)buffers_[i].start && buf.length == buffers_[i].length)
00566           break;
00567 
00568       assert(i < n_buffers_);
00569       len = buf.bytesused;
00570       process_image((void *)buf.m.userptr, len, image_);
00571 
00572       if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
00573         errno_exit("VIDIOC_QBUF");
00574 
00575       break;
00576   }
00577 
00578   return 1;
00579 }
00580 
00581 void UsbCam::stop_capturing(void)
00582 {
00583   enum v4l2_buf_type type;
00584 
00585   switch (io_)
00586   {
00587     case IO_METHOD_READ:
00588       /* Nothing to do. */
00589       break;
00590 
00591     case IO_METHOD_MMAP:
00592     case IO_METHOD_USERPTR:
00593       type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00594 
00595       if (-1 == xioctl(fd_, VIDIOC_STREAMOFF, &type))
00596         errno_exit("VIDIOC_STREAMOFF");
00597 
00598       break;
00599   }
00600 }
00601 
00602 void UsbCam::start_capturing(void)
00603 {
00604   unsigned int i;
00605   enum v4l2_buf_type type;
00606 
00607   switch (io_)
00608   {
00609     case IO_METHOD_READ:
00610       /* Nothing to do. */
00611       break;
00612 
00613     case IO_METHOD_MMAP:
00614       for (i = 0; i < n_buffers_; ++i)
00615       {
00616         struct v4l2_buffer buf;
00617 
00618         CLEAR(buf);
00619 
00620         buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00621         buf.memory = V4L2_MEMORY_MMAP;
00622         buf.index = i;
00623 
00624         if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
00625           errno_exit("VIDIOC_QBUF");
00626       }
00627 
00628       type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00629 
00630       if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type))
00631         errno_exit("VIDIOC_STREAMON");
00632 
00633       break;
00634 
00635     case IO_METHOD_USERPTR:
00636       for (i = 0; i < n_buffers_; ++i)
00637       {
00638         struct v4l2_buffer buf;
00639 
00640         CLEAR(buf);
00641 
00642         buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00643         buf.memory = V4L2_MEMORY_USERPTR;
00644         buf.index = i;
00645         buf.m.userptr = (unsigned long)buffers_[i].start;
00646         buf.length = buffers_[i].length;
00647 
00648         if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf))
00649           errno_exit("VIDIOC_QBUF");
00650       }
00651 
00652       type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00653 
00654       if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type))
00655         errno_exit("VIDIOC_STREAMON");
00656 
00657       break;
00658   }
00659 }
00660 
00661 void UsbCam::uninit_device(void)
00662 {
00663   unsigned int i;
00664 
00665   switch (io_)
00666   {
00667     case IO_METHOD_READ:
00668       free(buffers_[0].start);
00669       break;
00670 
00671     case IO_METHOD_MMAP:
00672       for (i = 0; i < n_buffers_; ++i)
00673         if (-1 == munmap(buffers_[i].start, buffers_[i].length))
00674           errno_exit("munmap");
00675       break;
00676 
00677     case IO_METHOD_USERPTR:
00678       for (i = 0; i < n_buffers_; ++i)
00679         free(buffers_[i].start);
00680       break;
00681   }
00682 
00683   free(buffers_);
00684 }
00685 
00686 void UsbCam::init_read(unsigned int buffer_size)
00687 {
00688   buffers_ = (buffer*)calloc(1, sizeof(*buffers_));
00689 
00690   if (!buffers_)
00691   {
00692     ROS_ERROR("Out of memory");
00693     exit(EXIT_FAILURE);
00694   }
00695 
00696   buffers_[0].length = buffer_size;
00697   buffers_[0].start = malloc(buffer_size);
00698 
00699   if (!buffers_[0].start)
00700   {
00701     ROS_ERROR("Out of memory");
00702     exit(EXIT_FAILURE);
00703   }
00704 }
00705 
00706 void UsbCam::init_mmap(void)
00707 {
00708   struct v4l2_requestbuffers req;
00709 
00710   CLEAR(req);
00711 
00712   req.count = 4;
00713   req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00714   req.memory = V4L2_MEMORY_MMAP;
00715 
00716   if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &req))
00717   {
00718     if (EINVAL == errno)
00719     {
00720       ROS_ERROR_STREAM(camera_dev_ << " does not support memory mapping");
00721       exit(EXIT_FAILURE);
00722     }
00723     else
00724     {
00725       errno_exit("VIDIOC_REQBUFS");
00726     }
00727   }
00728 
00729   if (req.count < 2)
00730   {
00731     ROS_ERROR_STREAM("Insufficient buffer memory on " << camera_dev_);
00732     exit(EXIT_FAILURE);
00733   }
00734 
00735   buffers_ = (buffer*)calloc(req.count, sizeof(*buffers_));
00736 
00737   if (!buffers_)
00738   {
00739     ROS_ERROR("Out of memory");
00740     exit(EXIT_FAILURE);
00741   }
00742 
00743   for (n_buffers_ = 0; n_buffers_ < req.count; ++n_buffers_)
00744   {
00745     struct v4l2_buffer buf;
00746 
00747     CLEAR(buf);
00748 
00749     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00750     buf.memory = V4L2_MEMORY_MMAP;
00751     buf.index = n_buffers_;
00752 
00753     if (-1 == xioctl(fd_, VIDIOC_QUERYBUF, &buf))
00754       errno_exit("VIDIOC_QUERYBUF");
00755 
00756     buffers_[n_buffers_].length = buf.length;
00757     buffers_[n_buffers_].start = mmap(NULL /* start anywhere */, buf.length, PROT_READ | PROT_WRITE /* required */,
00758                                       MAP_SHARED /* recommended */,
00759                                       fd_, buf.m.offset);
00760 
00761     if (MAP_FAILED == buffers_[n_buffers_].start)
00762       errno_exit("mmap");
00763   }
00764 }
00765 
00766 void UsbCam::init_userp(unsigned int buffer_size)
00767 {
00768   struct v4l2_requestbuffers req;
00769   unsigned int page_size;
00770 
00771   page_size = getpagesize();
00772   buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
00773 
00774   CLEAR(req);
00775 
00776   req.count = 4;
00777   req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00778   req.memory = V4L2_MEMORY_USERPTR;
00779 
00780   if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &req))
00781   {
00782     if (EINVAL == errno)
00783     {
00784       ROS_ERROR_STREAM(camera_dev_ << " does not support "
00785                 "user pointer i/o");
00786       exit(EXIT_FAILURE);
00787     }
00788     else
00789     {
00790       errno_exit("VIDIOC_REQBUFS");
00791     }
00792   }
00793 
00794   buffers_ = (buffer*)calloc(4, sizeof(*buffers_));
00795 
00796   if (!buffers_)
00797   {
00798     ROS_ERROR("Out of memory");
00799     exit(EXIT_FAILURE);
00800   }
00801 
00802   for (n_buffers_ = 0; n_buffers_ < 4; ++n_buffers_)
00803   {
00804     buffers_[n_buffers_].length = buffer_size;
00805     buffers_[n_buffers_].start = memalign(/* boundary */page_size, buffer_size);
00806 
00807     if (!buffers_[n_buffers_].start)
00808     {
00809       ROS_ERROR("Out of memory");
00810       exit(EXIT_FAILURE);
00811     }
00812   }
00813 }
00814 
00815 void UsbCam::init_device(int image_width, int image_height, int framerate)
00816 {
00817   struct v4l2_capability cap;
00818   struct v4l2_cropcap cropcap;
00819   struct v4l2_crop crop;
00820   struct v4l2_format fmt;
00821   unsigned int min;
00822 
00823   if (-1 == xioctl(fd_, VIDIOC_QUERYCAP, &cap))
00824   {
00825     if (EINVAL == errno)
00826     {
00827       ROS_ERROR_STREAM(camera_dev_ << " is no V4L2 device");
00828       exit(EXIT_FAILURE);
00829     }
00830     else
00831     {
00832       errno_exit("VIDIOC_QUERYCAP");
00833     }
00834   }
00835 
00836   if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
00837   {
00838     ROS_ERROR_STREAM(camera_dev_ << " is no video capture device");
00839     exit(EXIT_FAILURE);
00840   }
00841 
00842   switch (io_)
00843   {
00844     case IO_METHOD_READ:
00845       if (!(cap.capabilities & V4L2_CAP_READWRITE))
00846       {
00847         ROS_ERROR_STREAM(camera_dev_ << " does not support read i/o");
00848         exit(EXIT_FAILURE);
00849       }
00850 
00851       break;
00852 
00853     case IO_METHOD_MMAP:
00854     case IO_METHOD_USERPTR:
00855       if (!(cap.capabilities & V4L2_CAP_STREAMING))
00856       {
00857         ROS_ERROR_STREAM(camera_dev_ << " does not support streaming i/o");
00858         exit(EXIT_FAILURE);
00859       }
00860 
00861       break;
00862   }
00863 
00864   /* Select video input, video standard and tune here. */
00865 
00866   CLEAR(cropcap);
00867 
00868   cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00869 
00870   if (0 == xioctl(fd_, VIDIOC_CROPCAP, &cropcap))
00871   {
00872     crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00873     crop.c = cropcap.defrect; /* reset to default */
00874 
00875     if (-1 == xioctl(fd_, VIDIOC_S_CROP, &crop))
00876     {
00877       switch (errno)
00878       {
00879         case EINVAL:
00880           /* Cropping not supported. */
00881           break;
00882         default:
00883           /* Errors ignored. */
00884           break;
00885       }
00886     }
00887   }
00888   else
00889   {
00890     /* Errors ignored. */
00891   }
00892 
00893   CLEAR(fmt);
00894 
00895 //  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00896 //  fmt.fmt.pix.width = 640;
00897 //  fmt.fmt.pix.height = 480;
00898 //  fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
00899 //  fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
00900 
00901   fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00902   fmt.fmt.pix.width = image_width;
00903   fmt.fmt.pix.height = image_height;
00904   fmt.fmt.pix.pixelformat = pixelformat_;
00905   fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
00906 
00907   if (-1 == xioctl(fd_, VIDIOC_S_FMT, &fmt))
00908     errno_exit("VIDIOC_S_FMT");
00909 
00910   /* Note VIDIOC_S_FMT may change width and height. */
00911 
00912   /* Buggy driver paranoia. */
00913   min = fmt.fmt.pix.width * 2;
00914   if (fmt.fmt.pix.bytesperline < min)
00915     fmt.fmt.pix.bytesperline = min;
00916   min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
00917   if (fmt.fmt.pix.sizeimage < min)
00918     fmt.fmt.pix.sizeimage = min;
00919 
00920   image_width = fmt.fmt.pix.width;
00921   image_height = fmt.fmt.pix.height;
00922 
00923   struct v4l2_streamparm stream_params;
00924   memset(&stream_params, 0, sizeof(stream_params));
00925   stream_params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00926   if (xioctl(fd_, VIDIOC_G_PARM, &stream_params) < 0)
00927     errno_exit("Couldn't query v4l fps!");
00928 
00929   ROS_DEBUG("Capability flag: 0x%x", stream_params.parm.capture.capability);
00930 
00931   stream_params.parm.capture.timeperframe.numerator = 1;
00932   stream_params.parm.capture.timeperframe.denominator = framerate;
00933   if (xioctl(fd_, VIDIOC_S_PARM, &stream_params) < 0)
00934     ROS_WARN("Couldn't set camera framerate");
00935   else
00936     ROS_DEBUG("Set framerate to be %i", framerate);
00937 
00938   switch (io_)
00939   {
00940     case IO_METHOD_READ:
00941       init_read(fmt.fmt.pix.sizeimage);
00942       break;
00943 
00944     case IO_METHOD_MMAP:
00945       init_mmap();
00946       break;
00947 
00948     case IO_METHOD_USERPTR:
00949       init_userp(fmt.fmt.pix.sizeimage);
00950       break;
00951   }
00952 }
00953 
00954 void UsbCam::close_device(void)
00955 {
00956   if (-1 == close(fd_))
00957     errno_exit("close");
00958 
00959   fd_ = -1;
00960 }
00961 
00962 void UsbCam::open_device(void)
00963 {
00964   struct stat st;
00965 
00966   if (-1 == stat(camera_dev_.c_str(), &st))
00967   {
00968     ROS_ERROR_STREAM("Cannot identify '" << camera_dev_ << "': " << errno << ", " << strerror(errno));
00969     exit(EXIT_FAILURE);
00970   }
00971 
00972   if (!S_ISCHR(st.st_mode))
00973   {
00974     ROS_ERROR_STREAM(camera_dev_ << " is no device");
00975     exit(EXIT_FAILURE);
00976   }
00977 
00978   fd_ = open(camera_dev_.c_str(), O_RDWR /* required */| O_NONBLOCK, 0);
00979 
00980   if (-1 == fd_)
00981   {
00982     ROS_ERROR_STREAM("Cannot open '" << camera_dev_ << "': " << errno << ", " << strerror(errno));
00983     exit(EXIT_FAILURE);
00984   }
00985 }
00986 
00987 void UsbCam::start(const std::string& dev, io_method io_method,
00988                    pixel_format pixel_format, int image_width, int image_height,
00989                    int framerate)
00990 {
00991   camera_dev_ = dev;
00992 
00993   io_ = io_method;
00994   monochrome_ = false;
00995   if (pixel_format == PIXEL_FORMAT_YUYV)
00996     pixelformat_ = V4L2_PIX_FMT_YUYV;
00997   else if (pixel_format == PIXEL_FORMAT_UYVY)
00998     pixelformat_ = V4L2_PIX_FMT_UYVY;
00999   else if (pixel_format == PIXEL_FORMAT_MJPEG)
01000   {
01001     pixelformat_ = V4L2_PIX_FMT_MJPEG;
01002     init_mjpeg_decoder(image_width, image_height);
01003   }
01004   else if (pixel_format == PIXEL_FORMAT_YUVMONO10)
01005   {
01006     //actually format V4L2_PIX_FMT_Y16 (10-bit mono expresed as 16-bit pixels), but we need to use the advertised type (yuyv)
01007     pixelformat_ = V4L2_PIX_FMT_YUYV;
01008     monochrome_ = true;
01009   }
01010   else if (pixel_format == PIXEL_FORMAT_RGB24)
01011   {
01012     pixelformat_ = V4L2_PIX_FMT_RGB24;
01013   }
01014   else
01015   {
01016     ROS_ERROR("Unknown pixel format.");
01017     exit(EXIT_FAILURE);
01018   }
01019 
01020   open_device();
01021   init_device(image_width, image_height, framerate);
01022   start_capturing();
01023 
01024   image_ = (camera_image_t *)calloc(1, sizeof(camera_image_t));
01025 
01026   image_->width = image_width;
01027   image_->height = image_height;
01028   image_->bytes_per_pixel = 24;
01029 
01030   image_->image_size = image_->width * image_->height * image_->bytes_per_pixel;
01031   image_->is_new = 0;
01032   image_->image = (char *)calloc(image_->image_size, sizeof(char));
01033   memset(image_->image, 0, image_->image_size * sizeof(char));
01034 }
01035 
01036 void UsbCam::shutdown(void)
01037 {
01038   stop_capturing();
01039   uninit_device();
01040   close_device();
01041 
01042   if (avcodec_context_)
01043   {
01044     avcodec_close(avcodec_context_);
01045     av_free(avcodec_context_);
01046     avcodec_context_ = NULL;
01047   }
01048   if (avframe_camera_)
01049     av_free(avframe_camera_);
01050   avframe_camera_ = NULL;
01051   if (avframe_rgb_)
01052     av_free(avframe_rgb_);
01053   avframe_rgb_ = NULL;
01054   if(image_)
01055     free(image_);
01056   image_ = NULL;
01057 }
01058 
01059 void UsbCam::grab_image(sensor_msgs::Image* msg)
01060 {
01061   // grab the image
01062   grab_image();
01063   // stamp the image
01064   msg->header.stamp = ros::Time::now();
01065   // fill the info
01066   if (monochrome_)
01067   {
01068     fillImage(*msg, "mono8", image_->height, image_->width, image_->width,
01069         image_->image);
01070   }
01071   else
01072   {
01073     fillImage(*msg, "rgb8", image_->height, image_->width, 3 * image_->width,
01074         image_->image);
01075   }
01076 }
01077 
01078 void UsbCam::grab_image()
01079 {
01080   fd_set fds;
01081   struct timeval tv;
01082   int r;
01083 
01084   FD_ZERO(&fds);
01085   FD_SET(fd_, &fds);
01086 
01087   /* Timeout. */
01088   tv.tv_sec = 5;
01089   tv.tv_usec = 0;
01090 
01091   r = select(fd_ + 1, &fds, NULL, NULL, &tv);
01092 
01093   if (-1 == r)
01094   {
01095     if (EINTR == errno)
01096       return;
01097 
01098     errno_exit("select");
01099   }
01100 
01101   if (0 == r)
01102   {
01103     ROS_ERROR("select timeout");
01104     exit(EXIT_FAILURE);
01105   }
01106 
01107   read_frame();
01108   image_->is_new = 1;
01109 }
01110 
01111 // enables/disables auto focus
01112 void UsbCam::set_auto_focus(int value)
01113 {
01114   struct v4l2_queryctrl queryctrl;
01115   struct v4l2_ext_control control;
01116 
01117   memset(&queryctrl, 0, sizeof(queryctrl));
01118   queryctrl.id = V4L2_CID_FOCUS_AUTO;
01119 
01120   if (-1 == xioctl(fd_, VIDIOC_QUERYCTRL, &queryctrl))
01121   {
01122     if (errno != EINVAL)
01123     {
01124       perror("VIDIOC_QUERYCTRL");
01125       return;
01126     }
01127     else
01128     {
01129       ROS_INFO("V4L2_CID_FOCUS_AUTO is not supported");
01130       return;
01131     }
01132   }
01133   else if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
01134   {
01135     ROS_INFO("V4L2_CID_FOCUS_AUTO is not supported");
01136     return;
01137   }
01138   else
01139   {
01140     memset(&control, 0, sizeof(control));
01141     control.id = V4L2_CID_FOCUS_AUTO;
01142     control.value = value;
01143 
01144     if (-1 == xioctl(fd_, VIDIOC_S_CTRL, &control))
01145     {
01146       perror("VIDIOC_S_CTRL");
01147       return;
01148     }
01149   }
01150 }
01151 
01158 void UsbCam::set_v4l_parameter(const std::string& param, int value)
01159 {
01160   set_v4l_parameter(param, boost::lexical_cast<std::string>(value));
01161 }
01168 void UsbCam::set_v4l_parameter(const std::string& param, const std::string& value)
01169 {
01170   // build the command
01171   std::stringstream ss;
01172   ss << "v4l2-ctl --device=" << camera_dev_ << " -c " << param << "=" << value << " 2>&1";
01173   std::string cmd = ss.str();
01174 
01175   // capture the output
01176   std::string output;
01177   int buffer_size = 256;
01178   char buffer[buffer_size];
01179   FILE *stream = popen(cmd.c_str(), "r");
01180   if (stream)
01181   {
01182     while (!feof(stream))
01183       if (fgets(buffer, buffer_size, stream) != NULL)
01184         output.append(buffer);
01185     pclose(stream);
01186     // any output should be an error
01187     if (output.length() > 0)
01188       ROS_WARN("%s", output.c_str());
01189   }
01190   else
01191     ROS_WARN("usb_cam_node could not run '%s'", cmd.c_str());
01192 }
01193 
01194 UsbCam::io_method UsbCam::io_method_from_string(const std::string& str)
01195 {
01196   if (str == "mmap")
01197     return IO_METHOD_MMAP;
01198   else if (str == "read")
01199     return IO_METHOD_READ;
01200   else if (str == "userptr")
01201     return IO_METHOD_USERPTR;
01202   else
01203     return IO_METHOD_UNKNOWN;
01204 }
01205 
01206 UsbCam::pixel_format UsbCam::pixel_format_from_string(const std::string& str)
01207 {
01208     if (str == "yuyv")
01209       return PIXEL_FORMAT_YUYV;
01210     else if (str == "uyvy")
01211       return PIXEL_FORMAT_UYVY;
01212     else if (str == "mjpeg")
01213       return PIXEL_FORMAT_MJPEG;
01214     else if (str == "yuvmono10")
01215       return PIXEL_FORMAT_YUVMONO10;
01216     else if (str == "rgb24")
01217       return PIXEL_FORMAT_RGB24;
01218     else
01219       return PIXEL_FORMAT_UNKNOWN;
01220 }
01221 
01222 }


usb_cam
Author(s): Benjamin Pitzer
autogenerated on Fri Feb 12 2016 00:27:27