CameraImpl(ros::NodeHandle& nh, ros::NodeHandle& nh_private, const openni::DeviceInfo& device_info) :
rgb_sensor_(new SensorStreamManagerBase()),
depth_sensor_(new SensorStreamManagerBase()),
ir_sensor_(new SensorStreamManagerBase()),
reconfigure_server_(nh_private)
{
device_.open(device_info.getUri());
printDeviceInfo();
printVideoModes();
buildResolutionMap();
device_.setDepthColorSyncEnabled(true);
std::string rgb_frame_id, depth_frame_id;
nh_private.param(std::string("rgb_frame_id"), rgb_frame_id, std::string("camera_rgb_optical_frame"));
nh_private.param(std::string("depth_frame_id"), depth_frame_id, std::string("camera_depth_optical_frame"));
if(device_.hasSensor(SENSOR_COLOR))
{
rgb_sensor_.reset(new SensorStreamManager(nh, device_, SENSOR_COLOR, "rgb", rgb_frame_id, resolutions_[Camera_RGB_640x480_30Hz]));
}
if(device_.hasSensor(SENSOR_DEPTH))
{
depth_sensor_.reset(new DepthSensorStreamManager(nh, device_, rgb_frame_id, depth_frame_id, resolutions_[Camera_DEPTH_640x480_30Hz]));
}
if(device_.hasSensor(SENSOR_IR))
{
ir_sensor_.reset(new SensorStreamManager(nh, device_, SENSOR_IR, "ir", depth_frame_id, resolutions_[Camera_IR_640x480_30Hz]));
}
reconfigure_server_.setCallback(boost::bind(&CameraImpl::configure, this, _1, _2));
}
std::string oni2DevInfoStr(const openni::DeviceInfo& info, int tab){
std::stringstream sst;
std::string space;
for(int i = 0;i < tab;++i){
space += " ";
}
sst << space << "name=" << info.getName() << std::endl;
sst << space << "uri=" << info.getUri() << std::endl;
sst << space << "vendor=" << info.getVendor() << std::endl;
sst << space << "product=" << info.getUsbProductId();
return sst.str();
}
bool cmpONI2Device(const openni::DeviceInfo& i1, const openni::DeviceInfo& i2){
return (strcmp(i1.getUri(), i2.getUri()) == 0);
}