Esempio n. 1
0
//---------------------------------------------------------------------------
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";
}
Esempio n. 2
0
//---------------------------------------------------------------------------
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");
}
Esempio n. 3
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);
}
Esempio n. 4
0
void keyPressed(unsigned char key, int x, int y)
{
	if (key == 27) {
		die = 1;
		pthread_join(freenect_thread, NULL);
		glutDestroyWindow(window);
		free(depth_mid);
		free(depth_front);
		free(rgb_back);
		free(rgb_mid);
		free(rgb_front);
		pthread_exit(NULL);
	}
	if (key == 'w') {
		freenect_angle++;
		if (freenect_angle > 30) {
			freenect_angle = 30;
		}
	}
	if (key == 's') {
		freenect_angle = 0;
	}
	if (key == 'f') {
		if (requested_format == FREENECT_VIDEO_IR_8BIT)
			requested_format = FREENECT_VIDEO_RGB;
		else if (requested_format == FREENECT_VIDEO_RGB)
			requested_format = FREENECT_VIDEO_YUV_RGB;
		else
			requested_format = FREENECT_VIDEO_IR_8BIT;
	}
	if (key == 'x') {
		freenect_angle--;
		if (freenect_angle < -30) {
			freenect_angle = -30;
		}
	}
	if (key == '1') {
		freenect_set_led(f_dev,LED_GREEN);
	}
	if (key == '2') {
		freenect_set_led(f_dev,LED_RED);
	}
	if (key == '3') {
		freenect_set_led(f_dev,LED_YELLOW);
	}
	if (key == '4') {
		freenect_set_led(f_dev,LED_BLINK_GREEN);
	}
	if (key == '5') {
		// 5 is the same as 4
		freenect_set_led(f_dev,LED_BLINK_GREEN);
	}
	if (key == '6') {
		freenect_set_led(f_dev,LED_BLINK_RED_YELLOW);
	}
	if (key == '0') {
		freenect_set_led(f_dev,LED_OFF);
	}
	freenect_set_tilt_degs(f_dev,freenect_angle);
}
Esempio n. 5
0
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;
}
Esempio n. 6
0
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;
}
Esempio n. 7
0
//---------------------------------------------------------------------------
void ofxKinect::threadedFunction() {


    freenect_set_led(kinectDevice, LED_GREEN);
    freenect_set_video_format(kinectDevice, bInfrared?FREENECT_VIDEO_IR_8BIT:FREENECT_VIDEO_RGB);
    freenect_set_depth_format(kinectDevice, FREENECT_DEPTH_11BIT);
    freenect_set_depth_callback(kinectDevice, &grabDepthFrame);
    freenect_set_video_callback(kinectDevice, &grabRgbFrame);

    ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection opened");

    freenect_start_depth(kinectDevice);
    freenect_start_video(kinectDevice);

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

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

//TODO: uncomment these when they are implemented in freenect
    freenect_set_tilt_degs(kinectDevice, 0);
    freenect_update_tilt_state(kinectDevice);
    freenect_stop_depth(kinectDevice);
    freenect_stop_video(kinectDevice);
    freenect_set_led(kinectDevice, LED_YELLOW);

    freenect_close_device(kinectDevice);
    freenect_shutdown(kinectContext);

    ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection closed");
}
Esempio n. 8
0
File: kkcd.c Progetto: dlowe/kkcd
int main(int argc, char **argv) {
    freenect_context *fc;
    freenect_device *fd;

    if (freenect_init(&fc, 0) < 0) {
        printf("failed to init freenect\n");
        exit(1);
    }

    if (freenect_num_devices(fc) < 1) {
        printf("no kinect found\n");
        freenect_shutdown(fc);
        exit(1);
    }

    if (freenect_open_device(fc, &fd, 0) < 0) {
        printf("unable to open device 0\n");
        freenect_shutdown(fc);
        exit(1);
    }

    double want;
    while (1) {
        want = 30.0;
        freenect_set_tilt_degs(fd, want);
        freenect_set_led(fd, LED_RED);
        while (fabs(freenect_get_tilt_degs(freenect_get_tilt_state(fd)) - want) > 1) {
            freenect_update_tilt_state(fd);
            usleep(1000);
        }

        want = -30.0;
        freenect_set_tilt_degs(fd, want);
        freenect_set_led(fd, LED_GREEN);
        while (fabs(freenect_get_tilt_degs(freenect_get_tilt_state(fd)) - want) > 1) {
            freenect_update_tilt_state(fd);
            usleep(1000);
        }
    }

    freenect_close_device(fd);
    freenect_shutdown(fc);
    exit(0);
}
Esempio n. 9
0
void jit_freenect_grab_set_tilt(t_jit_freenect_grab *x,  void *attr, long argc, t_atom *argv)
{
	if(argv){
		x->tilt = jit_atom_getlong(argv);
		
		CLIP_ASSIGN(x->tilt, -30, 30);
		
		if(x->device){
			freenect_set_tilt_degs(x->device,x->tilt);
		}
	}
}
Esempio n. 10
0
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;
}
Esempio n. 11
0
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_format(f_dev, current_format);
	freenect_set_depth_format(f_dev, 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_format(f_dev, 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 KinectInterface::setAngle(
                                             double _angle
                                            )
{
    // constrain the angle from -30 - +30
    if(_angle > 30)
    {
        _angle = 30;
    }
    else if(_angle <-30)
    {
        _angle=-30;
    }
    freenect_set_tilt_degs(m_dev,_angle);
}
Esempio n. 13
0
int initFreenect() {
  //  int res = 0;

  //setup Freenect...
  if (freenect_init(&f_ctx, NULL) < 0) {
    printf("freenect_init() failed\n");
    return 1;
  }

  freenect_set_log_level(f_ctx, FREENECT_LOG_ERROR);

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

  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_video_callback(f_dev, rgb_cb);
  freenect_set_video_format(f_dev, FREENECT_VIDEO_RGB);
  freenect_set_depth_format(f_dev, FREENECT_DEPTH_11BIT);

  freenect_start_depth(f_dev);
  freenect_start_video(f_dev);

  //start the freenect thread to poll for events
  //  res = pthread_create(&ocv_thread, NULL, freenect_threadfunc, NULL);
  //  if (res) {
  //    printf("pthread_create failed\n");
  //    return 1;
  //  }
  return 0;
}
Esempio n. 14
0
void keyPressed(unsigned char key, int x, int y)
{
	if (key == 27) {
		die = 1;
		pthread_join(freenect_thread, NULL);
		glutDestroyWindow(window);
		pthread_exit(NULL);
	}
	if (key == 'w') {
		freenect_angle++;
		if (freenect_angle > 30) {
			freenect_angle = 30;
		}
	}
	if (key == 's') {
		freenect_angle = 0;
	}
	if (key == 'x') {
		freenect_angle--;
		if (freenect_angle < -30) {
			freenect_angle = -30;
		}
	}
	if (key == '1') {
		freenect_set_led(f_dev,LED_GREEN);
	}
	if (key == '2') {
		freenect_set_led(f_dev,LED_RED);
	}
	if (key == '3') {
		freenect_set_led(f_dev,LED_YELLOW);
	}
	if (key == '4') {
		freenect_set_led(f_dev,LED_BLINK_YELLOW);
	}
	if (key == '5') {
		freenect_set_led(f_dev,LED_BLINK_GREEN);
	}
	if (key == '6') {
		freenect_set_led(f_dev,LED_BLINK_RED_YELLOW);
	}
	if (key == '0') {
		freenect_set_led(f_dev,LED_OFF);
	}
	freenect_set_tilt_degs(f_dev,freenect_angle);
}
Esempio n. 15
0
int main(int argc, char **argv)
{
	ros::init(argc, argv, "kinect_motor");

	freenect_context * context = 0;
	if (freenect_init(&context, NULL) < 0) {
		ROS_ERROR("freenect_init() failed");
		return 1;
	}

	freenect_set_log_level(context, FREENECT_LOG_DEBUG);
	freenect_select_subdevices(context, (freenect_device_flags)(FREENECT_DEVICE_MOTOR));

	int devicesCout = freenect_num_devices (context);
	ROS_INFO("Number of devices found: %d", devicesCout);

	if (devicesCout < 1) {
		freenect_shutdown(context);
		return 1;
	}

	ROS_INFO("Connecting to device: 1");

	if (freenect_open_device(context, &device, 0) < 0) {
		printf("Could not open device\n");
		freenect_shutdown(context);
		return 1;
	}

	freenect_set_tilt_degs(device, 0);

	ros::NodeHandle nodeHandleForAngle;
	ros::Subscriber angleSubscriber = nodeHandleForAngle.subscribe("angle", 10, Angle);

	ros::NodeHandle nodeHandleForLed;
	ros::Subscriber ledSubscriber = nodeHandleForLed.subscribe("led", 10, Led);

	ros::spin();

	freenect_close_device(device);
	freenect_shutdown(context);

	return 0;
}
Esempio n. 16
0
int main (int argc, char **argv) {
    // Initalize the ROS node
    ros::init(argc, argv, "kinect_base_node");
    ros::NodeHandle n;
    // Initalize the Freenect Context
    if (freenect_init (&f_ctx, NULL) < 0) {
        ROS_INFO("freenect_init() failed\n");
        return 1;
    }
    freenect_set_log_level (f_ctx, FREENECT_LOG_INFO);
    // Scan for kinect devices
    int nr_devices = freenect_num_devices (f_ctx);
    ROS_INFO("Number of devices found: %d\n", nr_devices);
    // Get the device number
    if (argc > 1) user_device_number = atoi (argv[1]);
    if (nr_devices < 1) return 1;
    // Calculate the kinect node base name
    std::stringstream kinect_node_base;
    kinect_node_base << "/kinect_base_node/" << user_device_number << "/";
    // Open the base portion of the Kinect
    if (freenect_open_device (f_ctx, &f_dev, user_device_number) < 0) {
        ROS_INFO("Could not open device\n");
        return 1;
    }
    // Get the defaults 
    double  tiltDefaultPosition = 0.0;
    double  imuDefaultDuration = 1.0;
    int     ledDefaultState = LED_BLINK_GREEN;
    n.getParam(kinect_node_base.str() + "tilt", tiltDefaultPosition);
    n.getParam(kinect_node_base.str() + "led", ledDefaultState);
    n.getParam(kinect_node_base.str() + "imuDuration", imuDefaultDuration);
    // Set the default kinect state
    freenect_set_tilt_degs(f_dev, tiltDefaultPosition);
    freenect_set_led(f_dev, (freenect_led_options) ledDefaultState);
    // Create the provided services
    ros::Subscriber subTilt = n.subscribe(kinect_node_base.str() + "tilt", 1000, tilt_received_callback);
    ros::Subscriber subLED = n.subscribe(kinect_node_base.str() + "led", 1000, led_received_callback);
    imu_publisher = n.advertise<sensor_msgs::Imu>(kinect_node_base.str() + "imu", 1000);
    ros::Timer imu_publish_timer = n.createTimer(ros::Duration(imuDefaultDuration), imu_publish_data);
    ros::spin();   
    return 0;
}
// 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;
}
Esempio n. 18
0
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;
}
Esempio n. 19
0
void keyPressed( unsigned char key, int x, int y )
{
	if( key == 27 )
	{
		CloseAll();
	}
	if( key == 'w' )
	{
		f_angle++;
		if( f_angle > 30 )
			f_angle = 30;
	}
	if( key == 's' )
	{
		f_angle--;
		if( f_angle < -30 )
			f_angle = -30;
 	}

	if( key == 'x' )
	{
		f_angle = 0;
 	}
	if( key == 'q'  )
	{
		offset++;
		if( offset > maxdepth/3 )
			offset = maxdepth/3;
	}
	if( key == 'e' )
	{
		offset--;
		if( offset < 0 )
			offset = 0;
	}
	freenect_set_tilt_degs( f_device, f_angle );
}
Esempio n. 20
0
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;
}
Esempio n. 21
0
void*  f_threadfunc( void *arg )
{
	freenect_set_tilt_degs( f_device, f_angle );
	freenect_set_led( f_device, LED_RED );
	freenect_set_depth_callback( f_device, depth_cb );
	freenect_set_video_callback(f_device, rgb_cb);
	if( (W == 640) && H ==( 480 ))
	{
		freenect_set_video_mode( f_device, freenect_find_video_mode( FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB ) );
		printf("Resolution:%d:%d\n",W,H);
		fflush(stdout);
	}
	else
	if(( W == 1280) &&( H == 1024) )
	{
		freenect_set_video_mode( f_device, freenect_find_video_mode( FREENECT_RESOLUTION_HIGH, FREENECT_VIDEO_RGB ) );
		printf("Resolution:%d:%d\n",W,H);
		fflush(stdout);
	}
	else
	{
		printf("Unknown resolution. Shutting down...\n");
		CloseAll();
	}
	freenect_set_depth_mode( f_device, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT) );
	freenect_start_video( f_device );
	freenect_start_depth( f_device );
	while ( !shutdown && freenect_process_events( f_context ) >= 0 )
	{
	
	}
	CloseAll();
	printf( "Done. Shutting down...\n" );
	fflush(stdout);
	return NULL;
}
Esempio n. 22
0
void keyPressed(unsigned char key, int x, int y)
{
	if (key == 27) {
		die = 1;
		pthread_join(freenect_thread, NULL);
		glutDestroyWindow(window);
		free(depth_mid);
		free(depth_front);
		free(rgb_back);
		free(rgb_mid);
		free(rgb_front);
		// Not pthread_exit because OSX leaves a thread lying around and doesn't exit
		exit(0);
	}
	if (key == 'w') {
		freenect_angle++;
		if (freenect_angle > 30) {
			freenect_angle = 30;
		}
		tilt_changed++;
	}
	if (key == 's') {
		freenect_angle = 0;
		tilt_changed++;
	}
	if (key == 'f') {
		if (requested_format == FREENECT_VIDEO_IR_8BIT)
			requested_format = FREENECT_VIDEO_RGB;
		else if (requested_format == FREENECT_VIDEO_RGB)
			requested_format = FREENECT_VIDEO_YUV_RGB;
		else
			requested_format = FREENECT_VIDEO_IR_8BIT;
	}
	if (key == 'x') {
		freenect_angle--;
		if (freenect_angle < -30) {
			freenect_angle = -30;
		}
		tilt_changed++;
	}
	if (key == 'e') {
		static freenect_flag_value auto_exposure = FREENECT_ON;
		freenect_set_flag(f_dev, FREENECT_AUTO_EXPOSURE, auto_exposure);
		auto_exposure = auto_exposure ? FREENECT_OFF : FREENECT_ON;
	}
	if (key == 'b') {
		static freenect_flag_value white_balance = FREENECT_ON;
		freenect_set_flag(f_dev, FREENECT_AUTO_WHITE_BALANCE, white_balance);
		white_balance = white_balance ? FREENECT_OFF : FREENECT_ON;
	}
	if (key == 'r') {
		static freenect_flag_value raw_color = FREENECT_ON;
		freenect_set_flag(f_dev, FREENECT_RAW_COLOR, raw_color);
		raw_color = raw_color ? FREENECT_OFF : FREENECT_ON;
	}
	if (key == 'm') {
		static freenect_flag_value mirror = FREENECT_ON;
		freenect_set_flag(f_dev, FREENECT_MIRROR_DEPTH, mirror);
		freenect_set_flag(f_dev, FREENECT_MIRROR_VIDEO, mirror);
		mirror = mirror ? FREENECT_OFF : FREENECT_ON;
	}
	if (key == 'n') {
		static freenect_flag_value near_mode = FREENECT_ON;
		freenect_set_flag(f_dev, FREENECT_NEAR_MODE, near_mode);
		near_mode = near_mode ? FREENECT_OFF : FREENECT_ON;
	}

	if (key == '+') {
		uint16_t brightness = freenect_get_ir_brightness(f_dev) + 2;
		freenect_set_ir_brightness(f_dev, brightness);
	}
	if (key == '-') {
		uint16_t brightness = freenect_get_ir_brightness(f_dev) - 2;
		freenect_set_ir_brightness(f_dev, brightness);
	}

	if (key == '1') {
		freenect_set_led(f_dev,LED_GREEN);
	}
	if (key == '2') {
		freenect_set_led(f_dev,LED_RED);
	}
	if (key == '3') {
		freenect_set_led(f_dev,LED_YELLOW);
	}
	if (key == '4') {
		freenect_set_led(f_dev,LED_BLINK_GREEN);
	}
	if (key == '5') {
		// 5 is the same as 4
		freenect_set_led(f_dev,LED_BLINK_GREEN);
	}
	if (key == '6') {
		freenect_set_led(f_dev,LED_BLINK_RED_YELLOW);
	}
	if (key == '0') {
		freenect_set_led(f_dev,LED_OFF);
	}

	if (key == 'o') {
	    if (camera_rotate) {
	        camera_rotate = 0;
	        glDisable(GL_DEPTH_TEST);
	    }
	    else {
	        camera_rotate = 1;
	        glEnable(GL_DEPTH_TEST);
	    }
	}
	if (tilt_changed) {
	    freenect_set_tilt_degs(f_dev, freenect_angle);
	    tilt_changed = 0;
	}
}
Esempio n. 23
0
void FreenectGrabber :: setTiltAngle(int angle)
{
    if (!isConnected()) return;
    freenect_set_tilt_degs(f_dev, angle);
}
		void setTiltDegrees(double _angle) {
			if(freenect_set_tilt_degs(m_dev, _angle) < 0) throw std::runtime_error("Cannot set angle in degrees");
		}
Esempio n. 25
0
void qKinect::doStartGrabbing()
{
	assert(m_app);
	if (!m_app)
		return;

	f_ctx=0;
    f_dev=0;
	s_grabIndex=0;

	if (m_kDlg)
		delete m_kDlg;
	m_kDlg=0;

	s_max_depth_count = 0;
	if (s_depth_data)
		delete[] s_depth_data;
	s_depth_count = 0;
	s_depth_data = 0;
	s_wDepth = s_hDepth = 0;

	if (s_last_rgb_data)
		delete[] s_last_rgb_data;
	s_last_rgb_data = 0;
	s_rgb_count = 0;
	s_wRgb = s_hRgb = 0;

	if (freenect_init(&f_ctx, NULL) < 0)
	{
		m_app->dispToConsole("[qKinect] Failed to initialize kinect driver!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
		return;
	}

	freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);

	int nr_devices = freenect_num_devices(f_ctx);
	m_app->dispToConsole(qPrintable(QString("[qKinect] Number of devices found: %1").arg(nr_devices)));

	if (nr_devices < 1)
		return;

	if (freenect_open_device(f_ctx, &f_dev, 0) < 0)
	{
		m_app->dispToConsole("[qKinect] Failed to initialize kinect device!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
		return;
	}

	//test: try to init high resolution mode
	//freenect_frame_mode upDepthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_HIGH, FREENECT_DEPTH_11BIT);
	//int success = freenect_set_depth_mode(f_dev,upDepthMode);

	/*** Depth information ***/
	freenect_frame_mode depthMode = freenect_get_current_depth_mode(f_dev);
	if (!depthMode.depth_format == FREENECT_DEPTH_11BIT)
	{
		depthMode = freenect_find_depth_mode(depthMode.resolution, FREENECT_DEPTH_11BIT);
		if (freenect_set_depth_mode(f_dev,depthMode)<0)
		{
			m_app->dispToConsole("[qKinect] Failed to initialiaze depth mode!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
			return;
		}
	}
	if (!getResolution(depthMode.resolution,s_wDepth,s_hDepth))
	{
		m_app->dispToConsole("[qKinect] Failed to read depth resolution!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
		return;
	}
	m_app->dispToConsole(qPrintable(QString("[qKinect] Depth resolution: %1 x %2").arg(s_wDepth).arg(s_hDepth)));

	s_depth_data = new uint16_t[s_wDepth*s_hDepth];
	if (!s_depth_data)
	{
		m_app->dispToConsole("[qKinect] Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
		return;
	}
	s_max_depth_count = 1;

	/*** RGB information ***/
	bool grabRGB = true;
	{
		freenect_frame_mode rgbMode = freenect_get_current_video_mode(f_dev);
		if (!rgbMode.video_format == FREENECT_VIDEO_RGB || depthMode.resolution != rgbMode.resolution)
		{
			rgbMode = freenect_find_video_mode(depthMode.resolution, FREENECT_VIDEO_RGB);
			if (freenect_set_video_mode(f_dev,rgbMode)<0)
			{
				m_app->dispToConsole("[qKinect] Can't find a video mode compatible with current depth mode?!");
				grabRGB = false;
			}
		}

		//still want to/can grab RGB info?
		if (grabRGB)
		{
			getResolution(rgbMode.resolution,s_wRgb,s_hRgb);

			s_last_rgb_data = new uint8_t[s_wRgb*s_hRgb*3];
			if (!s_last_rgb_data) //not enough memory for RGB
			{
				m_app->dispToConsole("[qKinect] Not enough memory to grab RGB info!");
				grabRGB = false;
			}
			else
			{
				m_app->dispToConsole(qPrintable(QString("[qKinect] RGB resolution: %1 x %2").arg(s_wRgb).arg(s_hRgb)));
			}
		}
	}

    int freenect_angle = 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);
	if (grabRGB)
		freenect_set_video_buffer(f_dev, s_last_rgb_data);

	freenect_start_depth(f_dev);
	if (s_last_rgb_data)
		freenect_start_video(f_dev);

	m_kDlg = new ccKinectDlg(m_app->getMainWindow());
	if (grabRGB)
		m_kDlg->addMode(QString("%1 x %2").arg(s_wDepth).arg(s_hDepth));
	else
		m_kDlg->grabRGBCheckBox->setChecked(false);
	m_kDlg->grabRGBCheckBox->setEnabled(grabRGB);
	m_kDlg->grabPushButton->setEnabled(true);

	connect(m_kDlg->grabPushButton, SIGNAL(clicked()), this, SLOT(grabCloud()));
	connect(m_kDlg, SIGNAL(finished(int)), this, SLOT(dialogClosed(int)));

	//m_kDlg->setModal(false);
	//m_kDlg->setWindowModality(Qt::NonModal);
	m_kDlg->show();

	if (!m_timer)
	{
		m_timer = new QTimer(this);
		connect(m_timer, SIGNAL(timeout()), this, SLOT(updateRTView()));
	}
	m_timer->start(0);
}
Esempio n. 26
0
int main(int argc, char **argv)
{

    cv_depth_mat = cvCreateMat(480, 640, CV_16UC1);
    cv_rgb_mat = cvCreateMat(480, 640, CV_8UC3);

    int res;
	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_INFO);

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

	//freenect_set_tilt_degs(f_dev,15);
	freenect_set_tilt_degs(f_dev,0);
	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_format(f_dev, FREENECT_VIDEO_RGB);
	freenect_set_depth_format(f_dev, FREENECT_DEPTH_11BIT);

	freenect_start_depth(f_dev);
	freenect_start_video(f_dev);

    cvNamedWindow("rgb", CV_WINDOW_NORMAL);
    cvNamedWindow("depth", CV_WINDOW_NORMAL);
    cvNamedWindow("depth_th", CV_WINDOW_NORMAL);
    cvNamedWindow("contourWin", CV_WINDOW_NORMAL);

    CvMat* cv_depth_threshold_mat = cvCreateMat(480,640, CV_8UC1);

	res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
	if (res) {
		printf("pthread_create failed\n");
		return 1;
	}

    // Variables for contour finding
    CvSeq* contours = NULL;
    CvMemStorage* memStorage = cvCreateMemStorage(0);
    IplImage* contour_image = cvCreateImage(cvSize(640,480), 8, 1);


    //hist_segment_init();
    //feature_extract_init();

    optflow_init(cvSize(640,480));

	while (!die)
	{
        cvConvertScale(cv_depth_mat, cv_depth_threshold_mat, 255.0/2048.0, 0);

        // 120 ~ 2.7 m
        cvThreshold( cv_depth_threshold_mat, cv_depth_threshold_mat, 120.0, 120.0, CV_THRESH_TOZERO_INV);

        //cvThreshold( cv_depth_threshold_mat, cv_depth_threshold_mat, 120.0, 255.0, CV_THRESH_BINARY_INV);

        // Find contours
        /*cvClearMemStorage(memStorage);
        cvFindContours( cv_depth_threshold_mat, memStorage, &contours, sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0));
        cvZero(contour_image);

        // Draw the contours
        if (contours)
        {
            cvDrawContours(contour_image, contours, cvScalarAll(255.0), cvScalarAll(255.0), 1, 1, 8, cvPoint(0,0));
        }
        cvShowImage("contourWin",contour_image);*/

        //extractFeatures(cv_depth_threshold_mat);
        //hist_segment(cv_depth_threshold_mat, NULL);
        optflow_calculate(cv_depth_threshold_mat, NULL);


        cvShowImage("rgb", cv_rgb_mat);

		cvShowImage("depth_th", cv_depth_threshold_mat);


        char k = cvWaitKey(5);
        if( k == 27 ) break;

    }

    optflow_deinit();
    //hist_segment_deinit();
    //feature_extract_deinit();


	printf("-- done!\n");

	cvDestroyWindow("rgb");
	cvDestroyWindow("depth");
	cvDestroyWindow("depth_th");

    cvReleaseMat(&cv_depth_mat);
    cvReleaseMat(&cv_rgb_mat);

    cvReleaseMat(&cv_depth_threshold_mat);

    // Release Contour variables
    cvDestroyWindow("contourWin");

	pthread_join(freenect_thread, NULL);
	pthread_exit(NULL);
}
Esempio n. 27
0
int main(int argc, char **argv)
{
	int res;
	freenect_context *f_ctx;
	

	printf("Kinect camera test\n");

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


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

	res = pthread_create(&gl_thread, NULL, gl_threadfunc, NULL);
	if (res) {
		printf("pthread_create failed\n");
		return 1;
	}

	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("-- done!\n");

	pthread_exit(NULL);
}
Esempio n. 28
0
void tilt_received_callback(const std_msgs::Float64::ConstPtr& tilt) {
    /* Tilt the camera */
    freenect_set_tilt_degs (f_dev, tilt->data);
    ROS_INFO("Tilting to: %lf degrees", tilt->data);
}
Esempio n. 29
0
int main(int argc, char **argv)
{
	int i;
	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;
	
	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_set_led(f_dev,LED_RED);
	
	while(!die && freenect_process_events(f_ctx) >= 0 ){
		char buffer[6];
		int 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));
				}
			}
		}
	}
	
	network_close();
	
	printf("-- done!\n");
	
	pthread_exit(NULL);
}
Esempio n. 30
0
int main(int argc, char *argv[])
{
	struct kingrid_info data;
	freenect_context *kn;
	freenect_device *kn_dev;
	int rows = 40, cols = 96; // terminal size
	int opt;

	sigdata = &data;
	data.out_of_range = 0;
	data.done = 0;
	data.divisions = 6;
	data.boxwidth = 10;
	data.histrows = 8;
	data.frame = 0;
	data.zmin = 0.5;
	data.zmax = 5.0;
	data.disp_mode = STATS;

	if(getenv("LINES")) {
		rows = atoi(getenv("LINES"));
	}
	if(getenv("COLUMNS")) {
		cols = atoi(getenv("COLUMNS"));
	}

	// Handle command-line options
	while((opt = getopt(argc, argv, "shag:z:Z:")) != -1) {
		switch(opt) {
			case 's':
				// Stats mode
				data.disp_mode = STATS;
				break;
			case 'h':
				// Histogram mode
				data.disp_mode = HISTOGRAM;
				break;
			case 'a':
				// ASCII art mode
				data.disp_mode = ASCII;
				break;
			case 'g':
				// Grid divisions
				data.divisions = atoi(optarg);
				break;
			case 'z':
				// Near clipping
				data.zmin = atof(optarg);
				break;
			case 'Z':
				// Far clipping
				data.zmax = atof(optarg);
				break;
			default:
				fprintf(stderr, "Usage: %s -[sha] [-g divisions] [-zZ distance]\n", argv[0]);
				fprintf(stderr, "Use up to one of:\n");
				fprintf(stderr, "\ts - Stats mode (default)\n");
				fprintf(stderr, "\th - Histogram mode\n");
				fprintf(stderr, "\ta - ASCII art mode\n");
				fprintf(stderr, "Use any of:\n");
				fprintf(stderr, "\tg - Set grid divisions for both dimensions\n");
				fprintf(stderr, "\tz - Set near clipping plane in meters for ASCII art mode (default 0.5)\n");
				fprintf(stderr, "\tZ - Set far clipping plane in meters for ASCII art mode (default 5.0)\n");
				return -1;
		}
	}

	data.boxwidth = (cols - 1) / data.divisions - 3;
	if(data.boxwidth < 10) {
		data.boxwidth = 10;
	}
	data.histrows = (rows - 2) / data.divisions - 1;
	
	init_lut(data.depth_lut);

	if(signal(SIGINT, intr) == SIG_ERR ||
			signal(SIGTERM, intr) == SIG_ERR) {
		ERROR_OUT("Error setting signal handlers\n");
		return -1;
	}

	if(freenect_init(&kn, NULL) < 0) {
		ERROR_OUT("libfreenect init failed.\n");
		return -1;
	}

	INFO_OUT("Found %d Kinect devices.\n", freenect_num_devices(kn));

	if(freenect_num_devices(kn) == 0) {
		ERROR_OUT("No Kinect devices present.\n");
		return -1;
	}

	if(freenect_open_device(kn, &kn_dev, 0)) {
		ERROR_OUT("Error opening Kinect #0.\n");
		return -1;
	}

	freenect_set_user(kn_dev, &data);
	freenect_set_tilt_degs(kn_dev, -5);
	freenect_set_led(kn_dev, LED_GREEN);
	freenect_set_depth_callback(kn_dev, depth);
	freenect_set_depth_format(kn_dev, FREENECT_DEPTH_11BIT);

	freenect_start_depth(kn_dev);

	int last_oor = data.out_of_range;
	while(!data.done && freenect_process_events(kn) >= 0) {
		if(last_oor != data.out_of_range) {
			freenect_set_led(kn_dev, data.out_of_range ? LED_BLINK_RED_YELLOW : LED_GREEN);
			last_oor = data.out_of_range;
		}
	}

	freenect_stop_depth(kn_dev);
	freenect_set_led(kn_dev, LED_OFF);
	freenect_close_device(kn_dev);
	freenect_shutdown(kn);

	return 0;
}