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


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Aug 11 2017 02:21:55