00001 /* 00002 * Copyright (c) 2008, 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 00032 #include "tf/time_cache.h" 00033 00034 using namespace tf; 00035 00036 bool TimeCache::getData(ros::Time time, TransformStorage & data_out) //returns false if data not available 00037 { 00038 TransformStorage p_temp_1, p_temp_2; 00039 00040 int num_nodes; 00041 ros::Duration time_diff; 00042 boost::mutex::scoped_lock lock(storage_lock_); 00043 00044 ExtrapolationMode mode; 00045 num_nodes = findClosest(p_temp_1,p_temp_2, time, mode); 00046 if (num_nodes == 1) 00047 { 00048 data_out = p_temp_1; 00049 data_out.mode_ = mode; 00050 } 00051 else if (num_nodes == 2) 00052 { 00053 if(interpolating_ && ( p_temp_1.frame_id_num_ == p_temp_2.frame_id_num_) ) // if we're interpolating and haven't reparented 00054 { 00055 interpolate(p_temp_1, p_temp_2, time, data_out); 00056 data_out.mode_ = mode; 00057 } 00058 else 00059 { 00060 data_out = p_temp_1; 00061 data_out.mode_ = mode; 00062 } 00063 } 00064 00065 return (num_nodes > 0); 00066 00067 }; 00068 00069 00070 uint8_t TimeCache::findClosest(TransformStorage& one, TransformStorage& two, ros::Time target_time, ExtrapolationMode& mode) 00071 { 00072 //No values stored 00073 if (storage_.empty()) 00074 { 00075 return 0; 00076 } 00077 00078 //If time == 0 return the latest 00079 if (target_time == ros::Time()) 00080 { 00081 one = storage_.front(); 00082 mode = ONE_VALUE; 00083 return 1; 00084 } 00085 00086 // One value stored 00087 if (++storage_.begin() == storage_.end()) 00088 { 00089 one = *(storage_.begin()); 00090 mode = ONE_VALUE; 00091 return 1; 00092 } 00093 00094 //At least 2 values stored 00095 //Find the first value less than the target value 00096 std::list<TransformStorage >::iterator storage_it = storage_.begin(); 00097 while(storage_it != storage_.end()) 00098 { 00099 if (storage_it->stamp_ <= target_time) 00100 break; 00101 storage_it++; 00102 } 00103 //Catch the case it is the first value in the list 00104 if (storage_it == storage_.begin()) 00105 { 00106 one = *storage_it; 00107 two = *(++storage_it); 00108 mode = EXTRAPOLATE_FORWARD; 00109 00110 /* if (time_diff > max_extrapolation_time_) //Guarenteed in the future therefore positive 00111 { 00112 std::stringstream ss; 00113 ss << "Extrapolation Too Far in the future: target_time = "<< (target_time).toSec() <<", closest data at " 00114 << (one.stamp_).toSec() << " and " << (two.stamp_).toSec() <<" which are farther away than max_extrapolation_time " 00115 << (max_extrapolation_time_).toSec() <<" at "<< (target_time - one.stamp_).toSec()<< " and " << (target_time - two.stamp_).toSec() <<" respectively."; 00116 throw ExtrapolationException(ss.str()); 00117 } 00118 */ 00119 return 2; 00120 } 00121 00122 //Catch the case where it's in the past 00123 if (storage_it == storage_.end()) 00124 { 00125 one = *(--storage_it); 00126 two = *(--storage_it); 00127 mode = EXTRAPOLATE_BACK; 00128 /* 00129 time_diff = target_time - one.stamp_; 00130 if (time_diff < ros::Duration()-max_extrapolation_time_) //Guarenteed in the past ///\todo check negative sign 00131 { 00132 std::stringstream ss; 00133 ss << "Extrapolation Too Far in the past: target_time = "<< (target_time).toSec() <<", closest data at " 00134 << (one.stamp_).toSec() << " and " << (two.stamp_).toSec() <<" which are farther away than max_extrapolation_time " 00135 << (max_extrapolation_time_).toSec() <<" at "<< (target_time - one.stamp_).toSec()<< " and " << (target_time - two.stamp_).toSec() <<" respectively."; //sign flip since in the past 00136 throw ExtrapolationException(ss.str()); 00137 } 00138 */ 00139 return 2; 00140 } 00141 00142 //Finally the case were somewhere in the middle Guarenteed no extrapolation :-) 00143 one = *(storage_it); //Older 00144 two = *(--storage_it); //Newer 00145 mode = INTERPOLATE; 00146 return 2; 00147 00148 00149 }; 00150 00151 void TimeCache::interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output) 00152 { 00153 00154 // Check for zero distance case 00155 if( two.stamp_ == one.stamp_ ) 00156 { 00157 output = two; 00158 return; 00159 } 00160 //Calculate the ratio 00161 btScalar ratio = ((time - one.stamp_).toSec()) / ((two.stamp_ - one.stamp_).toSec()); 00162 00163 //Interpolate translation 00164 btVector3 v(0,0,0); //initialzed to fix uninitialized warning, not actually necessary 00165 v.setInterpolate3(one.getOrigin(), two.getOrigin(), ratio); 00166 output.setOrigin(v); 00167 00168 //Interpolate rotation 00169 btQuaternion q1,q2; 00170 one.getBasis().getRotation(q1); 00171 two.getBasis().getRotation(q2); 00172 output.setRotation(slerp( q1, q2 , ratio)); 00173 output.stamp_ = one.stamp_; 00174 output.frame_id_ = one.frame_id_; 00175 output.child_frame_id_ = one.child_frame_id_; 00176 output.frame_id_num_ = one.frame_id_num_; 00177 }; 00178