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