00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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_);
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
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154 }
00155
00156 config_ = new_config;
00157 }
00158
00159 void CameraDriver::ImageCallback(uvc_frame_t *frame) {
00160
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 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
00244 }
00245 }
00246
00247 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
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 };