bool UvcVideo::GrabNext( unsigned char* image, bool wait )
{
uvc_frame_t* frame = NULL;
uvc_error_t err = uvc_stream_get_frame(strm_, &frame, wait ? 0 : -1);
if(err!= UVC_SUCCESS) {
pango_print_error("UvcVideo Error: %s", uvc_strerror(err) );
return false;
}else{
if(frame) {
memcpy(image, frame->data, frame->data_bytes );
return true;
}else{
if(wait) {
pango_print_debug("UvcVideo: No frame data");
}
return false;
}
}
}
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;
}
static void check(const char * call, uvc_error_t status)
{
if (status < 0) throw std::runtime_error(to_string() << call << "(...) returned " << uvc_strerror(status));
}
