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