void PaintApp::Initialize()
{
// HACK for ubuntu1024: https://github.com/DrMcCoy/xoreos/commit/9a6c84d5458256ac5a0ff7525055ef2d8761e683
if (!XInitThreads()) {
throw "Failed to initialize Xlib muti-threading support";
}
mCurrentVideoMode = VideoMode::OpenGL;
mCurrentResolution = {640_px, 480_px};
InitVideo();
InitKinect("");
SelectRenderer(); // Kinect has to be set up for this!
mWindow = std::make_shared<kinex::Window>("paint");
mKinectBg = std::make_shared<NuiBackground>(mKinect);
mState = std::make_shared<PaintStatus>();
print_commands();
}
int Protonect::openKinect(std::string binpath){
if( bOpened ){
closeKinect();
}
cmd_seq = 0;
uint16_t vid = 0x045E;
uint16_t pid[2] = {0x02d8, 0x02C4};
uint16_t mi = 0x00;
bool debug_mode = false;
uint8_t bus;
int r;
const struct libusb_version* version;
version = libusb_get_version();
printf("Using libusbx v%d.%d.%d.%d\n\n", version->major, version->minor, version->micro, version->nano);
r = libusb_init(NULL);
if (r < 0) return r;
libusb_set_debug(NULL, debug_mode ? LIBUSB_LOG_LEVEL_DEBUG : LIBUSB_LOG_LEVEL_INFO);
for(int i = 0; i < 2; i++){
printf("Trying to open device %04X:%04X...\n", vid, pid[i]);
handle = NULL;
int tryCount = 4;
if (handle == NULL){
while(tryCount > 0 && handle == NULL){
handle = libusb_open_device_with_vid_pid(NULL, vid, pid[i]);
tryCount--;
usleep(100);
if( handle ){
libusb_reset_device(handle);
usleep(100);
handle = libusb_open_device_with_vid_pid(NULL, vid, pid[i]);
}
}
}
if(handle != NULL){
break;
}
}
if( handle == NULL ){
perr("Protonect::openKinect Failed. - handle is NULL\n");
return -1;
}
dev = libusb_get_device(handle);
bus = libusb_get_bus_number(dev);
/*
struct libusb_device_descriptor dev_desc;
printf("\nReading device descriptor:\n");
CALL_CHECK(libusb_get_device_descriptor(dev, &dev_desc));
printf(" length: %d\n", dev_desc.bLength);
printf(" device class: %d\n", dev_desc.bDeviceClass);
printf(" S/N: %d\n", dev_desc.iSerialNumber);
printf(" VID:PID: %04X:%04X\n", dev_desc.idVendor, dev_desc.idProduct);
printf(" bcdDevice: %04X\n", dev_desc.bcdDevice);
printf(" iMan:iProd:iSer: %d:%d:%d\n", dev_desc.iManufacturer, dev_desc.iProduct, dev_desc.iSerialNumber);
printf(" nb confs: %d\n", dev_desc.bNumConfigurations);
*/
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
int active_cfg = -5;
r = libusb_get_configuration(handle, &active_cfg);
printf("active configuration: %d, err: %d", active_cfg, r);
int configId = 1;
if (active_cfg != configId)
{
printf("Setting config: %d\n", configId);
r = libusb_set_configuration(handle, configId);
if (r != LIBUSB_SUCCESS)
{
perr(" Can't set configuration. Error code: %d (%s)\n", r, libusb_error_name(r));
}
}
int iface = 0;
printf("\nClaiming interface %d...\n", iface);
r = libusb_claim_interface(handle, iface);
if (r != LIBUSB_SUCCESS)
{
perr(" Failed: %d.\n", r);
}
iface = 1;
printf("\nClaiming interface %d...\n", iface);
r = libusb_claim_interface(handle, iface);
if (r != LIBUSB_SUCCESS)
{
perr(" Failed: %d.\n", r);
}
InitKinect(handle);
// install signal handler now
signal(SIGINT,sigint_handler);
shutdown = false;
usb_loop.start();
//INITIALIZE OBJECTS
// frame_listener = new libfreenect2::FrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
rgb_bulk_transfers = new libfreenect2::usb::BulkTransferPool(handle, 0x83);
//rgb_processor = new libfreenect2::ofRGBPacketProcessor();
// rgb_packet_stream_parser = new libfreenect2::RgbPacketStreamParser(rgb_processor);
// rgb_processor->setFrameListener(frame_listener);
rgb_bulk_transfers->allocate(50, 0x4000);
rgb_bulk_transfers->setCallback(rgb_packet_stream_parser);
rgb_bulk_transfers->enableSubmission();
rgb_bulk_transfersPtr = rgb_bulk_transfers;
depth_iso_transfers = new libfreenect2::usb::IsoTransferPool(handle, 0x84);
depth_processor = new libfreenect2::CpuDepthPacketProcessor();
depth_processor->setFrameListener(frame_listener);
depth_processor->load11To16LutFromFile((binpath + "11to16.bin").c_str());
depth_processor->loadXTableFromFile((binpath + "xTable.bin").c_str());
depth_processor->loadZTableFromFile((binpath + "zTable.bin").c_str());
depth_packet_stream_parser = new libfreenect2::DepthPacketStreamParser(depth_processor);
size_t max_packet_size = libusb_get_max_iso_packet_size(dev, 0x84);
std::cout << "iso max_packet_size: " << max_packet_size << std::endl;
depth_iso_transfers = new libfreenect2::usb::IsoTransferPool(handle, 0x84);
depth_iso_transfers->allocate(80, 8, max_packet_size);
depth_iso_transfers->setCallback(depth_packet_stream_parser);
depth_iso_transfers->enableSubmission();
depth_iso_transfersPtr = depth_iso_transfers;
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
RunKinect(handle, *depth_processor);
rgb_bulk_transfers->submit(10);
depth_iso_transfers->submit(60);
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
bOpened = true;
return 0;
}
int main(int argc, char *argv[])
{
std::string program_path(argv[0]);
size_t executable_name_idx = program_path.rfind("Protonect");
std::string binpath = "/";
if(executable_name_idx != std::string::npos)
{
binpath = program_path.substr(0, executable_name_idx);
}
uint16_t vid = 0x045E;
uint16_t pid = 0x02C4;
uint16_t mi = 0x00;
bool debug_mode = false;
libusb_device_handle *handle;
libusb_device *dev;
uint8_t bus;
const char* speed_name[5] =
{ "Unknown", "1.5 Mbit/s (USB LowSpeed)", "12 Mbit/s (USB FullSpeed)", "480 Mbit/s (USB HighSpeed)", "5000 Mbit/s (USB SuperSpeed)" };
int r;
const struct libusb_version* version;
version = libusb_get_version();
printf("Using libusbx v%d.%d.%d.%d\n\n", version->major, version->minor, version->micro, version->nano);
r = libusb_init(NULL);
if (r < 0)
return r;
libusb_set_debug(NULL, debug_mode ? LIBUSB_LOG_LEVEL_DEBUG : LIBUSB_LOG_LEVEL_INFO);
printf("Opening device %04X:%04X...\n", vid, pid);
handle = libusb_open_device_with_vid_pid(NULL, vid, pid);
if (handle == NULL)
{
perr(" Failed.\n");
//system("PAUSE");
return -1;
}
dev = libusb_get_device(handle);
bus = libusb_get_bus_number(dev);
/*
struct libusb_device_descriptor dev_desc;
printf("\nReading device descriptor:\n");
CALL_CHECK(libusb_get_device_descriptor(dev, &dev_desc));
printf(" length: %d\n", dev_desc.bLength);
printf(" device class: %d\n", dev_desc.bDeviceClass);
printf(" S/N: %d\n", dev_desc.iSerialNumber);
printf(" VID:PID: %04X:%04X\n", dev_desc.idVendor, dev_desc.idProduct);
printf(" bcdDevice: %04X\n", dev_desc.bcdDevice);
printf(" iMan:iProd:iSer: %d:%d:%d\n", dev_desc.iManufacturer, dev_desc.iProduct, dev_desc.iSerialNumber);
printf(" nb confs: %d\n", dev_desc.bNumConfigurations);
*/
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
int active_cfg = -5;
r = libusb_get_configuration(handle, &active_cfg);
printf("active configuration: %d, err: %d", active_cfg, r);
int configId = 1;
if (active_cfg != configId)
{
printf("Setting config: %d\n", configId);
r = libusb_set_configuration(handle, configId);
if (r != LIBUSB_SUCCESS)
{
perr(" Can't set configuration. Error code: %d (%s)\n", r, libusb_error_name(r));
}
}
int iface = 0;
printf("\nClaiming interface %d...\n", iface);
r = libusb_claim_interface(handle, iface);
if (r != LIBUSB_SUCCESS)
{
perr(" Failed: %d.\n", r);
}
iface = 1;
printf("\nClaiming interface %d...\n", iface);
r = libusb_claim_interface(handle, iface);
if (r != LIBUSB_SUCCESS)
{
perr(" Failed: %d.\n", r);
}
InitKinect(handle);
// install signal handler now
signal(SIGINT,sigint_handler);
shutdown = false;
libfreenect2::usb::EventLoop usb_loop;
usb_loop.start();
libfreenect2::FrameMap frames;
libfreenect2::FrameListener frame_listener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
//libfreenect2::DumpRgbPacketProcessor rgb_processor;
libfreenect2::TurboJpegRgbPacketProcessor rgb_processor;
rgb_processor.setFrameListener(&frame_listener);
libfreenect2::RgbPacketStreamParser rgb_packet_stream_parser(&rgb_processor);
libfreenect2::usb::BulkTransferPool rgb_bulk_transfers(handle, 0x83);
rgb_bulk_transfers.allocate(50, 0x4000);
rgb_bulk_transfers.setCallback(&rgb_packet_stream_parser);
rgb_bulk_transfers.enableSubmission();
libfreenect2::CpuDepthPacketProcessor depth_processor;
depth_processor.setFrameListener(&frame_listener);
depth_processor.load11To16LutFromFile((binpath + "../11to16.bin").c_str());
depth_processor.loadXTableFromFile((binpath + "../xTable.bin").c_str());
depth_processor.loadZTableFromFile((binpath + "../zTable.bin").c_str());
libfreenect2::DepthPacketStreamParser depth_packet_stream_parser(&depth_processor);
size_t max_packet_size = libusb_get_max_iso_packet_size(dev, 0x84);
std::cout << "iso max_packet_size: " << max_packet_size << std::endl;
libfreenect2::usb::IsoTransferPool depth_iso_transfers(handle, 0x84);
depth_iso_transfers.allocate(80, 8, max_packet_size);
depth_iso_transfers.setCallback(&depth_packet_stream_parser);
depth_iso_transfers.enableSubmission();
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
RunKinect(handle, depth_processor);
rgb_bulk_transfers.submit(10);
depth_iso_transfers.submit(60);
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
while(!shutdown)
{
frame_listener.waitForNewFrame(frames);
libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
cv::imshow("rgb", cv::Mat(rgb->height, rgb->width, CV_8UC3, rgb->data));
cv::imshow("ir", cv::Mat(ir->height, ir->width, CV_32FC1, ir->data) / 20000.0f);
cv::imshow("depth", cv::Mat(depth->height, depth->width, CV_32FC1, depth->data) / 4500.0f);
cv::waitKey(1);
frame_listener.release(frames);
}
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
rgb_bulk_transfers.disableSubmission();
depth_iso_transfers.disableSubmission();
CloseKinect(handle);
rgb_bulk_transfers.cancel();
depth_iso_transfers.cancel();
// wait for all transfers to cancel
// TODO: better implementation
libfreenect2::this_thread::sleep_for(libfreenect2::chrono::seconds(2));
rgb_bulk_transfers.deallocate();
depth_iso_transfers.deallocate();
r = libusb_get_device_speed(dev);
if ((r < 0) || (r > 4))
r = 0;
printf(" speed: %s\n", speed_name[r]);
iface = 0;
printf("Releasing interface %d...\n", iface);
libusb_release_interface(handle, iface);
iface = 1;
printf("Releasing interface %d...\n", iface);
libusb_release_interface(handle, iface);
printf("Closing device...\n");
libusb_close(handle);
usb_loop.stop();
libusb_exit(NULL);
//system("PAUSE");
return 0;
}
