openni2_frame_listener.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  *      Author: Julius Kammerl (jkammerl@willowgarage.com)
00030  */
00031 #include "OpenNI.h"
00032 
00033 #include "openni2_camera/openni2_frame_listener.h"
00034 #include "openni2_camera/openni2_timer_filter.h"
00035 
00036 #include <sensor_msgs/image_encodings.h>
00037 
00038 #include <ros/ros.h>
00039 
00040 #define TIME_FILTER_LENGTH 15
00041 
00042 namespace openni2_wrapper
00043 {
00044 
00045 OpenNI2FrameListener::OpenNI2FrameListener() :
00046     callback_(0),
00047     user_device_timer_(false),
00048     timer_filter_(new OpenNI2TimerFilter(TIME_FILTER_LENGTH)),
00049     prev_time_stamp_(0.0)
00050 {
00051   ros::Time::init();
00052 }
00053 
00054 bool OpenNI2FrameListener::setUseDeviceTimer(bool enable)
00055 {
00056   user_device_timer_ = enable;
00057 
00058   if (user_device_timer_)
00059     timer_filter_->clear();
00060 }
00061 
00062 void OpenNI2FrameListener::onNewFrame(openni::VideoStream& stream)
00063 {
00064   stream.readFrame(&m_frame);
00065 
00066   if (m_frame.isValid() && callback_)
00067   {
00068     sensor_msgs::ImagePtr image(new sensor_msgs::Image);
00069 
00070     ros::Time ros_now = ros::Time::now();
00071 
00072     if (!user_device_timer_)
00073     {
00074       image->header.stamp = ros_now;
00075 
00076       ROS_DEBUG("Time interval between frames: %.4f ms", (float)((ros_now.toSec()-prev_time_stamp_)*1000.0));
00077 
00078       prev_time_stamp_ = ros_now.toSec();
00079     } else
00080     {
00081       uint64_t device_time = m_frame.getTimestamp();
00082 
00083       double device_time_in_sec = static_cast<double>(device_time)/1000000.0;
00084       double ros_time_in_sec = ros_now.toSec();
00085 
00086       double time_diff = ros_time_in_sec-device_time_in_sec;
00087 
00088       timer_filter_->addSample(time_diff);
00089 
00090       double filtered_time_diff = timer_filter_->getMedian();
00091 
00092       double corrected_timestamp = device_time_in_sec+filtered_time_diff;
00093 
00094       image->header.stamp.fromSec(corrected_timestamp);
00095 
00096       ROS_DEBUG("Time interval between frames: %.4f ms", (float)((corrected_timestamp-prev_time_stamp_)*1000.0));
00097 
00098       prev_time_stamp_ = corrected_timestamp;
00099     }
00100 
00101     image->width = m_frame.getWidth();
00102     image->height = m_frame.getHeight();
00103 
00104     std::size_t data_size = m_frame.getDataSize();
00105 
00106     image->data.resize(data_size);
00107     memcpy(&image->data[0], m_frame.getData(), data_size);
00108 
00109     image->is_bigendian = 0;
00110 
00111     const openni::VideoMode& video_mode = m_frame.getVideoMode();
00112     switch (video_mode.getPixelFormat())
00113     {
00114       case openni::PIXEL_FORMAT_DEPTH_1_MM:
00115         image->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00116         image->step = sizeof(unsigned char) * 2 * image->width;
00117         break;
00118       case openni::PIXEL_FORMAT_DEPTH_100_UM:
00119         image->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00120         image->step = sizeof(unsigned char) * 2 * image->width;
00121         break;
00122       case openni::PIXEL_FORMAT_SHIFT_9_2:
00123         image->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00124         image->step = sizeof(unsigned char) * 2 * image->width;
00125         break;
00126       case openni::PIXEL_FORMAT_SHIFT_9_3:
00127         image->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00128         image->step = sizeof(unsigned char) * 2 * image->width;
00129         break;
00130 
00131       case openni::PIXEL_FORMAT_RGB888:
00132         image->encoding = sensor_msgs::image_encodings::RGB8;
00133         image->step = sizeof(unsigned char) * 3 * image->width;
00134         break;
00135       case openni::PIXEL_FORMAT_YUV422:
00136         image->encoding = sensor_msgs::image_encodings::YUV422;
00137         image->step = sizeof(unsigned char) * 4 * image->width;
00138         break;
00139       case openni::PIXEL_FORMAT_GRAY8:
00140         image->encoding = sensor_msgs::image_encodings::MONO8;
00141         image->step = sizeof(unsigned char) * 1 * image->width;
00142         break;
00143       case openni::PIXEL_FORMAT_GRAY16:
00144         image->encoding = sensor_msgs::image_encodings::MONO16;
00145         image->step = sizeof(unsigned char) * 2 * image->width;
00146         break;
00147       case openni::PIXEL_FORMAT_JPEG:
00148       default:
00149         ROS_ERROR("Invalid image encoding");
00150         break;
00151     }
00152 
00153     callback_(image);
00154   }
00155 
00156 }
00157 
00158 }
00159 


openni2_camera
Author(s): Julius Kammerl
autogenerated on Fri Aug 28 2015 11:54:12