void depth_callback(freenect_device *dev, void *pixels, uint32_t timestamp){
t_jit_freenect_grab *x;
x = freenect_get_user(dev);
if(!x)
{
error("Invalid max object supplied in depth_callback\n");// TODO:should print only in debug mode
return;
}
//post("depth_callback called\n");//TODO:r
if (x->is_open)
{
systhread_mutex_lock(x->backbuffer_mutex);
// pthread_mutex_lock(&x->cb_mutex);
//assert(x->depth_back == pixels);
// x->depth_back = x->depth_mid;
x->depth_back = x->depth_mid;
freenect_set_depth_buffer(dev, x->depth_back);
x->depth_mid = (uint16_t*)pixels;
x->got_depth++;
systhread_mutex_unlock(x->backbuffer_mutex);
}
//pthread_mutex_unlock(&x->cb_mutex);
}
/*
* Class: org_ensor_robots_sensors_kinect_Device
* Method: nativeStartDepth
* Signature: (I)V
*/
JNIEXPORT void JNICALL Java_org_ensor_robots_sensors_kinect_Device_nativeStartDepth
(JNIEnv *aJNIEnv, jobject aThisObject, jint aDeviceId)
{
pthread_mutex_lock(&mutex);
freenect_set_depth_mode(f_devices[aDeviceId].f_dev,
freenect_find_depth_mode(
FREENECT_RESOLUTION_MEDIUM,
FREENECT_DEPTH_11BIT
)
);
f_devices[aDeviceId].depthbuffersize = 640 * 480 * 2;
f_devices[aDeviceId].f_depth_buffer = malloc(f_devices[aDeviceId].depthbuffersize);
freenect_set_depth_buffer(
f_devices[aDeviceId].f_dev,
f_devices[aDeviceId].f_depth_buffer
);
f_devices[aDeviceId].depthbuffervalue =
(*aJNIEnv)->NewBooleanArray(aJNIEnv, f_devices[aDeviceId].depthbuffersize);
f_devices[aDeviceId].depthbuffervalue = (*aJNIEnv)->NewGlobalRef(aJNIEnv,
f_devices[aDeviceId].depthbuffervalue
);
freenect_set_depth_callback(f_devices[aDeviceId].f_dev, depth_cb);
pthread_mutex_unlock(&mutex);
freenect_start_depth(f_devices[aDeviceId].f_dev);
}
static int change_depth_format(sync_kinect_t *kinect, freenect_depth_format fmt)
{
freenect_stop_depth(kinect->dev);
free_buffer_ring(&kinect->depth);
if (alloc_buffer_ring_depth(fmt, &kinect->depth))
return -1;
freenect_set_depth_format(kinect->dev, fmt);
freenect_set_depth_buffer(kinect->dev, kinect->depth.bufs[2]);
freenect_start_depth(kinect->dev);
return 0;
}
static int change_depth_format(sync_kinect_t *kinect, freenect_depth_format fmt)
{
freenect_stop_depth(kinect->dev);
free_buffer_ring(&kinect->depth);
if (alloc_buffer_ring_depth(fmt, &kinect->depth))
return -1;
freenect_set_depth_mode(kinect->dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, fmt));
freenect_set_depth_buffer(kinect->dev, kinect->depth.bufs[2]);
freenect_start_depth(kinect->dev);
return 0;
}
//---------------------------------------------------------------------------
void ofxKinect::grabDepthFrame(freenect_device *dev, void *depth, uint32_t timestamp) {
ofxKinect* kinect = kinectContext.getKinect(dev);
if(kinect->kinectDevice == dev) {
kinect->lock();
swap(kinect->depthPixelsRawBack,kinect->depthPixelsRawIntra);
kinect->bNeedsUpdateDepth = true;
kinect->unlock();
freenect_set_depth_buffer(kinect->kinectDevice,kinect->depthPixelsRawBack.getPixels());
}
}
//--------------------------------------------------------------------
bool ofxKinect::open(int deviceIndex) {
if(!bGrabberInited) {
ofLogWarning("ofxKinect") << "open(): cannot open, init not called";
return false;
}
//we need to find the device id from the device list index.
//as the first device id could be 1 and not 0 when the device list is sorted by serial
int deviceIDFromIndex = -1;
if( deviceIndex > -1 ){
deviceIDFromIndex = kinectContext.getDeviceId(deviceIndex);
}
if(!kinectContext.open(*this, deviceIDFromIndex)) {
return false;
}
if(serial == "0000000000000000") {
bHasMotorControl = false;
//if we do motor control via the audio device ( ie: 1473 or k4w ) and we have firmware uploaded
//then we can do motor stuff! :)
if( kinectDevice->motor_control_with_audio_enabled ){
bHasMotorControl = true;
}else{
ofLogVerbose("ofxKinect") << "open(): device " << deviceId << " does not have motor control";
}
}
else {
bHasMotorControl = true;
}
lastDeviceIndex = deviceIndex;
timeSinceOpen = ofGetElapsedTimef();
bGotDataVideo = false;
bGotDataDepth = false;
bFirstUpdate = true;
freenect_set_user(kinectDevice, this);
freenect_set_depth_buffer(kinectDevice, depthPixelsRawBack.getData());
freenect_set_video_buffer(kinectDevice, videoPixelsBack.getData());
freenect_set_depth_callback(kinectDevice, &grabDepthFrame);
freenect_set_video_callback(kinectDevice, &grabVideoFrame);
startThread(); // blocking, not verbose
return true;
}
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_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_set_depth_buffer(f_dev, depth_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) {
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 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);
}
//--------------------------------------------------------------------
bool ofxKinect::open(int id) {
if(!bGrabberInited) {
ofLogWarning("ofxKinect") << "open(): cannot open, init not called";
return false;
}
if(!kinectContext.open(*this, id)) {
return false;
}
if(serial == "0000000000000000") {
bHasMotorControl = false;
//if we do motor control via the audio device ( ie: 1473 or k4w ) and we have firmware uploaded
//then we can do motor stuff! :)
if( kinectDevice->motor_control_with_audio_enabled ){
bHasMotorControl = true;
}else{
ofLogVerbose("ofxKinect") << "open(): device " << deviceId << " does not have motor control";
}
}
else {
bHasMotorControl = true;
}
lastDeviceId = deviceId;
timeSinceOpen = ofGetElapsedTimef();
bGotData = false;
freenect_set_user(kinectDevice, this);
freenect_set_depth_buffer(kinectDevice, depthPixelsRawBack.getPixels());
freenect_set_video_buffer(kinectDevice, videoPixelsBack.getPixels());
freenect_set_depth_callback(kinectDevice, &grabDepthFrame);
freenect_set_video_callback(kinectDevice, &grabVideoFrame);
startThread(true, false); // blocking, not verbose
return true;
}
void depth_cb(freenect_device *dev, void *v_depth, uint32_t timestamp)
{
bool local_acquire_flag = 0;
pthread_mutex_lock(&acquire_mutex);
local_acquire_flag = !acquired_depth;
pthread_mutex_unlock(&acquire_mutex);
pthread_mutex_lock(&gl_backbuf_mutex);
assert(depth_back == v_depth);
depth_back = depth_mid;
freenect_set_depth_buffer(dev, depth_back);
if (local_acquire_flag){
saveDepthFrame(depth_back);
}
depth_mid = v_depth;
got_depth++;
pthread_cond_signal(&gl_frame_cond);
pthread_mutex_unlock(&gl_backbuf_mutex);
}
void kinect_depth_callback(freenect_device* dev, void* depth, uint32_t timestamp) {
Kinect* kinect = static_cast<Kinect*>(freenect_get_user(dev));
if(!kinect) {
printf("Error: cannot get the Kinect* user ptr.\n");
::exit(EXIT_FAILURE);
}
if(depth != kinect->depth_back) {
printf("Error: wrong depth pointer!\n");
}
kinect->depth_back = kinect->depth_mid;
freenect_set_depth_buffer(dev, kinect->depth_back);
kinect->depth_mid = (uint8_t*)depth;
uv_mutex_lock(&kinect->mutex);
{
memcpy(kinect->depth_front, kinect->depth_mid, kinect->nbytes_rgb);
kinect->has_new_depth = true;
}
uv_mutex_unlock(&kinect->mutex);
}
int KntInit(kntCtx_t * const ctx, int devNum) {
if (freenect_init(&ctx->fn, NULL))
return 0;
if (freenect_open_device(ctx->fn, &ctx->dev, devNum))
return 0;
freenect_set_depth_format(ctx->dev, FREENECT_DEPTH_11BIT);
ctx->dbb = malloc(640*480*2);
ctx->dfb = malloc(640*480*2);
ctx->depthBuf = malloc(640*480*4);
ctx->ibb = malloc(640*480*3);
ctx->imageBuf = malloc(640*480*3);
freenect_set_depth_buffer(ctx->dev, ctx->dbb);
freenect_set_video_buffer(ctx->dev, ctx->ibb);
freenect_set_depth_callback(ctx->dev, KntDepthHandler);
freenect_set_video_callback(ctx->dev, KntImageHandler);
if (!contexts) {
contexts = (ctxList_t *)malloc(sizeof(*contexts));
contexts->next = NULL;
contexts->ctx = *ctx;
} else {
ctxList_t * ptr = contexts;
while (ptr->next)
ptr = ptr->next;
ptr->next = (ctxList_t *)malloc(sizeof(*contexts));
ptr = ptr->next;
ptr->ctx = *ctx;
}
}
