void XtionDepthDriverImpl::onNewFrame(VideoStream &stream)
{
VideoFrameRef ref;
stream.readFrame(&ref);
_lastCaptured = XtionDepthImage(ref.getData(), ref.getDataSize(),
ref.getWidth(), ref.getHeight(), 0, this);
}
virtual void onNewFrame(VideoStream& stream)
{
ros::Time ts = ros::Time::now();
VideoFrameRef frame;
stream.readFrame(&frame);
sensor_msgs::Image::Ptr img(new sensor_msgs::Image);
sensor_msgs::CameraInfo::Ptr info(new sensor_msgs::CameraInfo);
double scale = double(frame.getWidth()) / double(1280);
info->header.stamp = ts;
info->header.frame_id = frame_id_;
info->width = frame.getWidth();
info->height = frame.getHeight();
info->K.assign(0);
info->K[0] = 1050.0 * scale;
info->K[4] = 1050.0 * scale;
info->K[2] = frame.getWidth() / 2.0 - 0.5;
info->K[5] = frame.getHeight() / 2.0 - 0.5;
info->P.assign(0);
info->P[0] = 1050.0 * scale;
info->P[5] = 1050.0 * scale;
info->P[2] = frame.getWidth() / 2.0 - 0.5;
info->P[6] = frame.getHeight() / 2.0 - 0.5;
switch(frame.getVideoMode().getPixelFormat())
{
case PIXEL_FORMAT_GRAY8:
img->encoding = sensor_msgs::image_encodings::MONO8;
break;
case PIXEL_FORMAT_GRAY16:
img->encoding = sensor_msgs::image_encodings::MONO16;
break;
case PIXEL_FORMAT_YUV422:
img->encoding = sensor_msgs::image_encodings::YUV422;
break;
case PIXEL_FORMAT_RGB888:
img->encoding = sensor_msgs::image_encodings::RGB8;
break;
case PIXEL_FORMAT_SHIFT_9_2:
case PIXEL_FORMAT_DEPTH_1_MM:
img->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
break;
default:
ROS_WARN("Unknown OpenNI pixel format!");
break;
}
img->header.stamp = ts;
img->header.frame_id = frame_id_;
img->height = frame.getHeight();
img->width = frame.getWidth();
img->step = frame.getStrideInBytes();
img->data.resize(frame.getDataSize());
std::copy(static_cast<const uint8_t*>(frame.getData()), static_cast<const uint8_t*>(frame.getData()) + frame.getDataSize(), img->data.begin());
publish(img, info);
}