//---------------------------------------------------------------------------
void ofxKinect::threadedFunction(){
if(currentLed < 0) {
freenect_set_led(kinectDevice, (freenect_led_options)ofxKinect::LED_GREEN);
}
freenect_frame_mode videoMode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, bIsVideoInfrared?FREENECT_VIDEO_IR_8BIT:FREENECT_VIDEO_RGB);
freenect_set_video_mode(kinectDevice, videoMode);
freenect_frame_mode depthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, bUseRegistration?FREENECT_DEPTH_REGISTERED:FREENECT_DEPTH_MM);
freenect_set_depth_mode(kinectDevice, depthMode);
ofLogVerbose("ofxKinect") << "device " << deviceId << " " << serial << " connection opened";
freenect_start_depth(kinectDevice);
if(bGrabVideo) {
freenect_start_video(kinectDevice);
}
while(isThreadRunning() && freenect_process_events(kinectContext.getContext()) >= 0) {
if(bTiltNeedsApplying) {
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
if(bLedNeedsApplying) {
if(currentLed == ofxKinect::LED_DEFAULT) {
freenect_set_led(kinectDevice, (freenect_led_options)ofxKinect::LED_GREEN);
}
else {
freenect_set_led(kinectDevice, (freenect_led_options)currentLed);
}
bLedNeedsApplying = false;
}
freenect_update_tilt_state(kinectDevice);
freenect_raw_tilt_state * tilt = freenect_get_tilt_state(kinectDevice);
currentTiltAngleDeg = freenect_get_tilt_degs(tilt);
rawAccel.set(tilt->accelerometer_x, tilt->accelerometer_y, tilt->accelerometer_z);
double dx,dy,dz;
freenect_get_mks_accel(tilt, &dx, &dy, &dz);
mksAccel.set(dx, dy, dz);
}
// finish up a tilt on exit
if(bTiltNeedsApplying) {
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
freenect_stop_depth(kinectDevice);
freenect_stop_video(kinectDevice);
if(currentLed < 0) {
freenect_set_led(kinectDevice, (freenect_led_options)ofxKinect::LED_RED);
}
kinectContext.close(*this);
ofLogVerbose("ofxKinect") << "device " << deviceId << " connection closed";
}
void *kinect_threadFunc(void *arg) {
while(freenect_process_events(f_ctx) >= 0) {
}
pthread_exit(NULL);
return NULL;
}
void *freenect_threadfunc(void *arg)
{
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
freenect_start_depth(f_dev);
freenect_start_rgb(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
while(!die && freenect_process_events(f_ctx) >= 0 )
{
int16_t ax,ay,az;
freenect_get_raw_accel(f_dev, &ax, &ay, &az);
double dx,dy,dz;
freenect_get_mks_accel(f_dev, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f\r", ax, ay, az, dx, dy, dz);
fflush(stdout);
}
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_rgb(f_dev);
printf("-- done!\n");
return NULL;
}
void init()
{
freenect_context *ctx;
freenect_device *dev;
if (freenect_init(&ctx, 0)) {
printf("Error: Cannot get context\n");
return;
}
if (freenect_open_device(ctx, &dev, 0)) {
printf("Error: Cannot get device\n");
return;
}
freenect_set_depth_format(dev, FREENECT_DEPTH_11BIT);
freenect_start_depth(dev);
freenect_set_video_format(dev, FREENECT_VIDEO_RGB);
freenect_start_video(dev);
if (use_ffmpeg) {
init_ffmpeg_streams();
freenect_set_depth_callback(dev, depth_cb_ffmpeg);
freenect_set_video_callback(dev, rgb_cb_ffmpeg);
} else {
freenect_set_depth_callback(dev, depth_cb);
freenect_set_video_callback(dev, rgb_cb);
}
while (running && freenect_process_events(ctx) >= 0)
snapshot_accel(dev);
freenect_stop_depth(dev);
freenect_stop_video(dev);
freenect_close_device(dev);
freenect_shutdown(ctx);
}
void *freenect_threadfunc(void *arg)
{
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
if ( pthread_create(&data_in_thread, NULL, data_in, NULL) )
{
fprintf(stderr, "Error on pthread_create() for data_in\n");
}
if ( pthread_create(&data_out_thread, NULL, data_out, NULL) )
{
fprintf(stderr, "Error on pthread_create() for data_out\n");
}
while(!die && freenect_process_events(f_ctx) >= 0 );
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_rgb(f_dev);
network_close();
printf("-- done!\n");
return NULL;
}
static void *init(void *unused)
{
pending_runloop_tasks_wait_zero();
pthread_mutex_lock(&runloop_lock);
while (thread_running && freenect_process_events(ctx) >= 0) {
pthread_mutex_unlock(&runloop_lock);
// NOTE: This lets you run tasks while process_events isn't running
pending_runloop_tasks_wait_zero();
pthread_mutex_lock(&runloop_lock);
}
// Go through each device, call stop video, close device
int i;
for (i = 0; i < MAX_KINECTS; ++i) {
if (kinects[i]) {
freenect_stop_video(kinects[i]->dev);
freenect_stop_depth(kinects[i]->dev);
freenect_set_user(kinects[i]->dev, NULL);
freenect_close_device(kinects[i]->dev);
free_buffer_ring(&kinects[i]->video);
free_buffer_ring(&kinects[i]->depth);
free(kinects[i]);
kinects[i] = NULL;
}
}
freenect_shutdown(ctx);
pthread_mutex_unlock(&runloop_lock);
return NULL;
}
JNIEXPORT void JNICALL Java_com_libfreenect_KinectRGBCamera_captureRawImage (JNIEnv * e, jobject o) {
env = e;
obj = o;
freenect_context *f_ctx;
freenect_device *f_dev;
printf("Kinect camera test\n");
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
}
if (freenect_open_device(f_ctx, &f_dev, 0) < 0) {
printf("Could not open device\n");
}
printf("Set RGB Callback");
freenect_set_rgb_callback(f_dev, rgb_cb);
printf("Starting RGB");
freenect_start_rgb(f_dev);
while(freenect_process_events(f_ctx) >= 0 );
freenect_close_device(f_dev);
}
void start()
{
boost::thread encoderThread = boost::thread(&Encoder::encode, p);
freenect_start_depth(f_dev);
while (!die && freenect_process_events(f_ctx) >= 0) {
usleep(1000);
}
}
void *freenect_threadfunc(void *arg)
{
printf("Freenect threadfunc\n");
while( !die && freenect_process_events(f_ctx) >= 0 ) {}
printf("Freenect thread exit");
return NULL;
}
//---------------------------------------------------------------------------
void ofxKinect::threadedFunction(){
freenect_set_led(kinectDevice, LED_GREEN);
freenect_set_video_format(kinectDevice, bInfrared?FREENECT_VIDEO_IR_8BIT:FREENECT_VIDEO_YUV_RGB);
freenect_set_depth_format(kinectDevice, FREENECT_DEPTH_11BIT);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection opened");
freenect_start_depth(kinectDevice);
if(bGrabVideo) {
freenect_start_video(kinectDevice);
}
// call platform specific processors (needed for Win)
if(freenect_process_events(kinectContext) != 0){
ofLog(OF_LOG_ERROR, "ofxKinect: freenect_process_events failed!");
return;
}
while(isThreadRunning()){
if(bTiltNeedsApplying){
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
if(bLedNeedsApplying){
freenect_set_led(kinectDevice, targetLedStatus);
bLedNeedsApplying = false;
}
freenect_update_tilt_state(kinectDevice);
freenect_raw_tilt_state * tilt = freenect_get_tilt_state(kinectDevice);
rawAccel.set(tilt->accelerometer_x, tilt->accelerometer_y, tilt->accelerometer_z);
double dx,dy,dz;
freenect_get_mks_accel(tilt, &dx, &dy, &dz);
mksAccel.set(dx, dy, dz);
ofSleepMillis(10);
// printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", ax, ay, az, dx, dy, dz);
}
// finish up a tilt on exit
if(bTiltNeedsApplying){
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
freenect_stop_depth(kinectDevice);
freenect_stop_video(kinectDevice);
freenect_set_led(kinectDevice, LED_YELLOW);
freenect_close_device(kinectDevice);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection closed");
}
void* freenect_threadfunc(void* arg) {
while(!die && freenect_process_events(f_ctx) >= 0) {
// If we did anything else in the freenect thread, it might go here.
}
freenect_stop_audio(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
return NULL;
}
int main() {
freenect_context *f_ctx;
freenect_device *f_dev;
if (freenect_init(&f_ctx, NULL) < 0) {
fprintf(stderr, "freenect_init() failed\n");
return 1;
}
freenect_select_subdevices(f_ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
if (freenect_num_devices(f_ctx) < 1) {
fprintf(stderr, "no devices found\n");
freenect_shutdown(f_ctx);
return 1;
}
if (freenect_open_device(f_ctx, &f_dev, 0) < 0) {
fprintf(stderr, "can't open device\n");
freenect_shutdown(f_ctx);
return 1;
}
depth_image = image_create(640, 480);
freenect_set_led(f_dev, LED_GREEN);
freenect_set_depth_callback(f_dev, capture_depth_image);
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_start_depth(f_dev);
if (signal(SIGINT, handle_interrupt) == SIG_IGN) {
signal(SIGINT, SIG_IGN);
}
if (signal(SIGTERM, handle_interrupt) == SIG_IGN) {
signal(SIGTERM, SIG_IGN);
}
fprintf(stdout, "\x1B[2J");
while (running && freenect_process_events(f_ctx) >= 0) {
struct winsize w;
ioctl(STDOUT_FILENO, TIOCGWINSZ, &w);
fprintf(stdout, "\x1B[1;1H");
draw_depth_image(stdout, w.ws_col, w.ws_row - 1);
}
freenect_stop_depth(f_dev);
freenect_set_led(f_dev, LED_OFF);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
image_destroy(depth_image);
return 0;
}
//---------------------------------------------------------------------------
void ofxKinect::threadedFunction(){
freenect_set_led(kinectDevice, LED_GREEN);
freenect_frame_mode videoMode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, bIsVideoInfrared?FREENECT_VIDEO_IR_8BIT:FREENECT_VIDEO_RGB);
freenect_set_video_mode(kinectDevice, videoMode);
freenect_frame_mode depthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, bUseRegistration?FREENECT_DEPTH_REGISTERED:FREENECT_DEPTH_MM);
freenect_set_depth_mode(kinectDevice, depthMode);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Device %d %s connection opened", deviceId, serial.c_str());
freenect_start_depth(kinectDevice);
if(bGrabVideo) {
freenect_start_video(kinectDevice);
}
// call platform specific processors (needed for Win)
if(freenect_process_events(kinectContext.getContext()) != 0) {
ofLog(OF_LOG_ERROR, "ofxKinect: Device %d freenect_process_events failed!", deviceId);
return;
}
while(isThreadRunning()) {
if(bTiltNeedsApplying) {
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
freenect_update_tilt_state(kinectDevice);
freenect_raw_tilt_state * tilt = freenect_get_tilt_state(kinectDevice);
currentTiltAngleDeg = freenect_get_tilt_degs(tilt);
rawAccel.set(tilt->accelerometer_x, tilt->accelerometer_y, tilt->accelerometer_z);
double dx,dy,dz;
freenect_get_mks_accel(tilt, &dx, &dy, &dz);
mksAccel.set(dx, dy, dz);
// ... and $0.02 for the scheduler
ofSleepMillis(10);
}
// finish up a tilt on exit
if(bTiltNeedsApplying) {
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
freenect_stop_depth(kinectDevice);
freenect_stop_video(kinectDevice);
freenect_set_led(kinectDevice, LED_YELLOW);
kinectContext.close(*this);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Device %d connection closed", deviceId);
}
void *runLoop(void *ptr)
{
while(kinRunning && freenect_process_events(kinCtx) >= 0)
{
//usleep(100000);
//freenect_process_events(kinCtx);
//printf("Loop\n");
}
printf("Out\n");
}
//----------------------------------------------------------------------------------------------------------------------
void QKinectProcessEvents::run()
{
// loop while we are active and process the kinect event queue
while(m_active)
{
//qDebug()<<"process thread\n";
if(freenect_process_events(m_ctx) < 0)
{
throw std::runtime_error("Cannot process freenect events");
}
}
}
void FreenectGrabber :: run()
{
setThreadShouldExit(false);
m_current_image.setCalibration(m_calib_data);
m_rgbd_image.setCalibration(m_calib_data);
m_rgbd_image.rawRgbRef() = Mat3b(FREENECT_FRAME_H, FREENECT_FRAME_W);
m_rgbd_image.rawDepthRef() = Mat1f(FREENECT_FRAME_H, FREENECT_FRAME_W);
m_rgbd_image.rawIntensityRef() = Mat1f(FREENECT_FRAME_H, FREENECT_FRAME_W);
m_current_image.rawRgbRef() = Mat3b(FREENECT_FRAME_H, FREENECT_FRAME_W);
m_current_image.rawDepthRef() = Mat1f(FREENECT_FRAME_H, FREENECT_FRAME_W);
m_current_image.rawIntensityRef() = Mat1f(FREENECT_FRAME_H, FREENECT_FRAME_W);
startKinect();
int64 last_grab_time = 0;
while (!threadShouldExit())
{
waitForNewEvent(-1); // Use infinite timeout in order to honor sync mode.
while (m_depth_transmitted || m_rgb_transmitted)
freenect_process_events(f_ctx);
// m_current_image.rawDepth().copyTo(m_current_image.rawAmplitudeRef());
// m_current_image.rawDepth().copyTo(m_current_image.rawIntensityRef());
{
int64 grab_time = ntk::Time::getMillisecondCounter();
ntk_dbg_print(grab_time - last_grab_time, 2);
last_grab_time = grab_time;
QWriteLocker locker(&m_lock);
// FIXME: ugly hack to handle the possible time
// gaps between rgb and IR frames in dual mode.
if (m_dual_ir_rgb)
m_current_image.copyTo(m_rgbd_image);
else
m_current_image.swap(m_rgbd_image);
m_rgb_transmitted = true;
m_depth_transmitted = true;
}
if (m_dual_ir_rgb)
setIRMode(!m_ir_mode);
advertiseNewFrame();
#ifdef _WIN32
// FIXME: this is to avoid GUI freezes with libfreenect on Windows.
// See http://groups.google.com/group/openkinect/t/b1d828d108e9e69
Sleep(1);
#endif
}
}
void *freenect_threadfunc(void *arg)
{
int accelCount = 0;
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_video_callback(f_dev, rgb_cb);
freenect_set_video_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, current_format));
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_video_buffer(f_dev, rgb_back);
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
printf("'w' - tilt up, 's' - level, 'x' - tilt down, '0'-'6' - select LED mode, '+' & '-' - change IR intensity \n");
printf("'f' - change video format, 'm' - mirror video, 'o' - rotate video with accelerometer \n");
printf("'e' - auto exposure, 'b' - white balance, 'r' - raw color, 'n' - near mode (K4W only) \n");
while (!die && freenect_process_events(f_ctx) >= 0) {
//Throttle the text output
if (accelCount++ >= 2000)
{
accelCount = 0;
freenect_raw_tilt_state* state;
freenect_update_tilt_state(f_dev);
state = freenect_get_tilt_state(f_dev);
double dx,dy,dz;
freenect_get_mks_accel(state, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", state->accelerometer_x, state->accelerometer_y, state->accelerometer_z, dx, dy, dz);
fflush(stdout);
}
if (requested_format != current_format) {
freenect_stop_video(f_dev);
freenect_set_video_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, requested_format));
freenect_start_video(f_dev);
current_format = requested_format;
}
}
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_video(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
printf("-- done!\n");
return NULL;
}
void *freenect_threadfunc(void *arg)
{
printf("Freenect thread\n");
int accelCount = 0;
//freenect_set_tilt_degs(f_dev,freenect_angle);
//freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_video_callback(f_dev, rgb_cb);
freenect_set_video_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, current_format));
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_video_buffer(f_dev, rgb_back);
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode, 'f'-video format\n");
while (!die && freenect_process_events(f_ctx) >= 0) {
//Throttle the text output
if (accelCount++ >= 2000)
{
accelCount = 0;
//freenect_raw_tilt_state* state;
//freenect_update_tilt_state(f_dev);
//state = freenect_get_tilt_state(f_dev);
double dx,dy,dz;
//freenect_get_mks_accel(state, &dx, &dy, &dz);
//printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", state->accelerometer_x, state->accelerometer_y, state->accelerometer_z, dx, dy, dz);
fflush(stdout);
}
if (requested_format != current_format) {
freenect_stop_video(f_dev);
freenect_set_video_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, requested_format));
freenect_start_video(f_dev);
current_format = requested_format;
}
}
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_video(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
printf("-- done!\n");
return NULL;
}
int main(int argc, char **argv)
{
int i, res;
for (i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
g_argc = argc;
g_argv = argv;
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (argc > 1)
user_device_number = atoi(argv[1]);
if (nr_devices < 1)
return 1;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return 1;
}
if ( network_init() < 0 )
return -1;
res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
while(!die && freenect_process_events(f_ctx) >= 0 );
return 0;
}
/*
* Main method; primarily just initializes our thread and handles Kinect details.
*/
int main(int argc, char **argv)
{
freenect_context *f_ctx;
freenect_device *f_dev;
display = 0;
int res = 0;
int die = 0;
printf("Code Red Kinect Vision init\n");
if (argc > 1 && strcmp(argv[1],"--display") == 0) {
display = 1;
}
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
if (freenect_open_device(f_ctx, &f_dev, 0) < 0) {
printf("Could not open device\n");
return 1;
}
if (display) { cvNamedWindow( FREENECTOPENCV_WINDOW_N, CV_WINDOW_AUTOSIZE ); }
rgbimg = cvCreateImage(cvSize(FREENECTOPENCV_RGB_WIDTH, FREENECTOPENCV_RGB_HEIGHT), IPL_DEPTH_8U, FREENECTOPENCV_IR_DEPTH);
tempimg = cvCreateImage(cvSize(FREENECTOPENCV_RGB_WIDTH, FREENECTOPENCV_RGB_HEIGHT), IPL_DEPTH_8U, FREENECTOPENCV_RGB_DEPTH);
// create opencv display thread
res = pthread_create(&cv_thread, NULL, cv_threadfunc, (void*) tempimg);
if (res) {
printf("pthread_create failed\n");
return 1;
}
freenect_set_video_callback(f_dev, rgb_cb);
freenect_set_video_format(f_dev, FREENECT_VIDEO_IR_8BIT);
printf("init done\n");
freenect_start_video(f_dev);
while(!die && freenect_process_events(f_ctx) >= 0 );
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "kinect_audio");
freenect_context * contex;
if (freenect_init(&contex, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(contex, FREENECT_LOG_SPEW);
freenect_select_subdevices(contex, FREENECT_DEVICE_AUDIO);
int nr_devices = freenect_num_devices(contex);
printf ("Number of devices found: %d\n", nr_devices);
if (nr_devices < 1) {
freenect_shutdown(contex);
return 1;
}
freenect_device * device;
int deviceNumber = 0;
if (freenect_open_device(contex, &device, deviceNumber) < 0) {
printf("Could not open device\n");
freenect_shutdown(contex);
return 1;
}
freenect_set_audio_in_callback(device, callback);
freenect_start_audio(device);
signal(SIGINT, cleanup);
audioPublisher = ros::NodeHandle().advertise<kinect_audio::audio>("audio", 10);
while(!die && freenect_process_events(contex) >= 0) {
// If we did anything else, it might go here.
// Alternately, we might split off another thread
// to do this loop while the main thread did something
// interesting.
}
freenect_shutdown(contex);
return 0;
}
// Do not call directly, thread runs here
void operator()() {
while(!m_stop) {
//if(freenect_process_events(m_ctx) < 0) throw std::runtime_error("Cannot process freenect events");
int res = freenect_process_events(m_ctx);
if (res < 0)
{
// libusb signals an error has occurred
if (res == LIBUSB_ERROR_INTERRUPTED)
{
// This happens sometimes, it means that a system call in libusb was interrupted somehow (perhaps due to a signal)
// The simple solution seems to be just ignore it.
continue;
}
std::stringstream ss;
ss << "Cannot process freenect events (libusb error code: " << res << ")";
throw std::runtime_error(ss.str());
}
}
}
// the freenect thread.
void *freenect_threadfunc(void *arg) {
int accelCount = 0;
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_video_callback(f_dev, rgb_cb);
freenect_set_video_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, current_format));
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_video_buffer(f_dev, rgb_back);
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
while (!die && freenect_process_events(f_ctx) >= 0) {
if (accelCount++ >= 2000)
{
accelCount = 0;
freenect_raw_tilt_state* state;
freenect_update_tilt_state(f_dev);
state = freenect_get_tilt_state(f_dev);
double dx,dy,dz;
freenect_get_mks_accel(state, &dx, &dy, &dz);
}
if (requested_format != current_format) {
freenect_stop_video(f_dev);
freenect_set_video_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, requested_format));
freenect_start_video(f_dev);
current_format = requested_format;
}
}
freenect_stop_depth(f_dev);
freenect_stop_video(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
return NULL;
}
void qKinect::updateRTView()
{
if (!m_kDlg)
return;
s_depth_count = 0;
s_rgb_count = 0;
freenect_process_events(f_ctx);
if (s_last_rgb_data && s_rgb_count)
{
QPixmap pixmap = QPixmap::fromImage(QImage(s_last_rgb_data,s_wDepth,s_hDepth,QImage::Format_RGB888));
m_kDlg->view2D->setPixmap(pixmap);
}
if (s_depth_data && s_depth_count)
{
QImage image(s_wDepth,s_hDepth,QImage::Format_RGB888);
//convert depth array to image
const uint16_t* _depth = s_depth_data;
unsigned char* _bits = image.bits();
for (unsigned i=0;i<s_hDepth*s_wDepth;++i,++_depth,_bits+=3)
{
if (*_depth < FREENECT_DEPTH_RAW_NO_VALUE)
{
//see http://openkinect.org/wiki/Imaging_Information
float z = 12.36f * tanf((float)(*_depth) / 2842.5f + 1.1863f) - 3.7f;
//HSV --> V=1, S=1, H = cycling
ccNormalVectors::ConvertHSVToRGB((abs((int)z*10))%360,1.0,1.0,_bits[0],_bits[1],_bits[2]);
}
else
{
_bits[0]=_bits[1]=_bits[2]=0;
}
}
m_kDlg->viewDepth->setPixmap(QPixmap::fromImage(image));
}
}
void *data_in(void *arg) {
int n;
while(!die && freenect_process_events(f_ctx) >= 0 )
{
if(data_connected == 1){
char buffer[6];
n = read(data_child, buffer, 1024);
//printf("n: %d\n", n);
if(n == 6){
if (buffer[0] == 1) { //MOTOR
if (buffer[1] == 1) { //MOVE
int angle;
memcpy(&angle, &buffer[2], sizeof(int));
freenect_set_tilt_degs(f_dev,ntohl(angle));
}
}
}
}
}
return NULL;
}
void *capture_loop(void *arg) {
freenect_set_depth_callback(device, depth_captured);
freenect_set_depth_mode(device, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_MM));
freenect_start_depth(device);
freenect_set_video_callback(device, rgb_captured);
freenect_set_video_mode(device, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_video_buffer(device, kinect_rgb_buffer);
freenect_start_video(device);
while (freenect_process_events(context) >= 0) {
if (die) break;
}
printf("shutting down streams...\n");
freenect_stop_depth(device);
freenect_close_device(device);
freenect_shutdown(context);
printf("done!\n");
return NULL;
}
void kinect_thread(void* user) {
Kinect* kinect = static_cast<Kinect*>(user);
if(!kinect) {
printf("Error: kinect thread didn't receive a reference to the Kinect instance.\n");
::exit(EXIT_FAILURE);
}
freenect_device* dev = kinect->device;
//freenect_set_tilt_degs(dev, 15);
freenect_set_led(dev, LED_RED);
freenect_set_depth_callback(dev, kinect_depth_callback);
freenect_set_video_callback(dev, kinect_video_callback);
freenect_set_video_mode(dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_depth_mode(dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_video_buffer(dev, kinect->rgb_back);
freenect_set_depth_buffer(dev, kinect->depth_back);
freenect_start_depth(dev);
freenect_start_video(dev);
bool must_stop = false;
while(freenect_process_events(kinect->ctx) >= 0) {
uv_mutex_lock(&kinect->mutex);
must_stop = kinect->must_stop;
uv_mutex_unlock(&kinect->mutex);
if(must_stop) {
break;
}
}
freenect_set_led(dev, LED_GREEN);
freenect_stop_depth(dev);
freenect_stop_video(dev);
}
void *network_data(void *arg)
{
int childlen;
struct sockaddr_in childaddr;
childlen = sizeof(childaddr);
while ( !die )
{
printf("### Wait data client\n");
data_child = accept(s_data, (struct sockaddr *)&childaddr, (unsigned int *)&childlen);
if ( data_child < 0 )
{
fprintf(stderr, "Error on accept() for data, exit data thread.\n");
break;
}
printf("### Got data client\n");
while(!die && freenect_process_events(f_ctx) >= 0 )
{
int n;
int16_t ax,ay,az;
freenect_get_raw_accel(f_dev, &ax, &ay, &az);
double dx,dy,dz;
freenect_get_mks_accel(f_dev, &dx, &dy, &dz);
char buffer_send[3*2+3*8];
memcpy(&buffer_send,&ax, sizeof(int16_t));
memcpy(&buffer_send[2],&ay, sizeof(int16_t));
memcpy(&buffer_send[4],&az, sizeof(int16_t));
memcpy(&buffer_send[6],&dx, sizeof(double));
memcpy(&buffer_send[14],&dy, sizeof(double));
memcpy(&buffer_send[22],&dz, sizeof(double));
n = write(data_child, buffer_send, 3*2+3*8);
}
}
return NULL;
}
void init()
{
freenect_context *ctx;
freenect_device *dev;
if (freenect_init(&ctx, 0)) {
printf("Error: Cannot get context\n");
return;
}
// fakenect doesn't support audio yet, so don't bother claiming the device
freenect_select_subdevices(ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
if (freenect_open_device(ctx, &dev, 0)) {
printf("Error: Cannot get device\n");
return;
}
print_mode("Depth", freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
print_mode("Video", freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_depth_mode(dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_start_depth(dev);
freenect_set_video_mode(dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_start_video(dev);
if (use_ffmpeg) {
init_ffmpeg_streams();
freenect_set_depth_callback(dev, depth_cb_ffmpeg);
freenect_set_video_callback(dev, rgb_cb_ffmpeg);
} else {
freenect_set_depth_callback(dev, depth_cb);
freenect_set_video_callback(dev, rgb_cb);
}
while (running && freenect_process_events(ctx) >= 0)
snapshot_accel(dev);
freenect_stop_depth(dev);
freenect_stop_video(dev);
freenect_close_device(dev);
freenect_shutdown(ctx);
}
void *freenect_threadfunc(void *arg)
{
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
freenect_start_depth(f_dev);
freenect_start_rgb(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
while(!die && freenect_process_events(f_ctx) >= 0 )
{
freenect_raw_device_state* state;
freenect_update_device_state(f_dev);
state = freenect_get_device_state(f_dev);
double dx,dy,dz;
freenect_get_mks_accel(state, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", state->accelerometer_x, state->accelerometer_y, state->accelerometer_z, dx, dy, dz);
fflush(stdout);
}
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_rgb(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
printf("-- done!\n");
return NULL;
}
