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


uvc_camera
Author(s): Ken Tossell
autogenerated on Thu Jun 15 2017 02:29:40