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;
}
}
void set_power_state(power_state state) override {
uvc_error_t res;
uvc_format_t *formats;
/* if power became on and it was originally off. open the uvc device. */
if (state == D0 && _state == D3) {
res = uvc_find_device(_ctx, &_device, _info.vid, _info.pid, NULL);
if (res < 0) {
throw linux_backend_exception(
"Could not find the device.");
}
res = uvc_open2(_device, &_device_handle, _interface);
if (res < 0) {
uvc_unref_device(_device);
_device = NULL;
throw linux_backend_exception(
"Could not open device.");
}
}
else {
// we have been asked to close the device.
uvc_unref_device(_device);
uvc_stop_streaming(_device_handle);
_profiles.clear();
uvc_close(_device_handle);
_device = NULL;
_device_handle = NULL;
}
_state = state;
}
int main(int argc, char **argv) {
uvc_context_t *ctx;
uvc_device_t *dev;
uvc_device_handle_t *devh_d;
uvc_device_handle_t *devh_rgb;
uvc_stream_ctrl_t ctrl_rgb;
uvc_stream_ctrl_t ctrl_d;
uvc_error_t res;
/* 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");
/* Locates the first attached UVC device, stores in dev */
res = uvc_find_device(
ctx, &dev,
0x8086, 0x0a66, 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_open2(dev, &devh_rgb, 0); //Try to open camera 0 (RGB)
if (res < 0) {
uvc_perror(res, "uvc_open"); /* unable to open device */
} else {
puts("RGB Device opened");
/* Print out a message containing all the information that libuvc
* knows about the device */
uvc_print_diag(devh_rgb, stderr);
printf("Opening depth camera\n");
res = uvc_open2(dev, &devh_d, 1); //Try to open camera 1 (RGB)
uvc_print_diag(devh_d, stderr);
/* Try to negotiate a 640x480 30 fps YUYV stream profile */
res = uvc_get_stream_ctrl_format_size(
devh_rgb, &ctrl_rgb, /* 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_rgb, stderr);
/* Try to negotiate a 640x480 30 fps YUYV stream profile */
res = uvc_get_stream_ctrl_format_size(
devh_d, &ctrl_d, /* 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_d, 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_rgb, &ctrl_rgb, cb_rgb, 12345, 0);
res = uvc_start_streaming(devh_d, &ctrl_d, cb_gray, 12345, 0);
if (res < 0) {
uvc_perror(res, "start_streaming"); /* unable to start stream */
} else {
puts("Streaming...");
//uvc_set_ae_mode(devh, 1); /* e.g., turn on auto exposure */
sleep(30); /* stream for 10 seconds */
/* End the stream. Blocks until last callback is serviced */
uvc_stop_streaming(devh_rgb);
uvc_stop_streaming(devh_d);
puts("Done streaming.");
}
}
/* Release our handle on the device */
uvc_close(devh_d);
uvc_close(devh_rgb);
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;
}
subdevice & get_subdevice(int subdevice_index)
{
if(subdevice_index >= subdevices.size()) subdevices.resize(subdevice_index+1);
if(!subdevices[subdevice_index].handle) check("uvc_open2", uvc_open2(uvcdevice, &subdevices[subdevice_index].handle, subdevice_index));
return subdevices[subdevice_index];
}
