void updateDeviceList() {
device_serials_.clear();
freenect_device_attributes* attr_list;
freenect_device_attributes* item;
freenect_list_device_attributes(driver_, &attr_list);
for (item = attr_list; item != NULL; item = item->next) {
device_serials_.push_back(std::string(item->camera_serial));
}
freenect_free_device_attributes(attr_list);
}
//---------------------------------------------------------------------------
void ofxKinectContext::buildDeviceList() {
deviceList.clear();
// build the device list from freenect
freenect_device_attributes * devAttrib;
int numDevices = freenect_list_device_attributes(kinectContext, &devAttrib);
// save bus ids ...
for(int i = 0; i < numDevices; i++){
KinectPair kp;
kp.id = i;
kp.serial = (string) devAttrib->camera_serial;
deviceList.push_back(kp);
devAttrib = devAttrib->next;
}
freenect_free_device_attributes(devAttrib);
// sort devices by serial number
sort(deviceList.begin(), deviceList.end(), sortKinectPairs);
}
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;
}
