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;
}
}
