//TODO: get the deletion shutdown of objects ordered right int Protonect::closeKinect(){ if( bOpened ){ bOpened = false; shutdown = true; rgb_bulk_transfers->disableSubmission(); depth_iso_transfers->disableSubmission(); rgb_bulk_transfers->cancel(); depth_iso_transfers->cancel(); CloseKinect(handle); // 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(); int 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); if(frame_listener != NULL){ delete frame_listener; frame_listener = NULL; } if(rgb_bulk_transfers != NULL){ delete rgb_bulk_transfers; rgb_bulk_transfers = NULL; } if(depth_iso_transfers != NULL){ delete depth_iso_transfers; depth_iso_transfers = NULL; } if(depth_processor != NULL){ delete depth_processor; depth_processor = NULL; } // if(rgb_processor != NULL){ // delete rgb_processor; // rgb_processor = NULL; // } if(rgb_packet_stream_parser != NULL){ delete rgb_packet_stream_parser; rgb_packet_stream_parser = NULL; } if(depth_packet_stream_parser != NULL){ delete depth_packet_stream_parser; depth_packet_stream_parser = NULL; } } 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; }