uvc_cam.cpp
Go to the documentation of this file.
00001 /* This file is from the uvc_cam package by Morgan Quigley */
00002 #include <cstring>
00003 #include <string>
00004 #include <cstdio>
00005 #include <stdexcept>
00006 #include <dirent.h>
00007 #include <sys/stat.h>
00008 #include <sys/ioctl.h>
00009 #include <sys/mman.h>
00010 #include <fcntl.h>
00011 #include <errno.h>
00012 #include "uvc_cam/uvc_cam.h"
00013 #include <unistd.h>
00014 #ifndef INT64_C
00015 #define INT64_C(c) (c ## LL)
00016 #define UINT64_C(c) (c ## ULL)
00017 #endif
00018 
00019 extern "C" {
00020 #include <linux/videodev2.h>
00021 #include <libavcodec/avcodec.h>
00022 #include <libswscale/swscale.h>
00023 }
00024 
00025 static AVFrame *avframe_camera = NULL;
00026 static AVFrame *avframe_rgb = NULL;
00027 static AVCodec *avcodec = NULL;
00028 static AVCodecContext *avcodec_context = NULL;
00029 static int avframe_camera_size = 0;
00030 static int avframe_rgb_size = 0;
00031 struct SwsContext *video_sws = NULL;
00032 unsigned char* jpegBuf = NULL;
00033 
00034 using std::string;
00035 using namespace uvc_cam;
00036 
00037 Cam::Cam(const char *_device, mode_t _mode, int _width, int _height, int _fps)
00038 : mode(_mode), device(_device),
00039   motion_threshold_luminance(100), motion_threshold_count(-1),
00040   width(_width), height(_height), fps(_fps), rgb_frame(NULL)
00041 {
00042   printf("opening %s\n", _device);
00043   if ((fd = open(_device, O_RDWR)) == -1)
00044     throw std::runtime_error("couldn't open " + device);
00045   memset(&fmt, 0, sizeof(v4l2_format));
00046   memset(&cap, 0, sizeof(v4l2_capability));
00047   if (ioctl(fd, VIDIOC_QUERYCAP, &cap) < 0)
00048     throw std::runtime_error("couldn't query " + device);
00049   if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
00050     throw std::runtime_error(device + " does not support capture");
00051   if (!(cap.capabilities & V4L2_CAP_STREAMING))
00052     throw std::runtime_error(device + " does not support streaming");
00053   // enumerate formats
00054   v4l2_fmtdesc f;
00055   memset(&f, 0, sizeof(f));
00056   f.index = 0;
00057   f.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00058   int ret;
00059   while ((ret = ioctl(fd, VIDIOC_ENUM_FMT, &f)) == 0)
00060   {
00061     printf("pixfmt %d = '%4s' desc = '%s'\n",
00062            f.index++, (char *)&f.pixelformat, f.description);
00063     // enumerate frame sizes
00064     v4l2_frmsizeenum fsize;
00065     fsize.index = 0;
00066     fsize.pixel_format = f.pixelformat;
00067     while ((ret = ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &fsize)) == 0)
00068     {
00069       fsize.index++;
00070       if (fsize.type == V4L2_FRMSIZE_TYPE_DISCRETE)
00071       {
00072         printf("  discrete: %ux%u:   ",
00073                fsize.discrete.width, fsize.discrete.height);
00074         // enumerate frame rates
00075         v4l2_frmivalenum fival;
00076         fival.index = 0;
00077         fival.pixel_format = f.pixelformat;
00078         fival.width = fsize.discrete.width;
00079         fival.height = fsize.discrete.height;
00080         while ((ret = ioctl(fd, VIDIOC_ENUM_FRAMEINTERVALS, &fival)) == 0)
00081         {
00082           fival.index++;
00083           if (fival.type == V4L2_FRMIVAL_TYPE_DISCRETE)
00084           {
00085             printf("%u/%u ",
00086                    fival.discrete.numerator, fival.discrete.denominator);
00087           }
00088           else
00089             printf("I only handle discrete frame intervals...\n");
00090         }
00091         printf("\n");
00092       }
00093       else if (fsize.type == V4L2_FRMSIZE_TYPE_CONTINUOUS)
00094       {
00095         printf("  continuous: %ux%u to %ux%u\n",
00096                fsize.stepwise.min_width, fsize.stepwise.min_height,
00097                fsize.stepwise.max_width, fsize.stepwise.max_height);
00098       }
00099       else if (fsize.type == V4L2_FRMSIZE_TYPE_STEPWISE)
00100       {
00101         printf("  stepwise: %ux%u to %ux%u step %ux%u\n",
00102                fsize.stepwise.min_width,  fsize.stepwise.min_height,
00103                fsize.stepwise.max_width,  fsize.stepwise.max_height,
00104                fsize.stepwise.step_width, fsize.stepwise.step_height);
00105       }
00106       else
00107       {
00108         printf("  fsize.type not supported: %d\n", fsize.type);
00109       }
00110     }
00111   }
00112   if (errno != EINVAL)
00113     throw std::runtime_error("error enumerating frame formats");
00114   fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00115   fmt.fmt.pix.width = width;
00116   fmt.fmt.pix.height = height;
00117   printf("mode == RGB: %d, mode == YUYV: %d, mode == MJPEG:%d",mode == MODE_RGB, mode == MODE_YUYV, mode ==MODE_MJPG);
00118   if (mode == MODE_RGB || mode == MODE_YUYV) // we'll convert later
00119     fmt.fmt.pix.pixelformat = 'Y' | ('U' << 8) | ('Y' << 16) | ('V' << 24);
00120   else
00121     fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
00122   fmt.fmt.pix.field = V4L2_FIELD_ANY;
00123   if ((ret = ioctl(fd, VIDIOC_S_FMT, &fmt)) < 0)
00124     throw std::runtime_error("couldn't set format");
00125   if (fmt.fmt.pix.width != width || fmt.fmt.pix.height != height)
00126     throw std::runtime_error("pixel format unavailable");
00127   streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00128   streamparm.parm.capture.timeperframe.numerator = 1;
00129   streamparm.parm.capture.timeperframe.denominator = fps;
00130   if ((ret = ioctl(fd, VIDIOC_S_PARM, &streamparm)) < 0)
00131     throw std::runtime_error("unable to set framerate");
00132   v4l2_queryctrl queryctrl;
00133   memset(&queryctrl, 0, sizeof(queryctrl));
00134   uint32_t i = V4L2_CID_BASE;
00135   while (i != V4L2_CID_LAST_EXTCTR)
00136   {
00137     queryctrl.id = i;
00138     if ((ret = ioctl(fd, VIDIOC_QUERYCTRL, &queryctrl)) == 0 &&
00139         !(queryctrl.flags & V4L2_CTRL_FLAG_DISABLED))
00140     {
00141       const char *ctrl_type = NULL;
00142       if (queryctrl.type == V4L2_CTRL_TYPE_INTEGER)
00143         ctrl_type = "int";
00144       else if (queryctrl.type == V4L2_CTRL_TYPE_BOOLEAN)
00145         ctrl_type = "bool";
00146       else if (queryctrl.type == V4L2_CTRL_TYPE_BUTTON)
00147         ctrl_type = "button";
00148       else if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
00149         ctrl_type = "menu";
00150       printf("  %s (%s, %d, id = %x): %d to %d (%d)\n",
00151              ctrl_type,
00152              queryctrl.name, queryctrl.flags, queryctrl.id,
00153              queryctrl.minimum, queryctrl.maximum, queryctrl.step);
00154       if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
00155       {
00156         v4l2_querymenu querymenu;
00157         memset(&querymenu, 0, sizeof(querymenu));
00158         querymenu.id = queryctrl.id;
00159         querymenu.index = 0;
00160         while (ioctl(fd, VIDIOC_QUERYMENU, &querymenu) == 0)
00161         {
00162           printf("    %d: %s\n", querymenu.index, querymenu.name);
00163           querymenu.index++;
00164         }
00165       }
00166 
00167     }
00168     else if (errno != EINVAL)
00169       throw std::runtime_error("couldn't query control");
00170     i++;
00171     if (i == V4L2_CID_LAST_NEW)
00172       i = V4L2_CID_CAMERA_CLASS_BASE_NEW;
00173     else if (i == V4L2_CID_CAMERA_CLASS_LAST)
00174       i = V4L2_CID_PRIVATE_BASE_OLD;
00175     else if (i == V4L2_CID_PRIVATE_LAST)
00176       i = V4L2_CID_BASE_EXTCTR;
00177   }
00178 
00179   try
00180   {
00181     // the commented labels correspond to the controls in guvcview and uvcdynctrl  
00182 
00183         set_control(V4L2_CID_EXPOSURE_AUTO, 3);
00184     set_control(0x009a0903, 1);
00185    // set_control(10094851, 1); // Exposure, Auto Priority
00186   //  set_control(10094849, 1); // Exposure, Auto
00187   //  set_control(168062321, 0); //Disable video processing
00188   //  set_control(0x9a9010, 100);
00189   //  set_control(V4L2_CID_EXPOSURE_ABSOLUTE_NEW, 300);
00190     set_control(V4L2_CID_BRIGHTNESS, 140);
00191     set_control(V4L2_CID_CONTRAST, 40);
00192     set_control(V4L2_CID_AUTO_WHITE_BALANCE, 1);
00193     set_control(V4L2_CID_GAIN, 120);
00194     set_control(V4L2_CID_SHARPNESS, 140);
00195     set_control(V4L2_CID_SATURATION, 45);
00196  //   set_control(V4L2_CID_WHITE_BALANCE_TEMP_AUTO_OLD, 0);
00197   //  set_control(9963776, 128); //Brightness
00198   //  set_control(9963777, 32); //Contrast
00199   //  set_control(9963788, 1); // White Balance Temperature, Auto
00200   //  set_control(9963802, 5984); // White Balance Temperature
00201   //  set_control(9963800, 2);  // power line frequency to 60 hz
00202 //    set_control(9963795, 200); // Gain
00203 //    set_control(9963803, 224); // Sharpness
00204  //   set_control(9963804, 1); //Backlight Compensation
00205  //  set_control(10094850, 250); // Exposure (Absolute)
00206  //   set_control(168062212, 16); //Focus (absolute)
00207  //   set_control(168062213, 3); //LED1 Mode
00208  //   set_control(168062214, 0); //LED1 Frequency
00209     set_control(9963778, 32); // Saturation
00210   }
00211   catch (std::runtime_error &ex)
00212   {
00213     printf("ERROR: could not set some settings.  \n %s \n", ex.what());
00214   }
00215 
00216 /*
00217   v4l2_jpegcompression v4l2_jpeg;
00218   if (ioctl(fd, VIDIOC_G_JPEGCOMP, &v4l2_jpeg) < 0)
00219     throw std::runtime_error("no jpeg compression iface exposed");
00220   printf("jpeg quality: %d\n", v4l2_jpeg.quality);
00221 */
00222 
00223   memset(&rb, 0, sizeof(rb));
00224   rb.count = NUM_BUFFER;
00225   rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00226   rb.memory = V4L2_MEMORY_MMAP;
00227   if (ioctl(fd, VIDIOC_REQBUFS, &rb) < 0)
00228     throw std::runtime_error("unable to allocate buffers");
00229   for (unsigned i = 0; i < NUM_BUFFER; i++)
00230   {
00231     memset(&buf, 0, sizeof(buf));
00232     buf.index = i;
00233     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00234     buf.flags = V4L2_BUF_FLAG_TIMECODE;
00235     buf.timecode = timecode;
00236     buf.timestamp.tv_sec = 0;
00237     buf.timestamp.tv_usec = 0;
00238     buf.memory = V4L2_MEMORY_MMAP;
00239     if (ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0)
00240       throw std::runtime_error("unable to query buffer");
00241     if (buf.length <= 0)
00242       throw std::runtime_error("buffer length is bogus");
00243     mem[i] = mmap(0, buf.length, PROT_READ, MAP_SHARED, fd, buf.m.offset);
00244     //printf("buf length = %d at %x\n", buf.length, mem[i]);
00245     if (mem[i] == MAP_FAILED)
00246       throw std::runtime_error("couldn't map buffer");
00247   }
00248   buf_length = buf.length;
00249   for (unsigned i = 0; i < NUM_BUFFER; i++)
00250   {
00251     memset(&buf, 0, sizeof(buf));
00252     buf.index = i;
00253     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00254     buf.flags = V4L2_BUF_FLAG_TIMECODE;
00255     buf.timecode = timecode;
00256     buf.timestamp.tv_sec = 0;
00257     buf.timestamp.tv_usec = 0;
00258     buf.memory = V4L2_MEMORY_MMAP;
00259     if (ioctl(fd, VIDIOC_QBUF, &buf) < 0)
00260       throw std::runtime_error("unable to queue buffer");
00261   }
00262   int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00263   if (ioctl(fd, VIDIOC_STREAMON, &type) < 0)
00264     throw std::runtime_error("unable to start capture");
00265   rgb_frame = new unsigned char[width * height * 3];
00266   last_yuv_frame = new unsigned char[width * height * 2];
00267 
00268 
00269  // init_mjpeg_decoder(width, height);
00270   jpegBuf = new unsigned char[width*height*3];
00271 }
00272 
00273 Cam::~Cam()
00274 {
00275   // stop stream
00276   int type = V4L2_BUF_TYPE_VIDEO_CAPTURE, ret;
00277   if ((ret = ioctl(fd, VIDIOC_STREAMOFF, &type)) < 0)
00278     perror("VIDIOC_STREAMOFF");
00279   for (unsigned i = 0; i < NUM_BUFFER; i++)
00280     if (munmap(mem[i], buf_length) < 0)
00281       perror("failed to unmap buffer");
00282   close(fd);
00283   if (rgb_frame)
00284   {
00285     delete[] rgb_frame;
00286     delete[] last_yuv_frame;
00287   }
00288   last_yuv_frame = rgb_frame = NULL;
00289 
00290   if(jpegBuf)
00291     delete [] jpegBuf;
00292 }
00293 
00294 void Cam::enumerate()
00295 {
00296   string v4l_path = "/sys/class/video4linux";
00297   DIR *d = opendir(v4l_path.c_str());
00298   if (!d)
00299     throw std::runtime_error("couldn't open " + v4l_path);
00300   struct dirent *ent, *ent2, *ent3;
00301   int fd, ret;
00302   struct v4l2_capability v4l2_cap;
00303   while ((ent = readdir(d)) != NULL)
00304   {
00305     if (strncmp(ent->d_name, "video", 5))
00306       continue; // ignore anything not starting with "video"
00307     string dev_name = string("/dev/") + string(ent->d_name);
00308     printf("enumerating %s ...\n", dev_name.c_str());
00309     if ((fd = open(dev_name.c_str(), O_RDWR)) == -1)
00310       throw std::runtime_error("couldn't open " + dev_name + "  perhaps the " +
00311                                "permissions are not set correctly?");
00312     if ((ret = ioctl(fd, VIDIOC_QUERYCAP, &v4l2_cap)) < 0)
00313       throw std::runtime_error("couldn't query " + dev_name);
00314     printf("name = [%s]\n", v4l2_cap.card);
00315     printf("driver = [%s]\n", v4l2_cap.driver);
00316     printf("location = [%s]\n", v4l2_cap.bus_info);
00317     close(fd);
00318     string v4l_dev_path = v4l_path + string("/") + string(ent->d_name) +
00319                           string("/device");
00320     // my kernel is using /sys/class/video4linux/videoN/device/inputX/id
00321     DIR *d2 = opendir(v4l_dev_path.c_str());
00322     if (!d2)
00323       throw std::runtime_error("couldn't open " + v4l_dev_path);
00324     string input_dir;
00325     while ((ent2 = readdir(d2)) != NULL)
00326     {
00327       if (strncmp(ent2->d_name, "input", 5))
00328         continue; // ignore anything not beginning with "input"
00329 
00330       DIR *input = opendir((v4l_dev_path + string("/") + string(ent2->d_name)).c_str());
00331       bool output_set = false;
00332       while ((ent3 = readdir(input)) != NULL)
00333       {
00334         if (!strncmp(ent3->d_name, "input", 5))
00335         {
00336           input_dir = (string("input/") + string(ent3->d_name )).c_str();
00337           output_set = true;
00338           break;
00339         }
00340       }
00341       if (!output_set)
00342         input_dir = ent2->d_name;
00343       break;
00344     }
00345     closedir(d2);
00346     if (!input_dir.length())
00347       throw std::runtime_error("couldn't find input dir in " + v4l_dev_path);
00348     string vid_fname = v4l_dev_path + string("/") + input_dir +
00349                        string("/id/vendor");
00350     string pid_fname = v4l_dev_path + string("/") + input_dir +
00351                        string("/id/product");
00352     string ver_fname = v4l_dev_path + string("/") + input_dir +
00353                        string("/id/version");
00354     char vid[5], pid[5], ver[5];
00355     FILE *vid_fp = fopen(vid_fname.c_str(), "r");
00356     if (!vid_fp)
00357       throw std::runtime_error("couldn't open " + vid_fname);
00358     if (!fgets(vid, sizeof(vid), vid_fp))
00359       throw std::runtime_error("couldn't read VID from " + vid_fname);
00360     fclose(vid_fp);
00361     vid[4] = 0;
00362     printf("vid = [%s]\n", vid);
00363     FILE *pid_fp = fopen(pid_fname.c_str(), "r");
00364     if (!pid_fp)
00365       throw std::runtime_error("couldn't open " + pid_fname);
00366     if (!fgets(pid, sizeof(pid), pid_fp))
00367       throw std::runtime_error("couldn't read PID from " + pid_fname);
00368     fclose(pid_fp);
00369     printf("pid = [%s]\n", pid);
00370     FILE *ver_fp = fopen(ver_fname.c_str(), "r");
00371     if (!ver_fp)
00372       throw std::runtime_error("couldn't open " + ver_fname);
00373     if (!fgets(ver, sizeof(ver), ver_fp))
00374       throw std::runtime_error("couldn't read version from " + ver_fname);
00375     fclose(ver_fp);
00376     printf("ver = [%s]\n", ver);
00377   }
00378   closedir(d);
00379 }
00380 
00381 // saturate input into [0, 255]
00382 inline unsigned char sat(float f)
00383 {
00384   return (unsigned char)( f >= 255 ? 255 : (f < 0 ? 0 : f));
00385 }
00386 
00387 int Cam::grab(unsigned char **frame, uint32_t &bytes_used)
00388 {
00389   *frame = NULL;
00390   int ret = 0;
00391   fd_set rdset;
00392   timeval timeout;
00393   FD_ZERO(&rdset);
00394   FD_SET(fd, &rdset);
00395   timeout.tv_sec = 1;
00396   timeout.tv_usec = 0;
00397   bytes_used = 0;
00398   ret = select(fd + 1, &rdset, NULL, NULL, &timeout);
00399   if (ret == 0)
00400   {
00401     printf("select timeout in grab\n");
00402     return -1;
00403   }
00404   else if (ret < 0)
00405   {
00406     perror("couldn't grab image");
00407     return -1;
00408   }
00409   if (!FD_ISSET(fd, &rdset))
00410     return -1;
00411   memset(&buf, 0, sizeof(buf));
00412   buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00413   buf.memory = V4L2_MEMORY_MMAP;
00414   if (ioctl(fd, VIDIOC_DQBUF, &buf) < 0)
00415     throw std::runtime_error("couldn't dequeue buffer");
00416   bytes_used = buf.bytesused;
00417   if (mode == MODE_RGB)
00418   {
00419     int num_pixels_different = 0; // just look at the Y channel
00420     unsigned char *pyuv = (unsigned char *)mem[buf.index];
00421     // yuyv is 2 bytes per pixel. step through every pixel pair.
00422     unsigned char *prgb = rgb_frame;
00423     unsigned char *pyuv_last = last_yuv_frame;
00424     for (unsigned i = 0; i < width * height * 2; i += 4)
00425     {
00426       *prgb++ = sat(pyuv[i]+1.402f  *(pyuv[i+3]-128));
00427       *prgb++ = sat(pyuv[i]-0.34414f*(pyuv[i+1]-128)-0.71414f*(pyuv[i+3]-128));
00428       *prgb++ = sat(pyuv[i]+1.772f  *(pyuv[i+1]-128));
00429       *prgb++ = sat(pyuv[i+2]+1.402f*(pyuv[i+3]-128));
00430       *prgb++ = sat(pyuv[i+2]-0.34414f*(pyuv[i+1]-128)-0.71414f*(pyuv[i+3]-128));
00431       *prgb++ = sat(pyuv[i+2]+1.772f*(pyuv[i+1]-128));
00432       if ((int)pyuv[i] - (int)pyuv_last[i] > motion_threshold_luminance ||
00433           (int)pyuv_last[i] - (int)pyuv[i] > motion_threshold_luminance)
00434         num_pixels_different++;
00435       if ((int)pyuv[i+2] - (int)pyuv_last[i+2] > motion_threshold_luminance ||
00436           (int)pyuv_last[i+2] - (int)pyuv[i+2] > motion_threshold_luminance)
00437         num_pixels_different++;
00438 
00439       // this gives bgr images...
00440       /*
00441       *prgb++ = sat(pyuv[i]+1.772f  *(pyuv[i+1]-128));
00442       *prgb++ = sat(pyuv[i]-0.34414f*(pyuv[i+1]-128)-0.71414f*(pyuv[i+3]-128));
00443       *prgb++ = sat(pyuv[i]+1.402f  *(pyuv[i+3]-128));
00444       *prgb++ = sat(pyuv[i+2]+1.772f*(pyuv[i+1]-128));
00445       *prgb++ = sat(pyuv[i+2]-0.34414f*(pyuv[i+1]-128)-0.71414f*(pyuv[i+3]-128));
00446       *prgb++ = sat(pyuv[i+2]+1.402f*(pyuv[i+3]-128));
00447       */
00448     }
00449   //  memcpy(last_yuv_frame, pyuv, width * height * 2);
00450     if (num_pixels_different > motion_threshold_count) // default: always true
00451       *frame = rgb_frame;
00452     else
00453     {
00454       *frame = NULL; // not enough luminance change
00455       release(buf.index); // let go of this image
00456     }
00457   }
00458   else if (mode == MODE_YUYV)
00459   {
00460     *frame = (uint8_t *)mem[buf.index];
00461   }
00462   else // mode == MODE_JPEG
00463   {
00464     if (bytes_used > 100)
00465     {  printf("frame!\n");
00466     //  mjpeg2rgb((char*)mem[buf.index], bytes_used, (char*)jpegBuf, width*height);
00467         *frame = (unsigned char*)mem[buf.index];
00468         }
00469   }
00470   return buf.index;
00471 }
00472 
00473 void Cam::release(unsigned buf_idx)
00474 {
00475   if (buf_idx < NUM_BUFFER)
00476     if (ioctl(fd, VIDIOC_QBUF, &buf) < 0)
00477       throw std::runtime_error("couldn't requeue buffer");
00478 }
00479 
00480 void Cam::set_control(uint32_t id, int val)
00481 {
00482   v4l2_control c;
00483   c.id = id;
00484 
00485   if (ioctl(fd, VIDIOC_G_CTRL, &c) == 0)
00486   {
00487     printf("current value of %d is %d\n", id, c.value);
00488     /*
00489     perror("unable to get control");
00490     throw std::runtime_error("unable to get control");
00491     */
00492   }
00493   c.value = val;
00494   if (ioctl(fd, VIDIOC_S_CTRL, &c) < 0)
00495   {
00496     perror("unable to set control");
00497     throw std::runtime_error("unable to set control");
00498   }
00499 }
00500 
00501 void Cam::set_motion_thresholds(int lum, int count)
00502 {
00503   motion_threshold_luminance = lum;
00504   motion_threshold_count = count;
00505 }
00506 


corobot_camera
Author(s):
autogenerated on Wed Aug 26 2015 11:09:42