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 
00043 namespace libuvc_camera {
00044 
00045 CameraDriver::CameraDriver(ros::NodeHandle nh, ros::NodeHandle priv_nh)
00046   : nh_(nh), priv_nh_(priv_nh),
00047     state_(kInitial),
00048     ctx_(NULL), dev_(NULL), devh_(NULL), rgb_frame_(NULL),
00049     it_(nh_),
00050     config_server_(mutex_, priv_nh_),
00051     config_changed_(false),
00052     cinfo_manager_(nh) {
00053   cam_pub_ = it_.advertiseCamera("image_raw", 1, false);
00054 }
00055 
00056 CameraDriver::~CameraDriver() {
00057   if (rgb_frame_)
00058     uvc_free_frame(rgb_frame_);
00059 
00060   if (ctx_)
00061     uvc_exit(ctx_);  // Destroys dev_, devh_, etc.
00062 }
00063 
00064 bool CameraDriver::Start() {
00065   assert(state_ == kInitial);
00066 
00067   uvc_error_t err;
00068 
00069   err = uvc_init(&ctx_, NULL);
00070 
00071   if (err != UVC_SUCCESS) {
00072     uvc_perror(err, "ERROR: uvc_init");
00073     return false;
00074   }
00075 
00076   state_ = kStopped;
00077 
00078   config_server_.setCallback(boost::bind(&CameraDriver::ReconfigureCallback, this, _1, _2));
00079 
00080   return state_ == kRunning;
00081 }
00082 
00083 void CameraDriver::Stop() {
00084   boost::recursive_mutex::scoped_lock(mutex_);
00085 
00086   assert(state_ != kInitial);
00087 
00088   if (state_ == kRunning)
00089     CloseCamera();
00090 
00091   assert(state_ == kStopped);
00092 
00093   uvc_exit(ctx_);
00094   ctx_ = NULL;
00095 
00096   state_ = kInitial;
00097 }
00098 
00099 void CameraDriver::ReconfigureCallback(UVCCameraConfig &new_config, uint32_t level) {
00100   boost::recursive_mutex::scoped_lock(mutex_);
00101 
00102   if ((level & kReconfigureClose) == kReconfigureClose) {
00103     if (state_ == kRunning)
00104       CloseCamera();
00105   }
00106 
00107   if (state_ == kStopped) {
00108     OpenCamera(new_config);
00109   }
00110 
00111   if (new_config.camera_info_url != config_.camera_info_url)
00112     cinfo_manager_.loadCameraInfo(new_config.camera_info_url);
00113 
00114   if (state_ == kRunning) {
00115 #define PARAM_INT(name, fn, value) if (new_config.name != config_.name) { \
00116       int val = (value);                                                \
00117       if (uvc_set_##fn(devh_, val)) {                                   \
00118         ROS_WARN("Unable to set " #name " to %d", val);                 \
00119         new_config.name = config_.name;                                 \
00120       }                                                                 \
00121     }
00122 
00123     PARAM_INT(scanning_mode, scanning_mode, new_config.scanning_mode);
00124     PARAM_INT(auto_exposure, ae_mode, 1 << new_config.auto_exposure);
00125     PARAM_INT(auto_exposure_priority, ae_priority, new_config.auto_exposure_priority);
00126     PARAM_INT(exposure_absolute, exposure_abs, new_config.exposure_absolute * 10000);
00127     PARAM_INT(auto_focus, focus_auto, new_config.auto_focus ? 1 : 0);
00128     PARAM_INT(focus_absolute, focus_abs, new_config.focus_absolute);
00129     PARAM_INT(gain, gain, new_config.gain);
00130     PARAM_INT(iris_absolute, iris_abs, new_config.iris_absolute);
00131     PARAM_INT(brightness, brightness, new_config.brightness);
00132     
00133 
00134     if (new_config.pan_absolute != config_.pan_absolute || new_config.tilt_absolute != config_.tilt_absolute) {
00135       if (uvc_set_pantilt_abs(devh_, new_config.pan_absolute, new_config.tilt_absolute)) {
00136         ROS_WARN("Unable to set pantilt to %d, %d", new_config.pan_absolute, new_config.tilt_absolute);
00137         new_config.pan_absolute = config_.pan_absolute;
00138         new_config.tilt_absolute = config_.tilt_absolute;
00139       }
00140     }
00141     // TODO: roll_absolute
00142     // TODO: privacy
00143     // TODO: backlight_compensation
00144     // TODO: contrast
00145     // TODO: power_line_frequency
00146     // TODO: auto_hue
00147     // TODO: saturation
00148     // TODO: sharpness
00149     // TODO: gamma
00150     // TODO: auto_white_balance
00151     // TODO: white_balance_temperature
00152     // TODO: white_balance_BU
00153     // TODO: white_balance_RV
00154   }
00155 
00156   config_ = new_config;
00157 }
00158 
00159 void CameraDriver::ImageCallback(uvc_frame_t *frame) {
00160   // TODO: Switch to {frame}'s timestamp once that becomes reliable.
00161   ros::Time timestamp = ros::Time::now();
00162 
00163   boost::recursive_mutex::scoped_lock(mutex_);
00164 
00165   assert(state_ == kRunning);
00166   assert(rgb_frame_);
00167 
00168   uvc_error_t conv_ret = uvc_any2rgb(frame, rgb_frame_);
00169 
00170   if (conv_ret != UVC_SUCCESS) {
00171     uvc_perror(conv_ret, "Couldn't convert frame to RGB");
00172     return;
00173   }
00174 
00175   sensor_msgs::Image::Ptr image(new sensor_msgs::Image());
00176   image->width = config_.width;
00177   image->height = config_.height;
00178   image->encoding = "rgb8";
00179   image->step = image->width * 3;
00180   image->data.resize(image->step * image->height);
00181   memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes);
00182 
00183   sensor_msgs::CameraInfo::Ptr cinfo(
00184     new sensor_msgs::CameraInfo(cinfo_manager_.getCameraInfo()));
00185 
00186   image->header.frame_id = config_.frame_id;
00187   image->header.stamp = timestamp;
00188   cinfo->header.frame_id = config_.frame_id;
00189   cinfo->header.stamp = timestamp;
00190 
00191   cam_pub_.publish(image, cinfo);
00192 
00193   if (config_changed_) {
00194     config_server_.updateConfig(config_);
00195     config_changed_ = false;
00196   }
00197 }
00198 
00199 /* static */ void CameraDriver::ImageCallbackAdapter(uvc_frame_t *frame, void *ptr) {
00200   CameraDriver *driver = static_cast<CameraDriver*>(ptr);
00201 
00202   driver->ImageCallback(frame);
00203 }
00204 
00205 void CameraDriver::AutoControlsCallback(
00206   enum uvc_status_class status_class,
00207   int event,
00208   int selector,
00209   enum uvc_status_attribute status_attribute,
00210   void *data, size_t data_len) {
00211   boost::recursive_mutex::scoped_lock(mutex_);
00212 
00213   printf("Controls callback. class: %d, event: %d, selector: %d, attr: %d, data_len: %u\n",
00214          status_class, event, selector, status_attribute, data_len);
00215 
00216   if (status_attribute == UVC_STATUS_ATTRIBUTE_VALUE_CHANGE) {
00217     switch (status_class) {
00218     case UVC_STATUS_CLASS_CONTROL_CAMERA: {
00219       switch (selector) {
00220       case UVC_CT_EXPOSURE_TIME_ABSOLUTE_CONTROL:
00221         uint8_t *data_char = (uint8_t*) data;
00222         uint32_t exposure_int = ((data_char[0]) | (data_char[1] << 8) |
00223                                  (data_char[2] << 16) | (data_char[3] << 24));
00224         config_.exposure_absolute = exposure_int * 0.0001;
00225         config_changed_ = true;
00226         break;
00227       }
00228       break;
00229     }
00230     case UVC_STATUS_CLASS_CONTROL_PROCESSING: {
00231       switch (selector) {
00232       case UVC_PU_WHITE_BALANCE_TEMPERATURE_CONTROL:
00233         uint8_t *data_char = (uint8_t*) data;
00234         config_.white_balance_temperature = 
00235           data_char[0] | (data_char[1] << 8);
00236         config_changed_ = true;
00237         break;
00238       }
00239       break;
00240     }
00241     }
00242 
00243     // config_server_.updateConfig(config_);
00244   }
00245 }
00246 
00247 /* static */ void CameraDriver::AutoControlsCallbackAdapter(
00248   enum uvc_status_class status_class,
00249   int event,
00250   int selector,
00251   enum uvc_status_attribute status_attribute,
00252   void *data, size_t data_len,
00253   void *ptr) {
00254   CameraDriver *driver = static_cast<CameraDriver*>(ptr);
00255 
00256   driver->AutoControlsCallback(status_class, event, selector,
00257                                status_attribute, data, data_len);
00258 }
00259 
00260 void CameraDriver::OpenCamera(UVCCameraConfig &new_config) {
00261   assert(state_ == kStopped);
00262 
00263   int vendor_id = strtol(new_config.vendor.c_str(), NULL, 0);
00264   int product_id = strtol(new_config.product.c_str(), NULL, 0);
00265 
00266   ROS_INFO("Opening camera with vendor=0x%x, product=0x%x, serial=\"%s\", index=%d",
00267            vendor_id, product_id, new_config.serial.c_str(), new_config.index);
00268 
00269   uvc_error_t find_err = uvc_find_device(
00270     ctx_, &dev_,
00271     vendor_id,
00272     product_id,
00273     new_config.serial.empty() ? NULL : new_config.serial.c_str());
00274 
00275   // TODO: index
00276 
00277   if (find_err != UVC_SUCCESS) {
00278     uvc_perror(find_err, "uvc_find_device");
00279     return;
00280   }
00281 
00282   uvc_error_t open_err = uvc_open(dev_, &devh_);
00283   if (open_err != UVC_SUCCESS) {
00284     switch (open_err) {
00285     case UVC_ERROR_ACCESS:
00286 #ifdef __linux__
00287       ROS_ERROR("Permission denied opening /dev/bus/usb/%03d/%03d",
00288                 uvc_get_bus_number(dev_), uvc_get_device_address(dev_));
00289 #else
00290       ROS_ERROR("Permission denied opening device %d on bus %d",
00291                 uvc_get_device_address(dev_), uvc_get_bus_number(dev_));
00292 #endif
00293       break;
00294     default:
00295 #ifdef __linux__
00296       ROS_ERROR("Can't open /dev/bus/usb/%03d/%03d: %s (%d)",
00297                 uvc_get_bus_number(dev_), uvc_get_device_address(dev_),
00298                 uvc_strerror(open_err), open_err);
00299 #else
00300       ROS_ERROR("Can't open device %d on bus %d: %s (%d)",
00301                 uvc_get_device_address(dev_), uvc_get_bus_number(dev_),
00302                 uvc_strerror(open_err), open_err);
00303 #endif
00304       break;
00305     }
00306 
00307     uvc_unref_device(dev_);
00308     return;
00309   }
00310 
00311   uvc_set_status_callback(devh_, &CameraDriver::AutoControlsCallbackAdapter, this);
00312 
00313   uvc_stream_ctrl_t ctrl;
00314   uvc_error_t mode_err = uvc_get_stream_ctrl_format_size(
00315     devh_, &ctrl,
00316     UVC_COLOR_FORMAT_UNCOMPRESSED,
00317     new_config.width, new_config.height,
00318     new_config.frame_rate);
00319 
00320   if (mode_err != UVC_SUCCESS) {
00321     uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
00322     uvc_close(devh_);
00323     uvc_unref_device(dev_);
00324     return;
00325   }
00326 
00327   uvc_error_t stream_err = uvc_start_iso_streaming(devh_, &ctrl, &CameraDriver::ImageCallbackAdapter, this);
00328 
00329   if (stream_err != UVC_SUCCESS) {
00330     uvc_perror(stream_err, "uvc_start_iso_streaming");
00331     uvc_close(devh_);
00332     uvc_unref_device(dev_);
00333     return;
00334   }
00335 
00336   if (rgb_frame_)
00337     uvc_free_frame(rgb_frame_);
00338 
00339   rgb_frame_ = uvc_allocate_frame(new_config.width * new_config.height * 3);
00340   assert(rgb_frame_);
00341 
00342   state_ = kRunning;
00343 }
00344 
00345 void CameraDriver::CloseCamera() {
00346   assert(state_ == kRunning);
00347 
00348   uvc_close(devh_);
00349   devh_ = NULL;
00350 
00351   uvc_unref_device(dev_);
00352   dev_ = NULL;
00353 
00354   state_ = kStopped;
00355 }
00356 
00357 };


libuvc_camera
Author(s):
autogenerated on Thu Aug 27 2015 13:50:58