cache.cpp
Go to the documentation of this file.
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 "tf2/time_cache.h"
00033 #include "tf2/exceptions.h"
00034 
00035 #include <tf2/LinearMath/btVector3.h>
00036 #include <tf2/LinearMath/btQuaternion.h>
00037 #include <tf2/LinearMath/btTransform.h>
00038 #include <geometry_msgs/TransformStamped.h>
00039 #include <ros/assert.h>
00040 
00041 using namespace tf2;
00042 
00043 TransformStorage::TransformStorage()
00044 {
00045 }
00046 
00047 TransformStorage::TransformStorage(const geometry_msgs::TransformStamped& data, CompactFrameID frame_id,
00048                                    CompactFrameID child_frame_id)
00049 : stamp_(data.header.stamp)
00050 , frame_id_(frame_id)
00051 , child_frame_id_(child_frame_id)
00052 {
00053   const geometry_msgs::Quaternion& o = data.transform.rotation;
00054   rotation_ = btQuaternion(o.x, o.y, o.z, o.w);
00055   const geometry_msgs::Vector3& v = data.transform.translation;
00056   translation_ = btVector3(v.x, v.y, v.z);
00057 }
00058 
00059 TimeCache::TimeCache(ros::Duration max_storage_time)
00060 : max_storage_time_(max_storage_time)
00061 {}
00062 
00063 // hoisting these into separate functions causes an ~8% speedup.  Removing calling them altogether adds another ~10%
00064 void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string* error_str)
00065 {
00066   if (error_str)
00067   {
00068     std::stringstream ss;
00069     ss << "Lookup would require extrapolation at time " << t0 << ", but only time " << t1 << " is in the buffer";
00070     *error_str = ss.str();
00071   }
00072 }
00073 
00074 void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string* error_str)
00075 {
00076   if (error_str)
00077   {
00078     std::stringstream ss;
00079     ss << "Lookup would require extrapolation into the future.  Requested time " << t0 << " but the latest data is at time " << t1;
00080     *error_str = ss.str();
00081   }
00082 }
00083 
00084 void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string* error_str)
00085 {
00086   if (error_str)
00087   {
00088     std::stringstream ss;
00089     ss << "Lookup would require extrapolation into the past.  Requested time " << t0 << " but the earliest data is at time " << t1;
00090     *error_str = ss.str();
00091   }
00092 }
00093 
00094 
00095 uint8_t TimeCache::findClosest(TransformStorage*& one, TransformStorage*& two, ros::Time target_time, std::string* error_str)
00096 {
00097   //No values stored
00098   if (storage_.empty())
00099   {
00100     return 0;
00101   }
00102 
00103   //If time == 0 return the latest
00104   if (target_time.isZero())
00105   {
00106     one = &storage_.front();
00107     return 1;
00108   }
00109 
00110   // One value stored
00111   if (++storage_.begin() == storage_.end())
00112   {
00113     TransformStorage& ts = *storage_.begin();
00114     if (ts.stamp_ == target_time)
00115     {
00116       one = &ts;
00117       return 1;
00118     }
00119     else
00120     {
00121       createExtrapolationException1(target_time, ts.stamp_, error_str);
00122       return 0;
00123     }
00124   }
00125 
00126   ros::Time latest_time = (*storage_.begin()).stamp_;
00127   ros::Time earliest_time = (*(storage_.rbegin())).stamp_;
00128 
00129   if (target_time == latest_time)
00130   {
00131     one = &(*storage_.begin());
00132     return 1;
00133   }
00134   else if (target_time == earliest_time)
00135   {
00136     one = &(*storage_.rbegin());
00137     return 1;
00138   }
00139   // Catch cases that would require extrapolation
00140   else if (target_time > latest_time)
00141   {
00142     createExtrapolationException2(target_time, latest_time, error_str);
00143     return 0;
00144   }
00145   else if (target_time < earliest_time)
00146   {
00147     createExtrapolationException3(target_time, earliest_time, error_str);
00148     return 0;
00149   }
00150 
00151   //At least 2 values stored
00152   //Find the first value less than the target value
00153   L_TransformStorage::iterator storage_it = storage_.begin();
00154   while(storage_it != storage_.end())
00155   {
00156     if (storage_it->stamp_ <= target_time)
00157       break;
00158     storage_it++;
00159   }
00160 
00161   //Finally the case were somewhere in the middle  Guarenteed no extrapolation :-)
00162   one = &*(storage_it); //Older
00163   two = &*(--storage_it); //Newer
00164   return 2;
00165 
00166 
00167 }
00168 
00169 void TimeCache::interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output)
00170 {
00171   // Check for zero distance case
00172   if( two.stamp_ == one.stamp_ )
00173   {
00174     output = two;
00175     return;
00176   }
00177   //Calculate the ratio
00178   btScalar ratio = (time.toSec() - one.stamp_.toSec()) / (two.stamp_.toSec() - one.stamp_.toSec());
00179 
00180   //Interpolate translation
00181   output.translation_.setInterpolate3(one.translation_, two.translation_, ratio);
00182 
00183   //Interpolate rotation
00184   output.rotation_ = slerp( one.rotation_, two.rotation_, ratio);
00185 
00186   output.stamp_ = one.stamp_;
00187   output.frame_id_ = one.frame_id_;
00188   output.child_frame_id_ = one.child_frame_id_;
00189 }
00190 
00191 bool TimeCache::getData(ros::Time time, TransformStorage & data_out, std::string* error_str) //returns false if data not available
00192 {
00193   TransformStorage* p_temp_1;
00194   TransformStorage* p_temp_2;
00195 
00196   int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str);
00197   if (num_nodes == 0)
00198   {
00199     return false;
00200   }
00201   else if (num_nodes == 1)
00202   {
00203     data_out = *p_temp_1;
00204   }
00205   else if (num_nodes == 2)
00206   {
00207     if( p_temp_1->frame_id_ == p_temp_2->frame_id_)
00208     {
00209       interpolate(*p_temp_1, *p_temp_2, time, data_out);
00210     }
00211     else
00212     {
00213       data_out = *p_temp_1;
00214     }
00215   }
00216   else
00217   {
00218     ROS_BREAK();
00219   }
00220 
00221   return true;
00222 }
00223 
00224 CompactFrameID TimeCache::getParent(ros::Time time, std::string* error_str)
00225 {
00226   TransformStorage* p_temp_1;
00227   TransformStorage* p_temp_2;
00228 
00229   int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str);
00230   if (num_nodes == 0)
00231   {
00232     return 0;
00233   }
00234 
00235   return p_temp_1->frame_id_;
00236 }
00237 
00238 bool TimeCache::insertData(const TransformStorage& new_data)
00239 {
00240   L_TransformStorage::iterator storage_it = storage_.begin();
00241 
00242   if(storage_it != storage_.end())
00243   {
00244     if (storage_it->stamp_ > new_data.stamp_ + max_storage_time_)
00245     {
00246       return false;
00247     }
00248   }
00249 
00250 
00251   while(storage_it != storage_.end())
00252   {
00253     if (storage_it->stamp_ <= new_data.stamp_)
00254       break;
00255     storage_it++;
00256   }
00257   storage_.insert(storage_it, new_data);
00258 
00259   pruneList();
00260   return true;
00261 }
00262 
00263 void TimeCache::clearList()
00264 {
00265   storage_.clear();
00266 }
00267 
00268 unsigned int TimeCache::getListLength()
00269 {
00270   return storage_.size();
00271 }
00272 
00273 P_TimeAndFrameID TimeCache::getLatestTimeAndParent()
00274 {
00275   if (storage_.empty())
00276   {
00277     return std::make_pair(ros::Time(), 0);
00278   }
00279 
00280   const TransformStorage& ts = storage_.front();
00281   return std::make_pair(ts.stamp_, ts.frame_id_);
00282 }
00283 
00284 ros::Time TimeCache::getLatestTimestamp() 
00285 {   
00286   if (storage_.empty()) return ros::Time(); //empty list case
00287   return storage_.front().stamp_;
00288 }
00289 
00290 ros::Time TimeCache::getOldestTimestamp() 
00291 {   
00292   if (storage_.empty()) return ros::Time(); //empty list case
00293   return storage_.back().stamp_;
00294 }
00295 
00296 void TimeCache::pruneList()
00297 {
00298   ros::Time latest_time = storage_.begin()->stamp_;
00299   
00300   while(!storage_.empty() && storage_.back().stamp_ + max_storage_time_ < latest_time)
00301   {
00302     storage_.pop_back();
00303   }
00304   
00305 }


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Oct 6 2014 00:12:43