camera_driver.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (C) 2012 Ken Tossell
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the author nor other contributors may be
00018 *     used to endorse or promote products derived from this software
00019 *     without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #include "libuvc_camera/camera_driver.h"
00035 
00036 #include <ros/ros.h>
00037 #include <sensor_msgs/Image.h>
00038 #include <std_msgs/Header.h>
00039 #include <image_transport/camera_publisher.h>
00040 #include <dynamic_reconfigure/server.h>
00041 #include <libuvc/libuvc.h>
00042 #define libuvc_VERSION (libuvc_VERSION_MAJOR * 10000 \
00043                       + libuvc_VERSION_MINOR * 100 \
00044                       + libuvc_VERSION_PATCH)
00045 
00046 namespace libuvc_camera {
00047 
00048 CameraDriver::CameraDriver(ros::NodeHandle nh, ros::NodeHandle priv_nh)
00049   : nh_(nh), priv_nh_(priv_nh),
00050     state_(kInitial),
00051     ctx_(NULL), dev_(NULL), devh_(NULL), rgb_frame_(NULL),
00052     it_(nh_),
00053     config_server_(mutex_, priv_nh_),
00054     config_changed_(false),
00055     cinfo_manager_(nh) {
00056   cam_pub_ = it_.advertiseCamera("image_raw", 1, false);
00057 }
00058 
00059 CameraDriver::~CameraDriver() {
00060   if (rgb_frame_)
00061     uvc_free_frame(rgb_frame_);
00062 
00063   if (ctx_)
00064     uvc_exit(ctx_);  // Destroys dev_, devh_, etc.
00065 }
00066 
00067 bool CameraDriver::Start() {
00068   assert(state_ == kInitial);
00069 
00070   uvc_error_t err;
00071 
00072   err = uvc_init(&ctx_, NULL);
00073 
00074   if (err != UVC_SUCCESS) {
00075     uvc_perror(err, "ERROR: uvc_init");
00076     return false;
00077   }
00078 
00079   state_ = kStopped;
00080 
00081   config_server_.setCallback(boost::bind(&CameraDriver::ReconfigureCallback, this, _1, _2));
00082 
00083   return state_ == kRunning;
00084 }
00085 
00086 void CameraDriver::Stop() {
00087   boost::recursive_mutex::scoped_lock lock(mutex_);
00088 
00089   assert(state_ != kInitial);
00090 
00091   if (state_ == kRunning)
00092     CloseCamera();
00093 
00094   assert(state_ == kStopped);
00095 
00096   uvc_exit(ctx_);
00097   ctx_ = NULL;
00098 
00099   state_ = kInitial;
00100 }
00101 
00102 void CameraDriver::ReconfigureCallback(UVCCameraConfig &new_config, uint32_t level) {
00103   boost::recursive_mutex::scoped_lock lock(mutex_);
00104 
00105   if ((level & kReconfigureClose) == kReconfigureClose) {
00106     if (state_ == kRunning)
00107       CloseCamera();
00108   }
00109 
00110   if (state_ == kStopped) {
00111     OpenCamera(new_config);
00112   }
00113 
00114   if (new_config.camera_info_url != config_.camera_info_url)
00115     cinfo_manager_.loadCameraInfo(new_config.camera_info_url);
00116 
00117   if (state_ == kRunning) {
00118 #define PARAM_INT(name, fn, value) if (new_config.name != config_.name) { \
00119       int val = (value);                                                \
00120       if (uvc_set_##fn(devh_, val)) {                                   \
00121         ROS_WARN("Unable to set " #name " to %d", val);                 \
00122         new_config.name = config_.name;                                 \
00123       }                                                                 \
00124     }
00125 
00126     PARAM_INT(scanning_mode, scanning_mode, new_config.scanning_mode);
00127     PARAM_INT(auto_exposure, ae_mode, 1 << new_config.auto_exposure);
00128     PARAM_INT(auto_exposure_priority, ae_priority, new_config.auto_exposure_priority);
00129     PARAM_INT(exposure_absolute, exposure_abs, new_config.exposure_absolute * 10000);
00130     PARAM_INT(auto_focus, focus_auto, new_config.auto_focus ? 1 : 0);
00131     PARAM_INT(focus_absolute, focus_abs, new_config.focus_absolute);
00132 #if libuvc_VERSION     > 00005 /* version > 0.0.5 */
00133     PARAM_INT(gain, gain, new_config.gain);
00134     PARAM_INT(iris_absolute, iris_abs, new_config.iris_absolute);
00135     PARAM_INT(brightness, brightness, new_config.brightness);
00136 #endif
00137     
00138 
00139     if (new_config.pan_absolute != config_.pan_absolute || new_config.tilt_absolute != config_.tilt_absolute) {
00140       if (uvc_set_pantilt_abs(devh_, new_config.pan_absolute, new_config.tilt_absolute)) {
00141         ROS_WARN("Unable to set pantilt to %d, %d", new_config.pan_absolute, new_config.tilt_absolute);
00142         new_config.pan_absolute = config_.pan_absolute;
00143         new_config.tilt_absolute = config_.tilt_absolute;
00144       }
00145     }
00146     // TODO: roll_absolute
00147     // TODO: privacy
00148     // TODO: backlight_compensation
00149     // TODO: contrast
00150     // TODO: power_line_frequency
00151     // TODO: auto_hue
00152     // TODO: saturation
00153     // TODO: sharpness
00154     // TODO: gamma
00155     // TODO: auto_white_balance
00156     // TODO: white_balance_temperature
00157     // TODO: white_balance_BU
00158     // TODO: white_balance_RV
00159   }
00160 
00161   config_ = new_config;
00162 }
00163 
00164 void CameraDriver::ImageCallback(uvc_frame_t *frame) {
00165   ros::Time timestamp = ros::Time(frame->capture_time.tv_sec, frame->capture_time.tv_usec * 1000);
00166   if ( timestamp == ros::Time(0) ) {
00167     timestamp = ros::Time::now();
00168   }
00169 
00170   boost::recursive_mutex::scoped_lock lock(mutex_);
00171 
00172   assert(state_ == kRunning);
00173   assert(rgb_frame_);
00174 
00175   sensor_msgs::Image::Ptr image(new sensor_msgs::Image());
00176   image->width = config_.width;
00177   image->height = config_.height;
00178   image->step = image->width * 3;
00179   image->data.resize(image->step * image->height);
00180 
00181   if (frame->frame_format == UVC_FRAME_FORMAT_BGR){
00182     image->encoding = "bgr8";
00183     memcpy(&(image->data[0]), frame->data, frame->data_bytes);
00184   } else if (frame->frame_format == UVC_FRAME_FORMAT_RGB){
00185     image->encoding = "rgb8";
00186     memcpy(&(image->data[0]), frame->data, frame->data_bytes);
00187   } else if (frame->frame_format == UVC_FRAME_FORMAT_UYVY) {
00188     uvc_error_t conv_ret = uvc_uyvy2bgr(frame, rgb_frame_);
00189     if (conv_ret != UVC_SUCCESS) {
00190       uvc_perror(conv_ret, "Couldn't convert frame to RGB");
00191       return;
00192     }
00193     image->encoding = "bgr8";
00194     memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes);
00195   } else if (frame->frame_format == UVC_FRAME_FORMAT_GRAY8) {
00196     image->encoding = "8UC1";
00197     image->step = image->width;
00198     image->data.resize(image->step * image->height);
00199     memcpy(&(image->data[0]), frame->data, frame->data_bytes);
00200   } else if (frame->frame_format == UVC_FRAME_FORMAT_GRAY16) {
00201     image->encoding = "16UC1";
00202     image->step = image->width*2;
00203     image->data.resize(image->step * image->height);
00204     memcpy(&(image->data[0]), frame->data, frame->data_bytes);
00205   } else if (frame->frame_format == UVC_FRAME_FORMAT_YUYV) {
00206     // FIXME: uvc_any2bgr does not work on "yuyv" format, so use uvc_yuyv2bgr directly.
00207     uvc_error_t conv_ret = uvc_yuyv2bgr(frame, rgb_frame_);
00208     if (conv_ret != UVC_SUCCESS) {
00209       uvc_perror(conv_ret, "Couldn't convert frame to RGB");
00210       return;
00211     }
00212     image->encoding = "bgr8";
00213     memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes);
00214 #if libuvc_VERSION     > 00005 /* version > 0.0.5 */
00215   } else if (frame->frame_format == UVC_FRAME_FORMAT_MJPEG) {
00216     // Enable mjpeg support despite uvs_any2bgr shortcoming
00217     //  https://github.com/ros-drivers/libuvc_ros/commit/7508a09f
00218     uvc_error_t conv_ret = uvc_mjpeg2rgb(frame, rgb_frame_);
00219     if (conv_ret != UVC_SUCCESS) {
00220       uvc_perror(conv_ret, "Couldn't convert frame to RGB");
00221       return;
00222     }
00223     image->encoding = "rgb8";
00224     memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes);
00225 #endif
00226   } else {
00227     uvc_error_t conv_ret = uvc_any2bgr(frame, rgb_frame_);
00228     if (conv_ret != UVC_SUCCESS) {
00229       uvc_perror(conv_ret, "Couldn't convert frame to RGB");
00230       return;
00231     }
00232     image->encoding = "bgr8";
00233     memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes);
00234   }
00235 
00236 
00237   sensor_msgs::CameraInfo::Ptr cinfo(
00238     new sensor_msgs::CameraInfo(cinfo_manager_.getCameraInfo()));
00239 
00240   image->header.frame_id = config_.frame_id;
00241   image->header.stamp = timestamp;
00242   cinfo->header.frame_id = config_.frame_id;
00243   cinfo->header.stamp = timestamp;
00244 
00245   cam_pub_.publish(image, cinfo);
00246 
00247   if (config_changed_) {
00248     config_server_.updateConfig(config_);
00249     config_changed_ = false;
00250   }
00251 }
00252 
00253 /* static */ void CameraDriver::ImageCallbackAdapter(uvc_frame_t *frame, void *ptr) {
00254   CameraDriver *driver = static_cast<CameraDriver*>(ptr);
00255 
00256   driver->ImageCallback(frame);
00257 }
00258 
00259 void CameraDriver::AutoControlsCallback(
00260   enum uvc_status_class status_class,
00261   int event,
00262   int selector,
00263   enum uvc_status_attribute status_attribute,
00264   void *data, size_t data_len) {
00265   boost::recursive_mutex::scoped_lock lock(mutex_);
00266 
00267   printf("Controls callback. class: %d, event: %d, selector: %d, attr: %d, data_len: %zu\n",
00268          status_class, event, selector, status_attribute, data_len);
00269 
00270   if (status_attribute == UVC_STATUS_ATTRIBUTE_VALUE_CHANGE) {
00271     switch (status_class) {
00272     case UVC_STATUS_CLASS_CONTROL_CAMERA: {
00273       switch (selector) {
00274       case UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL:
00275         uint8_t *data_char = (uint8_t*) data;
00276         uint32_t exposure_int = ((data_char[0]) | (data_char[1] << 8) |
00277                                  (data_char[2] << 16) | (data_char[3] << 24));
00278         config_.exposure_absolute = exposure_int * 0.0001;
00279         config_changed_ = true;
00280         break;
00281       }
00282       break;
00283     }
00284     case UVC_STATUS_CLASS_CONTROL_PROCESSING: {
00285       switch (selector) {
00286       case UVC_PU_WHITE_BALANCE_TEMPERATURE_CONTROL:
00287         uint8_t *data_char = (uint8_t*) data;
00288         config_.white_balance_temperature = 
00289           data_char[0] | (data_char[1] << 8);
00290         config_changed_ = true;
00291         break;
00292       }
00293       break;
00294     }
00295     }
00296 
00297     // config_server_.updateConfig(config_);
00298   }
00299 }
00300 
00301 /* static */ void CameraDriver::AutoControlsCallbackAdapter(
00302   enum uvc_status_class status_class,
00303   int event,
00304   int selector,
00305   enum uvc_status_attribute status_attribute,
00306   void *data, size_t data_len,
00307   void *ptr) {
00308   CameraDriver *driver = static_cast<CameraDriver*>(ptr);
00309 
00310   driver->AutoControlsCallback(status_class, event, selector,
00311                                status_attribute, data, data_len);
00312 }
00313 
00314 enum uvc_frame_format CameraDriver::GetVideoMode(std::string vmode){
00315   if(vmode == "uncompressed") {
00316     return UVC_COLOR_FORMAT_UNCOMPRESSED;
00317   } else if (vmode == "compressed") {
00318     return UVC_COLOR_FORMAT_COMPRESSED;
00319   } else if (vmode == "yuyv") {
00320     return UVC_COLOR_FORMAT_YUYV;
00321   } else if (vmode == "uyvy") {
00322     return UVC_COLOR_FORMAT_UYVY;
00323   } else if (vmode == "rgb") {
00324     return UVC_COLOR_FORMAT_RGB;
00325   } else if (vmode == "bgr") {
00326     return UVC_COLOR_FORMAT_BGR;
00327   } else if (vmode == "mjpeg") {
00328     return UVC_COLOR_FORMAT_MJPEG;
00329   } else if (vmode == "gray8") {
00330     return UVC_COLOR_FORMAT_GRAY8;
00331   } else if (vmode == "gray16") {
00332     return UVC_COLOR_FORMAT_GRAY16;
00333   } else {
00334     ROS_ERROR_STREAM("Invalid Video Mode: " << vmode);
00335     ROS_WARN_STREAM("Continue using video mode: uncompressed");
00336     return UVC_COLOR_FORMAT_UNCOMPRESSED;
00337   }
00338 };
00339 
00340 void CameraDriver::OpenCamera(UVCCameraConfig &new_config) {
00341   assert(state_ == kStopped);
00342 
00343   int vendor_id = strtol(new_config.vendor.c_str(), NULL, 0);
00344   int product_id = strtol(new_config.product.c_str(), NULL, 0);
00345 
00346   ROS_INFO("Opening camera with vendor=0x%x, product=0x%x, serial=\"%s\", index=%d",
00347            vendor_id, product_id, new_config.serial.c_str(), new_config.index);
00348 
00349   uvc_device_t **devs;
00350 
00351   // Implement missing index select behavior
00352   // https://github.com/ros-drivers/libuvc_ros/commit/4f30e9a0
00353 #if libuvc_VERSION     > 00005 /* version > 0.0.5 */
00354   uvc_error_t find_err = uvc_find_devices(
00355     ctx_, &devs,
00356     vendor_id,
00357     product_id,
00358     new_config.serial.empty() ? NULL : new_config.serial.c_str());
00359 
00360   if (find_err != UVC_SUCCESS) {
00361     uvc_perror(find_err, "uvc_find_device");
00362     return;
00363   }
00364 
00365   // select device by index
00366   dev_ = NULL;
00367   int dev_idx = 0;
00368   while (devs[dev_idx] != NULL) {
00369     if(dev_idx == new_config.index) {
00370       dev_ = devs[dev_idx];
00371     }
00372     else {
00373       uvc_unref_device(devs[dev_idx]);
00374     }
00375 
00376     dev_idx++;
00377   }
00378 
00379   if(dev_ == NULL) {
00380     ROS_ERROR("Unable to find device at index %d", new_config.index);
00381     return;
00382   }
00383 #else
00384   uvc_error_t find_err = uvc_find_device(
00385     ctx_, &dev_,
00386     vendor_id,
00387     product_id,
00388     new_config.serial.empty() ? NULL : new_config.serial.c_str());
00389 
00390   if (find_err != UVC_SUCCESS) {
00391     uvc_perror(find_err, "uvc_find_device");
00392     return;
00393   }
00394 
00395 #endif
00396   uvc_error_t open_err = uvc_open(dev_, &devh_);
00397 
00398   if (open_err != UVC_SUCCESS) {
00399     switch (open_err) {
00400     case UVC_ERROR_ACCESS:
00401 #ifdef __linux__
00402       ROS_ERROR("Permission denied opening /dev/bus/usb/%03d/%03d",
00403                 uvc_get_bus_number(dev_), uvc_get_device_address(dev_));
00404 #else
00405       ROS_ERROR("Permission denied opening device %d on bus %d",
00406                 uvc_get_device_address(dev_), uvc_get_bus_number(dev_));
00407 #endif
00408       break;
00409     default:
00410 #ifdef __linux__
00411       ROS_ERROR("Can't open /dev/bus/usb/%03d/%03d: %s (%d)",
00412                 uvc_get_bus_number(dev_), uvc_get_device_address(dev_),
00413                 uvc_strerror(open_err), open_err);
00414 #else
00415       ROS_ERROR("Can't open device %d on bus %d: %s (%d)",
00416                 uvc_get_device_address(dev_), uvc_get_bus_number(dev_),
00417                 uvc_strerror(open_err), open_err);
00418 #endif
00419       break;
00420     }
00421 
00422     uvc_unref_device(dev_);
00423     return;
00424   }
00425 
00426   uvc_set_status_callback(devh_, &CameraDriver::AutoControlsCallbackAdapter, this);
00427 
00428   uvc_stream_ctrl_t ctrl;
00429   uvc_error_t mode_err = uvc_get_stream_ctrl_format_size(
00430     devh_, &ctrl,
00431     GetVideoMode(new_config.video_mode),
00432     new_config.width, new_config.height,
00433     new_config.frame_rate);
00434 
00435   if (mode_err != UVC_SUCCESS) {
00436     uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
00437     uvc_close(devh_);
00438     uvc_unref_device(dev_);
00439     ROS_ERROR("check video_mode/width/height/frame_rate are available");
00440     uvc_print_diag(devh_, NULL);
00441     return;
00442   }
00443 
00444   uvc_error_t stream_err = uvc_start_streaming(devh_, &ctrl, &CameraDriver::ImageCallbackAdapter, this, 0);
00445 
00446   if (stream_err != UVC_SUCCESS) {
00447     uvc_perror(stream_err, "uvc_start_streaming");
00448     uvc_close(devh_);
00449     uvc_unref_device(dev_);
00450     return;
00451   }
00452 
00453   if (rgb_frame_)
00454     uvc_free_frame(rgb_frame_);
00455 
00456   rgb_frame_ = uvc_allocate_frame(new_config.width * new_config.height * 3);
00457   assert(rgb_frame_);
00458 
00459   state_ = kRunning;
00460 }
00461 
00462 void CameraDriver::CloseCamera() {
00463   assert(state_ == kRunning);
00464 
00465   uvc_close(devh_);
00466   devh_ = NULL;
00467 
00468   uvc_unref_device(dev_);
00469   dev_ = NULL;
00470 
00471   state_ = kStopped;
00472 }
00473 
00474 };


libuvc_camera
Author(s):
autogenerated on Tue Jul 9 2019 03:20:57