std::vector<std::shared_ptr<device>> query_devices(std::shared_ptr<context> context) { std::vector<std::shared_ptr<device>> devices; uvc_device_t ** list; CALL_UVC(uvc_get_device_list, context->ctx, &list); for(auto it = list; *it; ++it) try { devices.push_back(std::make_shared<device>(context, *it)); } catch(std::runtime_error &e) { LOG_WARNING("usb:" << (int)uvc_get_bus_number(*it) << ':' << (int)uvc_get_device_address(*it) << ": " << e.what()); } uvc_free_device_list(list, 1); return devices; }
void CameraDriver::OpenCamera(UVCCameraConfig &new_config) { assert(state_ == kStopped); int vendor_id = strtol(new_config.vendor.c_str(), NULL, 0); int product_id = strtol(new_config.product.c_str(), NULL, 0); ROS_INFO("Opening camera with vendor=0x%x, product=0x%x, serial=\"%s\", index=%d", vendor_id, product_id, new_config.serial.c_str(), new_config.index); uvc_device_t **devs; // Implement missing index select behavior // https://github.com/ros-drivers/libuvc_ros/commit/4f30e9a0 #if libuvc_VERSION > 00005 /* version > 0.0.5 */ uvc_error_t find_err = uvc_find_devices( ctx_, &devs, vendor_id, product_id, new_config.serial.empty() ? NULL : new_config.serial.c_str()); if (find_err != UVC_SUCCESS) { uvc_perror(find_err, "uvc_find_device"); return; } // select device by index dev_ = NULL; int dev_idx = 0; while (devs[dev_idx] != NULL) { if(dev_idx == new_config.index) { dev_ = devs[dev_idx]; } else { uvc_unref_device(devs[dev_idx]); } dev_idx++; } if(dev_ == NULL) { ROS_ERROR("Unable to find device at index %d", new_config.index); return; } #else uvc_error_t find_err = uvc_find_device( ctx_, &dev_, vendor_id, product_id, new_config.serial.empty() ? NULL : new_config.serial.c_str()); if (find_err != UVC_SUCCESS) { uvc_perror(find_err, "uvc_find_device"); return; } #endif uvc_error_t open_err = uvc_open(dev_, &devh_); if (open_err != UVC_SUCCESS) { switch (open_err) { case UVC_ERROR_ACCESS: #ifdef __linux__ ROS_ERROR("Permission denied opening /dev/bus/usb/%03d/%03d", uvc_get_bus_number(dev_), uvc_get_device_address(dev_)); #else ROS_ERROR("Permission denied opening device %d on bus %d", uvc_get_device_address(dev_), uvc_get_bus_number(dev_)); #endif break; default: #ifdef __linux__ ROS_ERROR("Can't open /dev/bus/usb/%03d/%03d: %s (%d)", uvc_get_bus_number(dev_), uvc_get_device_address(dev_), uvc_strerror(open_err), open_err); #else ROS_ERROR("Can't open device %d on bus %d: %s (%d)", uvc_get_device_address(dev_), uvc_get_bus_number(dev_), uvc_strerror(open_err), open_err); #endif break; } uvc_unref_device(dev_); return; } uvc_set_status_callback(devh_, &CameraDriver::AutoControlsCallbackAdapter, this); uvc_stream_ctrl_t ctrl; uvc_error_t mode_err = uvc_get_stream_ctrl_format_size( devh_, &ctrl, GetVideoMode(new_config.video_mode), new_config.width, new_config.height, new_config.frame_rate); if (mode_err != UVC_SUCCESS) { uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size"); uvc_close(devh_); uvc_unref_device(dev_); ROS_ERROR("check video_mode/width/height/frame_rate are available"); uvc_print_diag(devh_, NULL); return; } uvc_error_t stream_err = uvc_start_streaming(devh_, &ctrl, &CameraDriver::ImageCallbackAdapter, this, 0); if (stream_err != UVC_SUCCESS) { uvc_perror(stream_err, "uvc_start_streaming"); uvc_close(devh_); uvc_unref_device(dev_); return; } if (rgb_frame_) uvc_free_frame(rgb_frame_); rgb_frame_ = uvc_allocate_frame(new_config.width * new_config.height * 3); assert(rgb_frame_); state_ = kRunning; }