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);
}
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 kinect_finish(Kinect* kt){
pthread_mutex_unlock(&G_kt.mutex);
freenect_stop_depth(kt->dev);
freenect_stop_video(kt->dev);
freenect_close_device(kt->dev);
freenect_shutdown(kt->ctx);
}
bool FreenectGrabber :: disconnectFromDevice()
{
// Exit requested.
m_connected = false;
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
return true;
}
//---------------------------------------------------------------------------
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");
}
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 CloseAll()
{
shutdown = 1;
pthread_join( f_thread, NULL );
freenect_stop_depth( f_device );
freenect_close_device( f_device );
//free(depth_gl);
exit( 0 );
}
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;
}
FREENECTAPI int freenect_shutdown(freenect_context *ctx)
{
while (ctx->first) {
FN_NOTICE("Device %p open during shutdown, closing...\n", ctx->first);
freenect_close_device(ctx->first);
}
fnusb_shutdown(&ctx->usb);
free(ctx);
return 0;
}
Kinect::~Kinect() {
printf("Error: need to free buffers.\n");
uv_mutex_lock(&mutex);
must_stop = true;
uv_mutex_unlock(&mutex);
uv_thread_join(&thread);
if(ctx) {
freenect_close_device(device);
freenect_shutdown(ctx);
ctx = NULL;
}
uv_mutex_destroy(&mutex);
has_new_rgb = false;
has_new_depth = false;
if(depth_back) {
delete[] depth_back;
}
if(depth_mid) {
delete[] depth_mid;
}
if(depth_front) {
delete[] depth_front;
}
depth_back = NULL;
depth_mid = NULL;
depth_front = NULL;
if(rgb_back) {
delete[] rgb_back;
}
if(rgb_mid) {
delete[] rgb_mid;
}
if(rgb_front) {
delete[] rgb_front;
}
rgb_back = NULL;
rgb_mid = NULL;
rgb_front = NULL;
device = NULL;
}
//----------------------------------------------------------------------------------------------------------------------
void KinectInterface::shutDownKinect()
{
/// stop the processing thread
m_process->setInActive();
/// stop the video and depth callbacks
freenect_stop_depth(m_dev);
freenect_stop_video(m_dev);
// close down our devices
freenect_close_device(m_dev);
freenect_shutdown(m_ctx);
}
int KinectFreenect::stop() {
die = 1;
freenect_stop_depth(f_dev);
freenect_stop_video(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
pthread_join(fnkt_thread, NULL);
return 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;
}
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;
}
/* Function: mdlTerminate =====================================================
* Abstract:
* In this function, you should perform any actions that are necessary
* at the termination of a simulation. For example, if memory was
* allocated in mdlStart, this is the place to free it.
*/
static void mdlTerminate(SimStruct *S)
{
SS_SimMode mode= ssGetSimMode(S);
if(mode==0)
{
printf("Term\n");
kinRunning= 0;
freenect_stop_depth(kinDev);
freenect_stop_video(kinDev);
freenect_close_device(kinDev);
freenect_shutdown(kinCtx);
}
}
//---------------------------------------------------------------------------
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");
}
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);
}
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;
}
// 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;
}
/*
* Class: org_ensor_robots_sensors_kinect_Device
* Method: nativeClose
* Signature: (I)V
*/
JNIEXPORT void JNICALL Java_org_ensor_robots_sensors_kinect_Device_nativeClose
(JNIEnv *aJNIEnv, jobject aThisObject, jint aDeviceId)
{
pthread_mutex_lock(&mutex);
if (f_devices[aDeviceId].f_video_buffer) {
freenect_stop_video(f_devices[aDeviceId].f_dev);
free(f_devices[aDeviceId].f_video_buffer);
f_devices[aDeviceId].f_video_buffer = NULL;
}
if (f_devices[aDeviceId].f_depth_buffer) {
freenect_stop_depth(f_devices[aDeviceId].f_dev);
free(f_devices[aDeviceId].f_depth_buffer);
f_devices[aDeviceId].f_depth_buffer = NULL;
}
freenect_close_device(f_devices[aDeviceId].f_dev);
(*aJNIEnv)->DeleteGlobalRef(aJNIEnv, f_devices[aDeviceId].object);
f_devices[aDeviceId].object = NULL;
pthread_mutex_unlock(&mutex);
}
void ofxKinectContext::close(ofxKinect& kinect) {
// check if it's already closed
int id = -1;
std::map<int,ofxKinect*>::iterator iter;
for(iter = kinects.begin(); iter != kinects.end(); ++iter) {
if(iter->second == &kinect) {
id = iter->first;
break;
}
}
if(id == -1)
return;
// remove connected device and close
iter = kinects.find(id);
if(iter != kinects.end()) {
kinects.erase(iter);
freenect_close_device(kinect.kinectDevice);
}
}
void jit_freenect_grab_close(t_jit_freenect_grab *x, t_symbol *s, long argc, t_atom *argv)
{
postNesa("closing device: start...");//TODO:r
if(f_ctx)
{
postNesa("closing device:f_ctx is valid.");//TODO:r
if(!f_ctx->first){
//if (x->x_systhread){
postNesa("closing device:last device, stopping thread...");
jit_freenect_thread_stop(x);
}
postNesa("closing device freenect side...");//TODO:r
if(!x->device)
{
postNesa("closing device:device not open");//TODO:r
return;
}
else {
//freenect_stop_depth(x->device);
//freenect_stop_video(x->device);
freenect_set_led(x->device,LED_BLINK_GREEN);
freenect_close_device(x->device);
x->device = NULL;
open_device_count--;
}
}
else {
postNesa("closing device:f_ctx is null, nothing to close");//TODO:r
}
postNesa("closing device:done\n");//TODO:r
}
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 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;
}
~FreenectDevice() {
if(freenect_close_device(m_dev) != 0) throw std::runtime_error("Cannot shutdown Kinect");
}
void destroyCamera()
{
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
}
virtual ~FreenectDevice() {
if(freenect_close_device(m_dev) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion");
}
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;
}