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;
}
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);
}
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");
}
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;
}
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());
}
}
Driver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices)
{
WriteMessage("Using libfreenect v" + to_string(PROJECT_VER));
freenect_set_log_level(m_ctx, FREENECT_LOG_DEBUG);
freenect_select_subdevices(m_ctx, FREENECT_DEVICE_CAMERA); // OpenNI2 doesn't use MOTOR or AUDIO
DriverServices = &getServices();
}
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);
}
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;
}
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)
{
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 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;
}
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);
}
/*
* Class: org_ensor_robots_sensors_kinect_Driver
* Method: nativeInit
* Signature: ()V
*/
JNIEXPORT jint JNICALL Java_org_ensor_robots_sensors_kinect_Driver_nativeInit
(JNIEnv *aJNIEnv, jobject aThisObject)
{
f_running = 0;
(*aJNIEnv)->GetJavaVM(aJNIEnv, &jvm);
int rc = freenect_init(&f_ctx, NULL);
if (rc == 0) {
f_device_count = freenect_num_devices(f_ctx);
if (f_device_count > 0) {
f_devices = malloc(sizeof(jfreenect_device_t)*f_device_count);
int i;
for (i = 0; i < f_device_count; i++) {
f_devices[i].f_dev = NULL;
f_devices[i].f_depth_buffer = NULL;
f_devices[i].f_video_buffer = NULL;
f_devices[i].object = NULL;
}
f_running = 1;
pthread_attr_t attr;
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
int res = pthread_create(&f_thread, &attr, freenect_threadfunc, NULL);
pthread_attr_destroy(&attr);
}
else {
f_devices = NULL;
}
}
freenect_select_subdevices(f_ctx,
(freenect_device_flags)(
FREENECT_DEVICE_MOTOR |
FREENECT_DEVICE_CAMERA));
}
//---------------------------------------------------------------------------
bool ofxKinectContext::init() {
if(freenect_init(&kinectContext, NULL) < 0) {
ofLogError("ofxKinect") << "init(): freenect_init failed";
bInited = false;
return false;
}
#ifdef OFX_KINECT_EXTRA_FW
freenect_set_fw_address_nui(kinectContext, ofxKinectExtras::getFWData1473(), ofxKinectExtras::getFWSize1473());
freenect_set_fw_address_k4w(kinectContext, ofxKinectExtras::getFWDatak4w(), ofxKinectExtras::getFWSizek4w());
#endif
freenect_set_log_level(kinectContext, FREENECT_LOG_WARNING);
freenect_select_subdevices(kinectContext, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
bInited = true;
ofLogVerbose("ofxKinect") << "context inited";
buildDeviceList();
listDevices(true);
return true;
}
int main(int argc, char **argv){
std::string topic;
std::string frame_id;
int device_num;
int depth_mode;
int rgb_mode;
int _registration;
int _frame_skip;
printf("starting\n");
fflush(stdout);
ros::init(argc, argv, "thin_kinect_node",ros::init_options::AnonymousName);
ros::NodeHandle n("~");
//Base topic name
n.param("topic", topic, std::string("/camera"));
//Resolution
//0 = 160x120
//1 = 320x240
n.param("depth_mode", depth_mode, -1);
n.param("rgb_mode", rgb_mode, -1);
n.param("registration", _registration,0);
n.param("frame_id", frame_id, std::string("camera_frame"));
n.param("device_num", device_num, -1);
n.param("frame_skip", _frame_skip, 0);
printf("Launched with params:\n");
printf("_topic:= %s\n",topic.c_str());
printf("_frame_id:= %s\n",frame_id.c_str());
printf("_device_num:= %d\n", device_num);
printf("_depth_mode:= %d\n",depth_mode);
printf("_rgb_mode:= %d\n",rgb_mode);
printf("_frame_skip:= %d\n", _frame_skip);
printf("_registration:= %d\n",_registration);
fflush(stdout);
if (_frame_skip<=0)
_frame_skip=1;
//freenect STUFF
freenect_context* context;
int retval = freenect_init(&context,NULL);
if (retval<0) {
printf("error in initializing freenect %d \n", retval);
return 0;
}
// select only the camera
freenect_select_subdevices(context, FREENECT_DEVICE_CAMERA);
//enumerate the devices
int num_devices = freenect_num_devices(context);
if (num_devices<=0) {
printf("no devices found\n");
return 0;
} else {
printf("found %d devices\n", num_devices);
}
// obtain the list of attributes for each device
struct freenect_device_attributes* attribute_list;
retval = freenect_list_device_attributes(context, &attribute_list);
if (retval<0) {
printf("error getting attribute list %d \n", retval);
return 0;
}
struct freenect_device_attributes* aux = attribute_list;
int k = 0;
while (aux) {
printf("device: %02d, serial %s\n", k, aux->camera_serial);
aux = aux->next;
k++;
}
int num_video_modes = freenect_get_video_mode_count();
printf ("found %d video modes\n", num_video_modes);
int i;
for (i = 0; i< num_video_modes; i++){
const char* fmt = 0;
freenect_frame_mode mode = freenect_get_video_mode(i);
switch(mode.video_format) {
case FREENECT_VIDEO_RGB: fmt = "FREENECT_VIDEO_RGB"; break;
case FREENECT_VIDEO_BAYER: fmt = "FREENECT_VIDEO_BAYER"; break;
case FREENECT_VIDEO_IR_8BIT: fmt = "FREENECT_VIDEO_IR_8BIT"; break;
case FREENECT_VIDEO_IR_10BIT: fmt = "FREENECT_VIDEO_IR_10BIT"; break;
case FREENECT_VIDEO_IR_10BIT_PACKED: fmt = "FREENECT_VIDEO_IR_10BIT_PACKED"; break;
case FREENECT_VIDEO_YUV_RGB: fmt = "FREENECT_VIDEO_YUV_RGB"; break;
case FREENECT_VIDEO_YUV_RAW: fmt = "FREENECT_VIDEO_YUV_RAW"; break;
}
if (device_num<0)
printf("mode: %02d, format: %s, width: %d, height: %d, bpp: %d, padding: %d, framerate: %d, valid: %d, bytes: %d\n",
i,
fmt, mode.width,
mode.height,
mode.data_bits_per_pixel,
mode.padding_bits_per_pixel,
mode.framerate,
mode.is_valid,
mode.bytes);
}
int num_depth_modes = freenect_get_depth_mode_count();
printf ("found %d depth modes\n", num_depth_modes);
for (i = 0; i< num_depth_modes; i++){
const char* fmt = 0;
freenect_frame_mode mode = freenect_get_depth_mode(i);
switch(mode.depth_format) {
case FREENECT_DEPTH_11BIT: fmt = "FREENECT_DEPTH_11BIT"; break;
case FREENECT_DEPTH_10BIT: fmt = "FREENECT_DEPTH_10BIT"; break;
case FREENECT_DEPTH_11BIT_PACKED: fmt = "FREENECT_DEPTH_11BIT_PACKED"; break;
case FREENECT_DEPTH_10BIT_PACKED: fmt = "FREENECT_DEPTH_10BIT_PACKED"; break;
case FREENECT_DEPTH_REGISTERED: fmt = "FREENECT_DEPTH_REGISTERED"; break;
case FREENECT_DEPTH_MM: fmt = "FREENECT_DEPTH_MM"; break;
}
if (device_num<0)
printf("mode: %02d, format: %s, width: %d, height: %d, bpp: %d, padding: %d, framerate: %d, valid: %d, bytes: %d\n",
i,
fmt, mode.width,
mode.height,
mode.data_bits_per_pixel,
mode.padding_bits_per_pixel,
mode.framerate,
mode.is_valid,
mode.bytes);
}
if (device_num>k || device_num<0 ){
printf("device number %d does not exist, aborting \n", k);
return 0;
}
if ((rgb_mode >num_video_modes || rgb_mode<0) &&
(depth_mode >num_depth_modes || depth_mode<0))
return 0;
char* video_buffer, *depth_buffer;
static freenect_thread_params params;
if (rgb_mode >num_video_modes || rgb_mode<0)
params.video_mode.video_format = FREENECT_VIDEO_DUMMY;
else {
params.video_mode = freenect_get_video_mode(rgb_mode);
params.pub_rgb = n.advertise<sensor_msgs::Image>("/"+topic+"/rgb/image_raw", 1);
params.pub_camera_info_rgb = n.advertise<sensor_msgs::CameraInfo>("/"+topic+"/rgb/camera_info", 1);
}
if (depth_mode >num_depth_modes || depth_mode<0)
params.depth_mode.depth_format = FREENECT_DEPTH_DUMMY;
else {
params.depth_mode = freenect_get_depth_mode(depth_mode);
params.pub_depth = n.advertise<sensor_msgs::Image>("/"+topic+"/depth/image_raw", 1);
params.pub_camera_info_depth = n.advertise<sensor_msgs::CameraInfo>("/"+topic+"/depth/camera_info", 1);
}
if (_registration) {
printf("Warning, registration enable, overriding user selection of depth mode");
params.depth_mode.depth_format = FREENECT_DEPTH_REGISTERED;
}
params.run = 1;
params.device_num = device_num;
params.frame_skip = _frame_skip;
params.frame_id = frame_id;
pthread_t runner;
pthread_create(&runner, 0, freenect_thread, (void*) ¶ms);
ros::spin();
params.run = 0;
void* result;
pthread_join(runner, &result);
printf("shutting down\n");
return 0;
}
int kinect_init(Kinect* kt){
pthread_mutex_init(&kt->mutex, NULL);
kt->depth_mid = (uint8_t*)malloc(640*480*3);
kt->depth_front = (uint8_t*)malloc(640*480*3);
kt->rgb_back = (uint8_t*)malloc(640*480*3);
kt->rgb_mid = (uint8_t*)malloc(640*480*3);
kt->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;
kt->t_gamma[i] = v*6*256;
}
if (freenect_init(&kt->ctx, NULL) < 0) {
fprintf(stderr, "freenect_init() failed\n");
return 1;
}
freenect_set_log_level(kt->ctx, FREENECT_LOG_DEBUG);
freenect_select_subdevices(kt->ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
int nr_devices = freenect_num_devices (kt->ctx);
fprintf(stderr, "Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (nr_devices < 1) {
fprintf(stderr, "No devices detected\n");
freenect_shutdown(kt->ctx);
return 1;
}
if (freenect_open_device(kt->ctx, &kt->dev, user_device_number) < 0) {
fprintf(stderr, "Could not open device\n");
freenect_shutdown(kt->ctx);
return 1;
}
int freenect_angle = 0;
freenect_set_tilt_degs(kt->dev,freenect_angle);
freenect_set_led(kt->dev,LED_RED);
freenect_set_depth_callback(kt->dev, depth_cb);
freenect_set_video_callback(kt->dev, rgb_cb);
freenect_set_video_mode(kt->dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_depth_mode(kt->dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_video_buffer(kt->dev, kt->rgb_back);
freenect_start_depth(kt->dev);
freenect_start_video(kt->dev);
return 0;
}
Freenect() : m_stop(false) {
if(freenect_init(&m_ctx, NULL) < 0) throw std::runtime_error("Cannot initialize freenect library");
freenect_select_subdevices(m_ctx, FREENECT_DEVICE_CAMERA);
if(pthread_create(&m_thread, NULL, pthread_callback, (void*)this) != 0) throw std::runtime_error("Cannot initialize freenect thread");
}
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_SPEW);
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;
}
capture state;
state.logfiles[0] = fopen("channel1.wav", "wb");
state.logfiles[1] = fopen("channel2.wav", "wb");
state.logfiles[2] = fopen("channel3.wav", "wb");
state.logfiles[3] = fopen("channel4.wav", "wb");
fwrite(wavheader, 1, 44, state.logfiles[0]);
fwrite(wavheader, 1, 44, state.logfiles[1]);
fwrite(wavheader, 1, 44, state.logfiles[2]);
fwrite(wavheader, 1, 44, state.logfiles[3]);
freenect_set_user(f_dev, &state);
freenect_set_audio_in_callback(f_dev, in_callback);
freenect_start_audio(f_dev);
signal(SIGINT, cleanup);
while(!die && freenect_process_events(f_ctx) >= 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.
}
// Make the WAV header valid for each of the four files
int i;
for(i = 0; i < 4 ; i++) {
char buf[4];
fseek(state.logfiles[i], 4, SEEK_SET);
// Write ChunkSize = 36 + subchunk2size
int chunksize = state.samples * 4 + 36;
buf[0] = (chunksize & 0x000000ff);
buf[1] = (chunksize & 0x0000ff00) >> 8;
buf[2] = (chunksize & 0x00ff0000) >> 16;
buf[3] = (chunksize & 0xff000000) >> 24;
fwrite(buf, 1, 4,state.logfiles[i]);
fseek(state.logfiles[i], 40, SEEK_SET);
// Write Subchunk2Size = NumSamples * NumChannels (1) * BitsPerSample/8 (4)
int subchunk2size = state.samples * 4;
buf[0] = (subchunk2size & 0x000000ff);
buf[1] = (subchunk2size & 0x0000ff00) >> 8;
buf[2] = (subchunk2size & 0x00ff0000) >> 16;
buf[3] = (subchunk2size & 0xff000000) >> 24;
fwrite(buf, 1, 4,state.logfiles[i]);
fclose(state.logfiles[i]);
}
return 0;
}
FreenectDriver() {
freenect_init(&driver_, NULL);
freenect_set_log_level(driver_, FREENECT_LOG_FATAL); // Prevent's printing stuff to the screen
freenect_select_subdevices(driver_, (freenect_device_flags)(FREENECT_DEVICE_CAMERA));
thread_running_ = false;
}
//-----SLOTS---------------------------------------------
void Server::startKinect()//hay que simplificar con funciones de la API_FU
{
die = 0;//achtung!!! usas die en freenect_usal.h y aquí en while????
emit sendMessage(QLatin1String("activamos Kinect"));
//-------------iniciamos freenect
rgb_back = (uint8_t*)malloc(W*H*3);//buffers
rgb_mid = (uint8_t*)malloc(W*H*3);
rgb_front = (uint8_t*)malloc(W*H*3);
if (freenect_init(&context, NULL) < 0) {// init freenect context
emit sendMessage(QLatin1String("error freenect_init()"));
return;
}
emit sendMessage(QLatin1String("freenect_init() started"));
//freenect_set_log_level(context, FREENECT_LOG_DEBUG);
freenect_select_subdevices(context, (freenect_device_flags)(FREENECT_DEVICE_CAMERA));
int nr_devices = freenect_num_devices(context);
QString msg;
msg.setNum(nr_devices);
msg.prepend("Number of devices found: ");
emit sendMessage(msg);
int user_device_number = 0;// only one device?
if (nr_devices < 1) {
freenect_shutdown(context);
return;
}
if (freenect_open_device(context, &device, user_device_number) < 0) {
emit sendMessage(QLatin1String("Could not open device"));
freenect_shutdown(context);
return;
}
int aux = pthread_create(&fu_thread, NULL, fu_thread_function, NULL);// load buffer whith rgb
if (aux) {
emit sendMessage(QLatin1String("pthread_create failed"));
freenect_shutdown(context);
return;
}
while(!die){//------------------------------------bucle while
pthread_mutex_lock(&buffer_mutex);
u_int8_t *aux;
if(got_rgb){
aux = rgb_front;
rgb_front = rgb_mid;
rgb_mid = aux;
got_rgb = 0;
}
pthread_mutex_unlock(&buffer_mutex);
image = QImage( rgb_front ,
W,
H,
QImage::Format_RGB888,
NULL, NULL);//generate image from buffer
emit sendImageMain(image);
qApp->processEvents();//stay responsive to button click
}//---------------------------------------------------end while
}
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;
}
printf("This is the libfreenect microphone waveform viewer. Press 'q' to quit or spacebar to pause/unpause the view.\n");
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_ALPHA );
glutInitWindowSize(800, 600);
glutInitWindowPosition(0, 0);
window = glutCreateWindow("Microphones");
glClearColor(0.0, 0.0, 0.0, 0.0);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
Reshape(800, 600);
glutReshapeFunc(Reshape);
glutDisplayFunc(DrawMicData);
glutIdleFunc(DrawMicData);
glutKeyboardFunc(Keyboard);
glutMainLoop();
return 0;
}
Driver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices)
{
freenect_set_log_level(m_ctx, FREENECT_LOG_DEBUG);
freenect_select_subdevices(m_ctx, FREENECT_DEVICE_CAMERA); // OpenNI2 doesn't use MOTOR or AUDIO
DriverServices = &getServices();
}
