SensorStreamManager(ros::NodeHandle& nh, Device& device, SensorType type, std::string name, std::string frame_id, VideoMode& default_mode) :
device_(device),
default_mode_(default_mode),
name_(name),
frame_id_(frame_id),
running_(false),
nh_(nh, name_),
it_(nh_),
camera_info_manager_(nh_)
{
assert(device_.hasSensor(type));
callback_ = boost::bind(&SensorStreamManager::onSubscriptionChanged, this, _1);
publisher_ = it_.advertiseCamera("image_raw", 1, callback_, callback_);
ROS_ERROR_STREAM_COND(stream_.create(device_, type) != STATUS_OK, "Failed to create stream '" << toString(type) << "'!");
stream_.addNewFrameListener(this);
ROS_ERROR_STREAM_COND(stream_.setVideoMode(default_mode_) != STATUS_OK, "Failed to set default video mode for stream '" << toString(type) << "'!");
}
virtual void endConfigure()
{
if(was_running_)
{
Status rc = stream_.start();
if(rc != STATUS_OK)
{
SensorType type = stream_.getSensorInfo().getSensorType();
ROS_WARN_STREAM("Failed to restart stream '" << name_ << "' after configuration!");
int max_trials = 1;
for(int trials = 0; trials < max_trials && rc != STATUS_OK; ++trials)
{
ros::Duration(0.1).sleep();
stream_.removeNewFrameListener(this);
stream_.destroy();
stream_.create(device_, type);
stream_.addNewFrameListener(this);
//stream_.setVideoMode(default_mode_);
rc = stream_.start();
ROS_WARN_STREAM_COND(rc != STATUS_OK, "Recovery trial " << trials << " failed!");
}
ROS_ERROR_STREAM_COND(rc != STATUS_OK, "Failed to recover stream '" << name_ << "'! Restart required!");
ROS_INFO_STREAM_COND(rc == STATUS_OK, "Recovered stream '" << name_ << "'.");
}
if(rc == STATUS_OK)
{
running_ = true;
}
}
}
int main()
{
Status rc = OpenNI::initialize();
if (rc != STATUS_OK)
{
printf("Initialize failed\n%s\n", OpenNI::getExtendedError());
return 1;
}
OpenNIDeviceListener devicePrinter;
OpenNI::addDeviceConnectedListener(&devicePrinter);
OpenNI::addDeviceDisconnectedListener(&devicePrinter);
OpenNI::addDeviceStateChangedListener(&devicePrinter);
openni::Array<openni::DeviceInfo> deviceList;
openni::OpenNI::enumerateDevices(&deviceList);
for (int i = 0; i < deviceList.getSize(); ++i)
{
printf("Device \"%s\" already connected\n", deviceList[i].getUri());
}
Device device;
rc = device.open(ANY_DEVICE);
if (rc != STATUS_OK)
{
printf("Couldn't open device\n%s\n", OpenNI::getExtendedError());
return 2;
}
VideoStream depth;
if (device.getSensorInfo(SENSOR_DEPTH) != NULL)
{
rc = depth.create(device, SENSOR_DEPTH);
if (rc != STATUS_OK)
{
printf("Couldn't create depth stream\n%s\n", OpenNI::getExtendedError());
}
}
rc = depth.start();
if (rc != STATUS_OK)
{
printf("Couldn't start the depth stream\n%s\n", OpenNI::getExtendedError());
}
PrintCallback depthPrinter;
// Register to new frame
depth.addNewFrameListener(&depthPrinter);
int i = 1;
while(i > 0)
{
scanf_s("%d", i);
printf("%d\n", i);
}
depth.removeNewFrameListener(&depthPrinter);
depth.stop();
depth.destroy();
device.close();
OpenNI::shutdown();
return 0;
}