void UvcVideo::InitDevice(int vid, int pid, const char* sn, int device_id, int width, int height, int fps)
{
uvc_error_t find_err = FindDevice(ctx_, &dev_, vid, pid, sn, device_id );
if (find_err != UVC_SUCCESS) {
uvc_perror(find_err, "uvc_find_device");
throw VideoException("Unable to open UVC Device");
}
if(!dev_) {
throw VideoException("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 VideoException("Unable to open device");
}
//uvc_print_diag(devh_, stderr);
uvc_error_t mode_err = uvc_get_stream_ctrl_format_size(
devh_, &ctrl_,
UVC_FRAME_FORMAT_ANY,
width, height,
fps);
//uvc_print_stream_ctrl(&ctrl_, stderr);
if (mode_err != UVC_SUCCESS) {
uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
uvc_close(devh_);
uvc_unref_device(dev_);
throw VideoException("Unable to set device mode.");
}
uvc_error_t strm_err = uvc_stream_open_ctrl(devh_, &strm_, &ctrl_);
if(strm_err != UVC_SUCCESS) {
uvc_perror(strm_err, "uvc_stream_open_ctrl");
uvc_close(devh_);
uvc_unref_device(dev_);
throw VideoException("Unable to open device stream.");
}
// Default to greyscale.
PixelFormat pfmt = PixelFormatFromString("GRAY8");
const uvc_format_desc_t* uvc_fmt = uvc_get_format_descs(devh_);
while( uvc_fmt->bFormatIndex != ctrl_.bFormatIndex && uvc_fmt ) {
uvc_fmt = uvc_fmt->next;
}
if(uvc_fmt) {
// TODO: Use uvc_fmt->fourccFormat
if( uvc_fmt->bBitsPerPixel == 16 ) {
pfmt = PixelFormatFromString("GRAY16LE");
}
}
const StreamInfo stream_info(pfmt, width, height, (width*pfmt.bpp)/8, 0);
streams.push_back(stream_info);
}
void RealSenseDriver::Start(int vid, int pid, char* sn)
{
//The RealSense camera is capable of delivering separate streams for rgb and depth at separate resolutions
//For now, use 640x480@30fps for each one
//There are separate streams defined for depth using non-standard UVC GUIDs - use the YUYV-advertised GUID, which is really just
//gray16 data
width_ = 640;
height_ = 480;
fps_ = 30;
if(ctx_) {
Stop();
}
uvc_init(&ctx_, NULL);
if(!ctx_) {
throw DeviceException("Unable to open UVC library context");
}
uvc_error_t find_err;
if (sn)
find_err = uvc_find_device(ctx_, &dev_, vid, pid, sn );
else
find_err = uvc_find_device(ctx_, &dev_, vid, pid, NULL);
if (find_err != UVC_SUCCESS) {
uvc_perror(find_err, "uvc_find_device");
throw DeviceException("Unable to open RealSense device");
}
if(!dev_) {
throw DeviceException("Unable to open RealSense Device - no pointer returned.");
}
uvc_error_t open_err = uvc_open2(dev_, &devh_rgb, 0); //Camera 0 is RGB
if (open_err != UVC_SUCCESS) {
uvc_perror(open_err, "uvc_open");
uvc_unref_device(dev_);
throw DeviceException("Unable to open RealSense RGB device");
}
open_err = uvc_open2(dev_, &devh_d, 1); //Camera 1 is depth
if (open_err != UVC_SUCCESS) {
uvc_perror(open_err, "uvc_open");
uvc_unref_device(dev_);
throw DeviceException("Unable to open RealSense Depth device");
}
// uvc_set_status_callback(devh_, &CameraDriver::AutoControlsCallbackAdapter, this);
uvc_error_t mode_err = uvc_get_stream_ctrl_format_size(
devh_rgb, &ctrl_rgb,
UVC_COLOR_FORMAT_YUYV,
width_, height_,
60);
pbtype_rgb = pb::PB_UNSIGNED_BYTE;
pbformat_rgb = pb::PB_RGB;
if (mode_err != UVC_SUCCESS) {
uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
uvc_close(devh_rgb);
uvc_close(devh_d);
uvc_unref_device(dev_);
throw DeviceException("Unable to get RGB device mode.");
}
//Use the same frame parameters for both RGB and depth (for now)
if (useIR)
{
//Depth as mono8
mode_err = uvc_get_stream_ctrl_format_size(
devh_d, &ctrl_d,
UVC_FRAME_FORMAT_INVI,
width_, height_,
fps_);
pbtype_d = pb::PB_UNSIGNED_BYTE;
}
else
{
//Grayscale depth as yuyv, mono16
mode_err = uvc_get_stream_ctrl_format_size(
devh_d, &ctrl_d,
//UVC_FRAME_FORMAT_INVI,
UVC_FRAME_FORMAT_YUYV,
width_, height_,
fps_);
pbtype_d = pb::PB_UNSIGNED_SHORT;
}
pbformat_d = pb::PB_LUMINANCE;
if (mode_err != UVC_SUCCESS) {
uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
uvc_close(devh_rgb);
uvc_close(devh_d);
uvc_unref_device(dev_);
throw DeviceException("Unable to get depth device mode.");
}
uvc_error_t stream_ctl_err = uvc_stream_open_ctrl(devh_rgb, &streamh_rgb, &ctrl_rgb);
if (stream_ctl_err != UVC_SUCCESS) {
uvc_perror(stream_ctl_err, "uvc_stream_open_ctrl");
uvc_close(devh_rgb);
uvc_close(devh_d);
uvc_unref_device(dev_);
throw DeviceException("Unable to open stream on RGB.");
}
stream_ctl_err = uvc_stream_open_ctrl(devh_d, &streamh_d, &ctrl_d);
if (stream_ctl_err != UVC_SUCCESS) {
uvc_perror(stream_ctl_err, "uvc_stream_open_ctrl");
uvc_close(devh_rgb);
uvc_close(devh_d);
uvc_unref_device(dev_);
throw DeviceException("Unable to open stream on depth.");
}
uvc_error_t stream_start_err = uvc_stream_start(streamh_rgb, NULL, this, 0);
if (stream_start_err != UVC_SUCCESS) {
uvc_perror(stream_start_err, "uvc_stream_start");
uvc_close(devh_rgb);
uvc_close(devh_d);
uvc_unref_device(dev_);
throw DeviceException("Unable to start streaming on rgb.");
}
stream_start_err = uvc_stream_start(streamh_d, NULL, this, 0);
if (stream_start_err != UVC_SUCCESS) {
uvc_perror(stream_start_err, "uvc_stream_start");
uvc_close(devh_rgb);
uvc_close(devh_d);
uvc_unref_device(dev_);
throw DeviceException("Unable to start streaming on depth.");
}
if (frame_rgb)
uvc_free_frame(frame_rgb);
if (frame_d)
uvc_free_frame(frame_d);
frame_rgb = uvc_allocate_frame(ctrl_rgb.dwMaxVideoFrameSize);
if(!frame_rgb) {
throw DeviceException("Unable to allocate RGB frame.");
}
frame_d = uvc_allocate_frame(ctrl_d.dwMaxVideoFrameSize);
if(!frame_d) {
throw DeviceException("Unable to allocate depth frame.");
}
if (useSync)
{
std::cout << "RealSense: Using SCR to sync" << std::endl;
}
}
bool UvcDriver::Capture( hal::CameraMsg& vImages )
{
vImages.Clear();
uvc_frame_t* frame = NULL;
uvc_error_t err = uvc_get_frame(devh_, &frame, 0);
if(err!= UVC_SUCCESS) {
uvc_perror(err, "uvc_get_frame");
}else{
if(frame) {
hal::ImageMsg* pimg = vImages.add_image();
pimg->set_type( (hal::Type) pbtype );
pimg->set_format( (hal::Format) pbformat );
pimg->set_width(frame->width);
pimg->set_height(frame->height);
pimg->set_data(frame->data, width_ * height_);
return true;
}else{
std::cout << "No data..." << std::endl;
}
}
return true;
}
void UVCPreview::do_preview(uvc_stream_ctrl_t *ctrl) {
ENTER();
uvc_frame_t *frame = NULL;
uvc_error_t result = uvc_start_iso_streaming(
mDeviceHandle, ctrl, uvc_preview_frame_callback, (void *)this);
if (LIKELY(!result)) {
clearPreviewFrame();
pthread_create(&capture_thread, NULL, capture_thread_func, (void *)this);
#if LOCAL_DEBUG
LOGI("Streaming...");
#endif
while (LIKELY(isRunning())) {
frame = waitPreviewFrame();
if (LIKELY(frame)) {
frame = draw_preview_one(frame, &mPreviewWindow, uvc_any2rgbx, 4);
addCaptureFrame(frame);
}
}
pthread_cond_signal(&capture_sync);
#if LOCAL_DEBUG
LOGI("preview_thread_func:wait for all callbacks complete");
#endif
uvc_stop_streaming(mDeviceHandle);
#if LOCAL_DEBUG
LOGI("Streaming finished");
#endif
} else {
uvc_perror(result, "failed start_streaming");
}
EXIT();
}
uvc_error_t RealSenseDriver::getFrame(uvc_stream_handle_t *streamh, uvc_frame_t **frame)
{
uvc_error_t err;
err = uvc_stream_get_frame(streamh, frame, -1); //use the latest acquired frame
if(err != UVC_SUCCESS)
{
uvc_perror(err, "uvc_get_frame");
}
return err;
}
/* This callback function runs once per frame. Use it to perform any
* quick processing you need, or have it put the frame into your application's
* input queue. If this function takes too long, you'll start losing frames. */
void cb_rgb(uvc_frame_t *frame, void *ptr) {
uvc_frame_t *bgr;
uvc_error_t ret;
/* We'll convert the image from YUV/JPEG to BGR, so allocate space */
bgr = uvc_allocate_frame(frame->width * frame->height * 3);
if (!bgr) {
printf("unable to allocate bgr frame!");
return;
}
/* Do the BGR conversion */
ret = uvc_any2bgr(frame, bgr);
if (ret) {
uvc_perror(ret, "uvc_any2bgr");
uvc_free_frame(bgr);
return;
}
/* Call a user function:
*
* my_type *my_obj = (*my_type) ptr;
* my_user_function(ptr, bgr);
* my_other_function(ptr, bgr->data, bgr->width, bgr->height);
*/
/* Call a C++ method:
*
* my_type *my_obj = (*my_type) ptr;
* my_obj->my_func(bgr);
*/
/* Use opencv.highgui to display the image: */
IplImage* cvImg;
cvImg = cvCreateImageHeader(
cvSize(bgr->width, bgr->height),
IPL_DEPTH_8U,
3);
cvSetData(cvImg, bgr->data, bgr->width * 3);
cvNamedWindow("Test", CV_WINDOW_AUTOSIZE);
cvShowImage("Test", cvImg);
cvWaitKey(10);
cvReleaseImageHeader(&cvImg);
uvc_free_frame(bgr);
}
bool CameraDriver::Start() {
assert(state_ == kInitial);
uvc_error_t err;
err = uvc_init(&ctx_, NULL);
if (err != UVC_SUCCESS) {
uvc_perror(err, "ERROR: uvc_init");
return false;
}
state_ = kStopped;
config_server_.setCallback(boost::bind(&CameraDriver::ReconfigureCallback, this, _1, _2));
return state_ == kRunning;
}
void UvcVideo::Start()
{
uvc_error_t stream_err = uvc_stream_start(strm_, NULL, this, 0);
if (stream_err != UVC_SUCCESS) {
uvc_perror(stream_err, "uvc_stream_start");
uvc_close(devh_);
uvc_unref_device(dev_);
throw VideoException("Unable to start streaming.");
}
if (frame_) {
uvc_free_frame(frame_);
}
size_bytes = ctrl_.dwMaxVideoFrameSize;
frame_ = uvc_allocate_frame(size_bytes);
if(!frame_) {
throw VideoException("Unable to allocate frame.");
}
}
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;
}
void CameraDriver::ImageCallback(uvc_frame_t *frame) {
ros::Time timestamp = ros::Time::now();
boost::recursive_mutex::scoped_lock(mutex_);
assert(state_ == kRunning);
assert(rgb_frame_);
sensor_msgs::Image::Ptr image(new sensor_msgs::Image());
image->width = config_.width;
image->height = config_.height;
image->step = image->width * 3;
image->data.resize(image->step * image->height);
if (frame->frame_format == UVC_FRAME_FORMAT_BGR){
image->encoding = "bgr8";
memcpy(&(image->data[0]), frame->data, frame->data_bytes);
} else if (frame->frame_format == UVC_FRAME_FORMAT_RGB){
image->encoding = "rgb8";
memcpy(&(image->data[0]), frame->data, frame->data_bytes);
} else if (frame->frame_format == UVC_FRAME_FORMAT_UYVY) {
image->encoding = "yuv422";
memcpy(&(image->data[0]), frame->data, frame->data_bytes);
} else if (frame->frame_format == UVC_FRAME_FORMAT_YUYV) {
// FIXME: uvc_any2bgr does not work on "yuyv" format, so use uvc_yuyv2bgr directly.
uvc_error_t conv_ret = uvc_yuyv2bgr(frame, rgb_frame_);
if (conv_ret != UVC_SUCCESS) {
uvc_perror(conv_ret, "Couldn't convert frame to RGB");
return;
}
image->encoding = "bgr8";
memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes);
#if libuvc_VERSION > 00005 /* version > 0.0.5 */
} else if (frame->frame_format == UVC_FRAME_FORMAT_MJPEG) {
// Enable mjpeg support despite uvs_any2bgr shortcoming
// https://github.com/ros-drivers/libuvc_ros/commit/7508a09f
uvc_error_t conv_ret = uvc_mjpeg2rgb(frame, rgb_frame_);
if (conv_ret != UVC_SUCCESS) {
uvc_perror(conv_ret, "Couldn't convert frame to RGB");
return;
}
image->encoding = "rgb8";
memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes);
#endif
} else {
uvc_error_t conv_ret = uvc_any2bgr(frame, rgb_frame_);
if (conv_ret != UVC_SUCCESS) {
uvc_perror(conv_ret, "Couldn't convert frame to RGB");
return;
}
image->encoding = "bgr8";
memcpy(&(image->data[0]), rgb_frame_->data, rgb_frame_->data_bytes);
}
sensor_msgs::CameraInfo::Ptr cinfo(
new sensor_msgs::CameraInfo(cinfo_manager_.getCameraInfo()));
image->header.frame_id = config_.frame_id;
image->header.stamp = timestamp;
cinfo->header.frame_id = config_.frame_id;
cinfo->header.stamp = timestamp;
cam_pub_.publish(image, cinfo);
if (config_changed_) {
config_server_.updateConfig(config_);
config_changed_ = false;
}
}
/* This callback function runs once per frame. Use it to perform any
* quick processing you need, or have it put the frame into your application's
* input queue. If this function takes too long, you'll start losing frames. */
void cb(uvc_frame_t *frame, void *ptr) {
frame->frame_format = UVC_FRAME_FORMAT_YUYV;
std::cout << "Frame format" << std::endl;
std::cout << frame->frame_format << std::endl;
uvc_frame_t *greyLeft;
uvc_frame_t *greyRight;
uvc_error_t retLeft;
uvc_error_t retRight;
/* We'll convert the image from YUV/JPEG to gray8, so allocate space */
greyLeft = uvc_allocate_frame(frame->width * frame->height);
greyRight = uvc_allocate_frame(frame->width * frame->height);
if (!greyLeft) {
printf("unable to allocate grey left frame!");
return;
}
if (!greyRight) {
printf("unable to allocate grey right frame!");
return;
}
/* Do the BGR conversion */
retLeft = uvc_yuyv2y(frame, greyLeft);
retRight = uvc_yuyv2uv(frame, greyRight);
if (retLeft) {
uvc_perror(retLeft, "uvc_yuyv2y");
uvc_free_frame(greyLeft);
printf("Error with yuyv to y conversion");
return;
}
if (retRight) {
uvc_perror(retRight, "uvc_yuyv2uv");
uvc_free_frame(greyRight);
printf("Error with yuyv to uv conversion");
return;
}
/* Call a user function:
*
* my_type *my_obj = (*my_type) ptr;
* my_user_function(ptr, bgr);
* my_other_function(ptr, bgr->data, bgr->width, bgr->height);
*/
/* Call a C++ method:
*
* my_type *my_obj = (*my_type) ptr;
* my_obj->my_func(bgr);
*/
/* Use opencv.highgui to display the image:
*
* cvImg = cvCreateImageHeader(
* cvSize(bgr->width, bgr->height),
* IPL_DEPTH_8U,
* 3);
*
* cvSetData(cvImg, bgr->data, bgr->width * 3);
*
* cvNamedWindow("Test", CV_WINDOW_AUTOSIZE);
* cvShowImage("Test", cvImg);
* cvWaitKey(10);
*
* cvReleaseImageHeader(&cvImg);
*/
IplImage *cvImg, *cvImg2;
cvImg = cvCreateImage(cvSize(greyLeft->width, greyLeft->height),
IPL_DEPTH_8U, 1);
cvImg2 = cvCreateImage(cvSize(greyRight->width, greyRight->height),
IPL_DEPTH_8U, 1);
cvImg->imageData = (char*) greyLeft->data;
cvImg2->imageData = (char*) greyRight->data;
cv::Mat left (greyLeft->height, greyLeft->width, CV_8UC1, greyLeft->data);
cv::Mat right (greyRight->height, greyRight->width, CV_8UC1, greyRight->data);
//cv::Mat right (cv::cvarrToMat(cvImg2));
cv::imshow("Test", left);
cv::imshow("Test2", right);
cvWaitKey(10);
cvReleaseImage(&cvImg);
cvReleaseImage(&cvImg2);
uvc_free_frame(greyLeft);
uvc_free_frame(greyRight);
}
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.");
}
}
