freenect_frame_mode freenect_get_current_video_mode(freenect_device *dev) { return freenect_get_video_mode(0); }
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; }