diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index 3494fa2ea0b38683e2d428de061900155e748976..f112f535a692be586410ff56536adaf043d37530 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -309,19 +309,38 @@ void CameraDriver::OpenCamera(UVCCameraConfig &new_config) { 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_error_t find_err = uvc_find_device( - ctx_, &dev_, + uvc_device_t **devs; + + uvc_error_t find_err = uvc_find_devices( + ctx_, &devs, vendor_id, product_id, new_config.serial.empty() ? NULL : new_config.serial.c_str()); - // TODO: index - 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; + } + uvc_error_t open_err = uvc_open(dev_, &devh_); if (open_err != UVC_SUCCESS) { @@ -370,10 +389,10 @@ void CameraDriver::OpenCamera(UVCCameraConfig &new_config) { return; } - uvc_error_t stream_err = uvc_start_iso_streaming(devh_, &ctrl, &CameraDriver::ImageCallbackAdapter, this); + 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_iso_streaming"); + uvc_perror(stream_err, "uvc_start_streaming"); uvc_close(devh_); uvc_unref_device(dev_); return;