bool initCamera()
{
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return false;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
freenect_select_subdevices(f_ctx, (freenect_device_flags)(FREENECT_DEVICE_CAMERA));
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (nr_devices < 1)
return false;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return false;
}
int accelCount = 0;
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_user(f_dev, (void*)this);
}
int main( int argc, char **argv )
{
//depth_gl = (uint8_t*)malloc(640*480*3);
if( freenect_init( &f_context, NULL ) < 0 )
{
printf( "Freenect initialization failed.\n" );
return 1;
}
freenect_set_log_level( f_context, FREENECT_LOG_DEBUG );
freenect_select_subdevices( f_context, ( freenect_device_flags )( FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA ) );
if( freenect_num_devices ( f_context ) < 1 )
{
printf( "No devices found. Shutting down...\n" );
freenect_shutdown( f_context );
return 1;
}
if( freenect_open_device( f_context, &f_device, 0 ) <0 )
{
printf( "Couldn't open device. Shutting down...\n" );
freenect_shutdown( f_context );
return 1;
}
if( pthread_create( &f_thread, NULL, f_threadfunc, NULL ) )
{
printf( "Thread creation failed. Shutting down...\n" );
freenect_shutdown( f_context );
return 1;
}
glutInit( &argc, argv );
gl_threadfunc( NULL );
return 0;
}
int main(int argc, char **argv)
{
int res;
depth_mid = (uint8_t*)malloc(640*480*3);
depth_front = (uint8_t*)malloc(640*480*3);
rgb_back = (uint8_t*)malloc(640*480*3);
rgb_mid = (uint8_t*)malloc(640*480*3);
rgb_front = (uint8_t*)malloc(640*480*3);
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);
freenect_select_subdevices(f_ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
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) {
freenect_shutdown(f_ctx);
return 1;
}
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
freenect_shutdown(f_ctx);
return 1;
}
res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
freenect_shutdown(f_ctx);
return 1;
}
// OS X requires GLUT to run on the main thread
gl_threadfunc(NULL);
return 0;
}
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);
}
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);
}
Freenect() : m_stop(false) {
if(freenect_init(&m_ctx, NULL) < 0) throw std::runtime_error("Cannot initialize freenect library");
// We claim both the motor and camera devices, since this class exposes both.
// It does not support audio, so we do not claim it.
freenect_select_subdevices(m_ctx, static_cast<freenect_device_flags>(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
if(pthread_create(&m_thread, NULL, pthread_callback, (void*)this) != 0) throw std::runtime_error("Cannot initialize freenect thread");
}
//--------------------------------------------------------------------
bool ofxKinect::open() {
if(!bGrabberInited) {
ofLog(OF_LOG_WARNING, "ofxKinect: Cannot open, init not called");
return false;
}
if (freenect_init(&kinectContext, NULL) < 0) {
ofLog(OF_LOG_ERROR, "ofxKinect: freenet_init failed");
return false;
}
int number_devices = freenect_num_devices(kinectContext);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Number of Devices found: " + ofToString(number_devices));
if (number_devices < 1) {
ofLog(OF_LOG_ERROR, "ofxKinect: Did not find a device");
return false;
}
if (freenect_open_device(kinectContext, &kinectDevice, 0) < 0) {
ofLog(OF_LOG_ERROR, "ofxKinect: Could not open device");
return false;
}
freenect_set_user(kinectDevice, this);
startThread(true, false); // blocking, not verbose
return true;
}
bool FreenectGrabber :: connectToDevice()
{
if (m_connected)
return true;
if (freenect_init(&f_ctx, NULL) < 0)
{
ntk_dbg(0) << "freenect_init() failed";
return false;
}
ntk_dbg(0) << "Connecting to device: " << m_device_id;
if (freenect_open_device(f_ctx, &f_dev, m_device_id) < 0)
{
ntk_dbg(0) << "freenect_open_device() failed";
return false;
}
freenect_set_user(f_dev, this);
freenect_set_depth_callback(f_dev, kinect_depth_db);
freenect_set_video_callback(f_dev, kinect_video_db);
this->setIRMode(m_ir_mode);
m_connected = true;
return true;
}
KinectControl::KinectControl() {
printf("\nKinectControl Init\n");
errorCode = 0;
isInited = false;
if (kinectControlRef != NULL) {
errorCode = 1;
errorString = "KinectControl Already Started.";
printf("%s.\n",errorString.c_str());
return;
}
for (int i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
kinectControlRef = this;
freenect_angle = 0;
user_device_number = -1;
requested_format = FREENECT_VIDEO_RGB;
current_format = FREENECT_VIDEO_RGB;
//depth_mid = (uint8_t*)malloc(640*480*3);
depth_front = (uint8_t*)malloc(640*480*3);
rgb_back = (uint8_t*)malloc(640*480*3);
rgb_mid = (uint8_t*)malloc(640*480*3);
rgb_front = (uint8_t*)malloc(640*480*3);
depth_mid = NULL;
memset(depth_front,0,(640*480*3));
memset(rgb_back,0,(640*480*3));
memset(rgb_mid,0,(640*480*3));
memset(rgb_front,0,(640*480*3));
if (freenect_init(&f_ctx, NULL) < 0) {
errorCode = 2;
errorString = "freenect_init() failed.";
printf("%s\n",errorString.c_str());
return;
} else {
printf("freenect_init() OK\n");
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
freenect_select_subdevices(f_ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
// check if there were no device found.
if (nr_devices < 0) {
freenect_shutdown(f_ctx);
errorCode = 3;
errorString = "No freenect devices found!";
printf("%s\n", errorString.c_str());
}
}
jboolean openSync(JNIEnv* env) {
__android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, "Start Sync");
sendMsgToJava(NULL, "Starting Sync...");
int res;
depth_mid = (uint8_t*) malloc(640 * 480 * 4);
depth_front = (uint8_t*) malloc(640 * 480 * 3);
rgb_back = (uint8_t*) malloc(640 * 480 * 3);
rgb_mid = (uint8_t*) malloc(640 * 480 * 3);
rgb_front = (uint8_t*) malloc(640 * 480 * 3);
int i;
for (i = 0; i < 2048; i++) {
float v = i / 2048.0;
v = powf(v, 3) * 6;
t_gamma[i] = v * 6 * 256;
}
//init kinect context
if (freenect_init(&f_ctx, NULL) < 0) {
__android_log_print(ANDROID_LOG_ERROR, LOG_TAG,
"Kinect open sync failed\n");
sendMsgToJava(env, "Kinect open sync failed");
return FALSE;
}
//settup
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
freenect_select_subdevices(
f_ctx,
(freenect_device_flags) (FREENECT_DEVICE_MOTOR
| FREENECT_DEVICE_CAMERA));
//get num dev
int nr_devices = freenect_num_devices(f_ctx);
sprintf(my_log, "Number of Devices found %d\n", nr_devices);
sendMsgToJava(env, my_log);
__android_log_print(ANDROID_LOG_INFO, LOG_TAG, my_log);
int user_device_number = 0;
if (nr_devices < 1)
return FALSE;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, "Cannot Open device\n");
sendMsgToJava(env, "Cannot Open device");
return FALSE;
}
//create thread
res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, env);
if (res) {
__android_log_print(ANDROID_LOG_ERROR, LOG_TAG,
"pthread_create failed...\n");
sendMsgToJava(env, "pthread_create failed...");
return FALSE;
}
return TRUE;
}
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;
}
bool Kinect::setup() {
if(ctx) {
printf("Error: the freenect context has been setup already.\n");
return false;
}
if(freenect_init(&ctx, NULL) < 0) {
printf("Error: cannot init libfreenect.\n");
return false;
}
freenect_set_log_level(ctx, FREENECT_LOG_DEBUG);
int ndevices = freenect_num_devices(ctx);
if(ndevices < 1) {
printf("Error: cannot find a kinect. @todo cleanup mem.\n");
freenect_shutdown(ctx);
ctx = NULL;
return false;
}
printf("Number of found kinect devices: %d\n", ndevices);
freenect_select_subdevices(ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
//freenect_select_subdevices(ctx, (freenect_device_flags)( FREENECT_DEVICE_CAMERA));
int devnum = 0;
if(freenect_open_device(ctx, &device, devnum) < 0) {
printf("Error: cannot open device: %d\n", devnum);
freenect_shutdown(ctx);
ctx = NULL;
return false;
}
freenect_set_user(device, this);
uint32_t w = 640;
uint32_t h = 480;
uint32_t nbytes = w * h * 3;
nbytes_rgb = nbytes;
rgb_back = new uint8_t[nbytes];
rgb_mid = new uint8_t[nbytes];
rgb_front = new uint8_t[nbytes];
depth_back = new uint8_t[nbytes];
depth_mid = new uint8_t[nbytes];
depth_front = new uint8_t[nbytes];
uv_mutex_lock(&mutex);
must_stop = false;
uv_mutex_unlock(&mutex);
uv_thread_create(&thread, kinect_thread, this);
return true;
}
static void init_thread(void)
{
thread_running = 1;
freenect_init(&ctx, 0);
// We claim both the motor and the camera, because we can't know in advance
// which devices the caller will want, and the c_sync interface doesn't
// support audio, so there's no reason to claim the device needlessly.
freenect_select_subdevices(ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
pthread_create(&thread, NULL, init, NULL);
}
JNIEXPORT jlong JNICALL Java_org_openkinect_Context_jniInit
(JNIEnv *, jobject)
{
freenect_context *f_ctx = 0;
if (freenect_init(&f_ctx, NULL) < 0)
{
return 0;
}
freenect_set_log_callback(f_ctx, log_cb);
return (jlong) f_ctx;
}
int main(int argc, char **argv)
{
int res;
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;
}
res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
// OS X requires GLUT to run on the main thread
gl_threadfunc(NULL);
return 0;
}
int main(int argc, char** argv) {
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_INFO);
freenect_select_subdevices(f_ctx, FREENECT_DEVICE_AUDIO);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
if (nr_devices < 1) {
freenect_shutdown(f_ctx);
return 1;
}
int user_device_number = 0;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
freenect_shutdown(f_ctx);
return 1;
}
state.max_samples = 256 * 60;
state.current_idx = 0;
state.buffers[0] = (int32_t*)malloc(state.max_samples * sizeof(int32_t));
state.buffers[1] = (int32_t*)malloc(state.max_samples * sizeof(int32_t));
state.buffers[2] = (int32_t*)malloc(state.max_samples * sizeof(int32_t));
state.buffers[3] = (int32_t*)malloc(state.max_samples * sizeof(int32_t));
memset(state.buffers[0], 0, state.max_samples * sizeof(int32_t));
memset(state.buffers[1], 0, state.max_samples * sizeof(int32_t));
memset(state.buffers[2], 0, state.max_samples * sizeof(int32_t));
memset(state.buffers[3], 0, state.max_samples * sizeof(int32_t));
freenect_set_user(f_dev, &state);
freenect_set_audio_in_callback(f_dev, in_callback);
freenect_start_audio(f_dev);
int res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
freenect_shutdown(f_ctx);
return 1;
}
ros::init(argc, argv, "kinect_audio");
MicView mv;
mv.spin();
return 0;
}
//--------------------------------------------------------------------
bool ofxKinect::init(bool infrared, bool video, bool texture){
if(isConnected()){
ofLog(OF_LOG_WARNING, "ofxKinect: Do not call init while ofxKinect is running!");
return false;
}
clear();
bInfrared = infrared;
bGrabVideo = video;
bytespp = infrared?1:3;
calibration.init(bytespp);
bUseTexture = texture;
int length = width*height;
depthPixelsRaw = new unsigned short[length];
depthPixelsBack = new unsigned short[length];
videoPixels = new unsigned char[length*bytespp];
// pixels.setFromExternalPixels(videoPixels, width, height, OF_IMAGE_COLOR);
videoPixelsBack = new unsigned char[length*bytespp];
memset(depthPixelsRaw, 0, length*sizeof(unsigned short));
memset(depthPixelsBack, 0, length*sizeof(unsigned short));
memset(videoPixels, 0, length*bytespp*sizeof(unsigned char));
memset(videoPixelsBack, 0, length*bytespp*sizeof(unsigned char));
if(bUseTexture){
depthTex.allocate(width, height, GL_LUMINANCE);
videoTex.allocate(width, height, infrared?GL_LUMINANCE:GL_RGB);
}
if (freenect_init(&kinectContext, NULL) < 0){
ofLog(OF_LOG_ERROR, "ofxKinect: freenet_init failed");
return false;
}
ofLog(OF_LOG_VERBOSE, "ofxKinect: Inited");
int number_devices = freenect_num_devices(kinectContext);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Number of Devices found: " + ofToString(number_devices));
bGrabberInited = true;
return bGrabberInited;
}
//---------------------------------------------------------------------------
bool ofxKinectContext::init() {
if(freenect_init(&kinectContext, NULL) < 0) {
ofLog(OF_LOG_ERROR, "ofxKinect: freenect_init failed");
bInited = false;
return false;
}
bInited = true;
ofLog(OF_LOG_VERBOSE, "ofxKinect: Context inited");
buildDeviceList();
listDevices(true);
return true;
}
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;
}
/* Function: mdlStart =======================================================
* Abstract:
* This function is called once at start of model execution. If you
* have states that should be initialized once, this is the place
* to do it.
*/
static void mdlStart(SimStruct *S)
{
SS_SimMode mode= ssGetSimMode(S);
if(mode==0)
{
kinFail=0;
kinRunning=1;
newFrame=0;
//change
memset (kinR,'1',307200);
memset (kinG,'1',307200);
memset (kinB,'1',307200);
memset (kinD,'1',307200);
/*-----INIT---------------*/
if (freenect_init(&kinCtx, 0)) {
printf("Error: Cannot get context\n");
kinFail=1;
return;
}
if (freenect_open_device(kinCtx, &kinDev, 0)) {
printf("Error: Cannot get device\n");
kinFail=1;
return;
}
freenect_set_depth_format(kinDev, FREENECT_DEPTH_11BIT);
freenect_start_depth(kinDev);
//change
freenect_set_video_format(kinDev, FREENECT_VIDEO_YUV_RGB);
freenect_start_video(kinDev);
freenect_set_depth_callback(kinDev, depth_cb);
freenect_set_video_callback(kinDev, rgb_cb);
/*-----END INIT------------*/
pthread_create( &kinThread, NULL, runLoop, NULL);
}
}
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;
}
/*
* 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 );
}
freenect_context* init_kinect() {
freenect_context *context;
if (freenect_init(&context, NULL) < 0) {
printf("failed to initialize kinect\n");
return NULL;
} else {
printf("initialized kinect\n");
if (getenv("DEBUG")) {
freenect_set_log_level(context, FREENECT_LOG_DEBUG);
}
freenect_select_subdevices(context, (freenect_device_flags)FREENECT_DEVICE_CAMERA);
return context;
}
}
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;
}
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_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;
}
int KinectFreenect::start() {
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
int nr_devices = freenect_num_devices(f_ctx);
int user_device_number = 0;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device.\n");
return 1;
}
freenect_set_led(f_dev, LED_RED);
freenect_set_depth_callback(f_dev, KinectFreenect::depth_cb);
freenect_set_video_callback(f_dev, KinectFreenect::rgb_cb);
freenect_set_video_mode(f_dev, freenect_find_video_mode(current_resolution, current_format));
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
int res;
res = pthread_create(&fnkt_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
int status = 0;
while (!die && status >= 0) {
char k = cvWaitKey(5);
if( k == 27 ) {
die = 1;
break;
}
}
return 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;
}
//---------------------------------------------------------------------------
void ofxKinect::threadedFunction(){
if (freenect_init(&kinectContext, NULL) < 0) {
ofLog(OF_LOG_ERROR, "ofxKinect: freenet_init failed");
}
int number_devices = freenect_num_devices(kinectContext);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Number of Devices found: " + ofToString(number_devices));
if (number_devices < 1) {
ofLog(OF_LOG_ERROR, "ofxKinect: didnt find a device");
return;
}
if (freenect_open_device(kinectContext, &kinectDevice, 0) < 0) {
ofLog(OF_LOG_ERROR, "ofxKinect: could not open device");
return;
}
freenect_set_led(kinectDevice, LED_GREEN);
freenect_set_depth_callback(kinectDevice, &grabDepthFrame);
freenect_set_rgb_callback(kinectDevice, &grabRgbFrame);
freenect_set_rgb_format(kinectDevice, FREENECT_FORMAT_RGB);
freenect_set_depth_format(kinectDevice, FREENECT_FORMAT_11_BIT);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection opened");
freenect_start_depth(kinectDevice);
freenect_start_rgb(kinectDevice);
while (isThreadRunning()) {
int16_t ax,ay,az;
freenect_get_raw_accelerometers(kinectDevice, &ax, &ay, &az);
rawAccel.set(ax, ay, az);
double dx,dy,dz;
freenect_get_mks_accelerometers(kinectDevice, &dx, &dy, &dz);
mksAccel.set(dx, dy, dz);
// printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", ax, ay, az, dx, dy, dz);
}
}
int audio_setup(){
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return -1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_INFO);
freenect_select_subdevices(f_ctx, FREENECT_DEVICE_AUDIO);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
if (nr_devices < 1)
return -1;
int user_device_number = 0;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return 1;
}
freenect_set_audio_in_callback(f_dev, in_callback);
freenect_start_audio(f_dev);
return 0;
}
