void start_streaming(device & device, int num_transfer_bufs)
{
for(auto & sub : device.subdevices)
{
if(sub.callback)
{
#if defined (ENABLE_DEBUG_SPAM)
uvc_print_stream_ctrl(&sub.ctrl, stdout);
#endif
check("uvc_start_streaming", uvc_start_streaming(sub.handle, &sub.ctrl, [](uvc_frame * frame, void * user)
{
reinterpret_cast<subdevice *>(user)->callback(frame->data);
}, &sub, 0, num_transfer_bufs));
}
}
}
/* request to start streaming*/
void stream_on(std::function<void(const notification& n)> error_handler) override
{
uvc_error_t res;
// loop over each prfile and start streaming.
for (auto i=0;i< _profiles.size(); ++i) {
callback_context *context = new callback_context();
context->_callback = _callbacks[i];
context->_this = this;
context->_profile = _profiles[i];
res = uvc_start_streaming(_device_handle,
&_stream_ctrls[i],
internal_uvc_callback,
context,
0);
if (res < 0) {
throw linux_backend_exception(
"fail to start streaming.");
}
}
}
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;
}
int main(int argc, char **argv) {
uvc_context_t *ctx;
uvc_device_t *dev;
uvc_device_handle_t *devh;
uvc_stream_ctrl_t ctrl;
uvc_error_t res;
uvc_device_t **list;
/* Initialize a UVC service context. Libuvc will set up its own libusb
* context. Replace NULL with a libusb_context pointer to run libuvc
* from an existing libusb context. */
res = uvc_init(&ctx, NULL);
if (res < 0) {
uvc_perror(res, "uvc_init");
return res;
}
puts("UVC initialized");
res = uvc_get_device_list(ctx, &list);
if (res < 0) {
uvc_perror(res, "uvc_get_device_list");
return res;
}
/* for(int i = 0; i < 1; i++) {
res = uvc_open(list[i], &devh);
uvc_print_diag(devh, stderr);
uvc_close(devh);
}
uchar x;
uvc_free_device_list(list, x);
*/
/* Locates the first attached UVC device, stores in dev */
res = uvc_find_device(
ctx, &dev,
0, 0, NULL); /* filter devices: vendor_id, product_id, "serial_num" */
if (res < 0) {
uvc_perror(res, "uvc_find_device"); /* no devices found */
} else {
puts("Device found");
/* Try to open the device: requires exclusive access */
res = uvc_open(dev, &devh);
if (res < 0) {
uvc_perror(res, "uvc_open"); /* unable to open device */
printf("Unable to open device");
} else {
puts("Device opened");
/* Print out a message containing all the information that libuvc
* knows about the device */
uvc_print_diag(devh, stderr);
/* Try to negotiate a 640x480 30 fps YUYV stream profile */
res = uvc_get_stream_ctrl_format_size(
devh, &ctrl, /* result stored in ctrl */
UVC_FRAME_FORMAT_YUYV, /* YUV 422, aka YUV 4:2:2. try _COMPRESSED */
640, 480, 30 /* width, height, fps */
);
/* Print out the result */
uvc_print_stream_ctrl(&ctrl, stderr);
if (res < 0) {
uvc_perror(res, "get_mode"); /* device doesn't provide a matching stream */
} else {
/* Start the video stream. The library will call user function cb:
* cb(frame, (void*) 12345)
*/
res = uvc_start_streaming(devh, &ctrl, cb, (void*)12345, 0);
if (res < 0) {
uvc_perror(res, "start_streaming"); /* unable to start stream */
} else {
puts("Streaming...");
uvc_set_ae_mode(devh, 0); /* e.g., turn on auto exposure */
sleep(10); /* stream for 10 seconds */
/* End the stream. Blocks until last callback is serviced */
uvc_stop_streaming(devh);
puts("Done streaming.");
}
}
/* Release our handle on the device */
uvc_close(devh);
puts("Device closed");
}
/* Release the device descriptor */
uvc_unref_device(dev);
}
/* Close the UVC context. This closes and cleans up any existing device handles,
* and it closes the libusb context if one was not provided. */
uvc_exit(ctx);
puts("UVC exited");
return 0;
}
void UvcDriver::Start(int vid, int pid, char* sn)
{
width_ = 640;
height_ = 480;
fps_ = 30;
if(ctx_) {
Stop();
}
uvc_init(&ctx_, NULL);
if(!ctx_) {
throw DeviceException("Unable to open UVC Context");
}
uvc_error_t find_err = uvc_find_device(ctx_, &dev_, vid, pid, sn );
if (find_err != UVC_SUCCESS) {
uvc_perror(find_err, "uvc_find_device");
throw DeviceException("Unable to open UVC Device");
}
if(!dev_) {
throw DeviceException("Unable to open UVC Device - no pointer returned.");
}
uvc_error_t open_err = uvc_open(dev_, &devh_);
if (open_err != UVC_SUCCESS) {
uvc_perror(open_err, "uvc_open");
uvc_unref_device(dev_);
throw DeviceException("Unable to open device");
}
// 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,
UVC_COLOR_FORMAT_YUYV,
width_, height_,
fps_);
pbtype = hal::PB_UNSIGNED_BYTE;
pbformat = hal::PB_LUMINANCE;
if (mode_err != UVC_SUCCESS) {
uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
uvc_close(devh_);
uvc_unref_device(dev_);
throw DeviceException("Unable to device mode.");
}
// uvc_error_t stream_err = uvc_start_iso_streaming(devh_, &ctrl, &UvcDriver::ImageCallbackAdapter, this);
//uvc_error_t stream_err = uvc_start_iso_streaming(devh_, &ctrl, NULL, this);
uvc_error_t stream_err = uvc_start_streaming(devh_, &ctrl, NULL, this, 0);
if (stream_err != UVC_SUCCESS) {
uvc_perror(stream_err, "uvc_start_iso_streaming");
uvc_close(devh_);
uvc_unref_device(dev_);
throw DeviceException("Unable to start iso streaming.");
}
if (frame_)
uvc_free_frame(frame_);
frame_ = uvc_allocate_frame(ctrl.dwMaxVideoFrameSize);
if(!frame_) {
throw DeviceException("Unable to allocate frame.");
}
}
