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