Fawkes API  Fawkes Development Version
time_cache.cpp
00001 /***************************************************************************
00002  *  time_cache.cpp - Fawkes tf time cache (based on ROS tf)
00003  *
00004  *  Created: Thu Oct 20 11:26:40 2011
00005  *  Copyright  2011  Tim Niemueller [www.niemueller.de]
00006  ****************************************************************************/
00007 
00008 /*  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version. A runtime exception applies to
00012  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00020  */
00021 
00022 /* This code is based on ROS tf with the following copyright and license:
00023  *
00024  * Copyright (c) 2008, Willow Garage, Inc.
00025  * All rights reserved.
00026  * 
00027  * Redistribution and use in source and binary forms, with or without
00028  * modification, are permitted provided that the following conditions are met:
00029  * 
00030  *     * Redistributions of source code must retain the above copyright
00031  *       notice, this list of conditions and the following disclaimer.
00032  *     * Redistributions in binary form must reproduce the above copyright
00033  *       notice, this list of conditions and the following disclaimer in the
00034  *       documentation and/or other materials provided with the distribution.
00035  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00036  *       contributors may be used to endorse or promote products derived from
00037  *       this software without specific prior written permission.
00038  * 
00039  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00040  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00041  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00042  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00043  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00044  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00045  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00046  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00047  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00048  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00049  * POSSIBILITY OF SUCH DAMAGE.
00050  */
00051 
00052 #include <tf/time_cache.h>
00053 
00054 #include <cstdio>
00055 
00056 namespace fawkes {
00057   namespace tf {
00058 #if 0 /* just to make Emacs auto-indent happy */
00059   }
00060 }
00061 #endif
00062 
00063 /** @class TransformStorage <tf/time_cache.h>
00064  * Storage for transforms and their parent.
00065  *
00066  * @fn TransformStorage & TransformStorage::operator=(const TransformStorage& rhs)
00067  * Assignment operator.
00068  * @param rhs storage to assign
00069  * @return reference to this instance
00070  */
00071 
00072 /** Constructor. */
00073 TransformStorage::TransformStorage()
00074 {
00075 }
00076 
00077 /** Constructor.
00078  * @param data initial stamped transform data
00079  * @param frame_id parent frame ID
00080  * @param child_frame_id child frame ID
00081  */
00082 TransformStorage::TransformStorage(const StampedTransform& data,
00083                                    CompactFrameID frame_id,
00084                                    CompactFrameID child_frame_id)
00085 : rotation_(data.getRotation())
00086 , translation_(data.getOrigin())
00087 , stamp(data.stamp)
00088 , frame_id_(frame_id)
00089 , child_frame_id_(child_frame_id)
00090 { }
00091 
00092 
00093 /** Copy constructor.
00094  * @param rhs storage to copy
00095  */
00096 TransformStorage::TransformStorage(const TransformStorage& rhs)
00097 {
00098   *this = rhs;
00099 }
00100 
00101 
00102 /** @class TimeCache <tf/time_cache.h>
00103  * Time based transform cache.
00104  * A class to keep a sorted linked list in time. This builds and
00105  * maintains a list of timestamped data.  And provides lookup
00106  * functions to get data out as a function of time.
00107  */
00108 
00109 /** Constructor.
00110  * @param max_storage_time maximum time in seconds to cache, defaults to 10 seconds
00111  */
00112 TimeCache::TimeCache(float max_storage_time)
00113 : max_storage_time_(max_storage_time)
00114 {}
00115 
00116 /** Create extrapolation error string.
00117  * @param t0 requested time
00118  * @param t1 available time
00119  * @param error_str upon return contains the error string.
00120  */
00121 // hoisting these into separate functions causes an ~8% speedup.
00122 // Removing calling them altogether adds another ~10%
00123 void
00124 create_extrapolation_exception1(fawkes::Time t0, fawkes::Time t1, std::string* error_str)
00125 {
00126   if (error_str)
00127   {
00128     char *tmp;
00129     if (asprintf(&tmp, "Lookup would require extrapolation at time %li.%li, "
00130                  "but only time %li.%li is in the buffer", t0.get_sec(), t0.get_nsec(),
00131                  t1.get_sec(), t1.get_nsec()) != -1)
00132     {
00133       *error_str = tmp;
00134       free(tmp);
00135     }
00136   }
00137 }
00138 
00139 /** Create extrapolation error string.
00140  * @param t0 requested time
00141  * @param t1 available time
00142  * @param error_str upon return contains the error string.
00143  */
00144 void
00145 create_extrapolation_exception2(fawkes::Time t0, fawkes::Time t1, std::string* error_str)
00146 {
00147   if (error_str)
00148   {
00149     char *tmp;
00150     if (asprintf(&tmp,"Lookup would require extrapolation into the future. "
00151                  "Requested time %s, but the latest data is at time %s",
00152                  t0.str(), t1.str()) != -1)
00153     {
00154       *error_str = tmp;
00155       free(tmp);
00156     }
00157   }
00158 }
00159 
00160 /** Create extrapolation error string.
00161  * @param t0 requested time
00162  * @param t1 available time
00163  * @param error_str upon return contains the error string.
00164  */
00165 void
00166 create_extrapolation_exception3(fawkes::Time t0, fawkes::Time t1, std::string* error_str)
00167 {
00168   if (error_str)
00169   {
00170     char *tmp;
00171     if (asprintf(&tmp,"Lookup would require extrapolation into the past. "
00172                  "Requested time %s, but the latest data is at time %s",
00173                  t0.str(), t1.str()) != -1)
00174     {
00175       *error_str = tmp;
00176       free(tmp);
00177     }
00178   }
00179 }
00180 
00181 /// A helper function for getData
00182 //Assumes storage is already locked for it
00183 uint8_t
00184 TimeCache::find_closest(TransformStorage*& one, TransformStorage*& two,
00185                         fawkes::Time target_time, std::string* error_str)
00186 {
00187   //No values stored
00188   if (storage_.empty()) {
00189     return 0;
00190   }
00191 
00192   //If time == 0 return the latest
00193   if (target_time.is_zero()) {
00194     one = &storage_.front();
00195     return 1;
00196   }
00197 
00198   // One value stored
00199   if (++storage_.begin() == storage_.end()) {
00200     TransformStorage& ts = *storage_.begin();
00201     if (ts.stamp == target_time) {
00202       one = &ts;
00203       return 1;
00204     } else {
00205       create_extrapolation_exception1(target_time, ts.stamp, error_str);
00206       return 0;
00207     }
00208   }
00209 
00210   fawkes::Time latest_time = (*storage_.begin()).stamp;
00211   fawkes::Time earliest_time = (*(storage_.rbegin())).stamp;
00212 
00213   if (target_time == latest_time) {
00214     one = &(*storage_.begin());
00215     return 1;
00216   } else if (target_time == earliest_time) {
00217     one = &(*storage_.rbegin());
00218     return 1;
00219   } else if (target_time > latest_time) {
00220     // Catch cases that would require extrapolation
00221     create_extrapolation_exception2(target_time, latest_time, error_str);
00222     return 0;
00223   } else if (target_time < earliest_time) {
00224     create_extrapolation_exception3(target_time, earliest_time, error_str);
00225     return 0;
00226   }
00227 
00228   //At least 2 values stored
00229   //Find the first value less than the target value
00230   L_TransformStorage::iterator storage_it = storage_.begin();
00231   while(storage_it != storage_.end()) {
00232     if (storage_it->stamp <= target_time)  break;
00233     storage_it++;
00234   }
00235 
00236   //Finally the case were somewhere in the middle  Guarenteed no extrapolation :-)
00237   one = &*(storage_it); //Older
00238   two = &*(--storage_it); //Newer
00239   return 2;
00240 }
00241 
00242 
00243 void
00244 TimeCache::interpolate(const TransformStorage& one,
00245                        const TransformStorage& two,
00246                        fawkes::Time time, TransformStorage& output)
00247 {
00248   // Check for zero distance case
00249   if( two.stamp == one.stamp ) {
00250     output = two;
00251     return;
00252   }
00253   //Calculate the ratio
00254   btScalar ratio =
00255     (time.in_sec() - one.stamp.in_sec()) /
00256     (two.stamp.in_sec() - one.stamp.in_sec());
00257 
00258   //Interpolate translation
00259   output.translation_.setInterpolate3(one.translation_, two.translation_, ratio);
00260 
00261   //Interpolate rotation
00262   output.rotation_ = slerp( one.rotation_, two.rotation_, ratio);
00263 
00264   output.stamp = one.stamp;
00265   output.frame_id_ = one.frame_id_;
00266   output.child_frame_id_ = one.child_frame_id_;
00267 }
00268 
00269 /** Get data.
00270  * @param time time for which go get data
00271  * @param data_out upon return contains requested data
00272  * @param error_str error stirng
00273 * @return false if data not available
00274 */
00275 bool
00276 TimeCache::get_data(fawkes::Time time, TransformStorage & data_out,
00277                     std::string* error_str)
00278 {
00279   TransformStorage* p_temp_1 = NULL;
00280   TransformStorage* p_temp_2 = NULL;
00281 
00282   int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
00283   if (num_nodes == 0) {
00284     return false;
00285   } else if (num_nodes == 1) {
00286     data_out = *p_temp_1;
00287   } else if (num_nodes == 2) {
00288     if( p_temp_1->frame_id_ == p_temp_2->frame_id_) {
00289       interpolate(*p_temp_1, *p_temp_2, time, data_out);
00290     } else {
00291       data_out = *p_temp_1;
00292     }
00293   }
00294 
00295   return true;
00296 }
00297 
00298 /** Get parent frame number
00299  * @param time point in time
00300  * @param error_str error string
00301  * @return frame number
00302  */
00303 CompactFrameID
00304 TimeCache::get_parent(fawkes::Time time, std::string* error_str)
00305 {
00306   TransformStorage* p_temp_1 = NULL;
00307   TransformStorage* p_temp_2 = NULL;
00308 
00309   int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
00310   if (num_nodes == 0) {
00311     return 0;
00312   }
00313 
00314   return p_temp_1->frame_id_;
00315 }
00316 
00317 
00318 /** Insert data.
00319  * @param new_data data to insert
00320  * @return true on success, false otherwise
00321  */
00322 bool
00323 TimeCache::insert_data(const TransformStorage& new_data)
00324 {
00325   L_TransformStorage::iterator storage_it = storage_.begin();
00326 
00327   if(storage_it != storage_.end()) {
00328     if (storage_it->stamp > new_data.stamp + max_storage_time_) {
00329       return false;
00330     }
00331   }
00332 
00333 
00334   while(storage_it != storage_.end()) {
00335     if (storage_it->stamp <= new_data.stamp)
00336       break;
00337     storage_it++;
00338   }
00339   storage_.insert(storage_it, new_data);
00340 
00341   prune_list();
00342   return true;
00343 }
00344 
00345 /** Clear storage. */
00346 void
00347 TimeCache::clear_list()
00348 {
00349   storage_.clear();
00350 }
00351 
00352 /** Get storage list length.
00353  * @return storage list length
00354  */
00355 unsigned int
00356 TimeCache::get_list_length() const
00357 {
00358   return storage_.size();
00359 }
00360 
00361 /** Get latest time and parent frame number.
00362  * @return latest time and parent frame number
00363  */
00364 P_TimeAndFrameID
00365 TimeCache::get_latest_time_and_parent() const
00366 {
00367   if (storage_.empty()) {
00368     return std::make_pair(fawkes::Time(), 0);
00369   }
00370 
00371   const TransformStorage& ts = storage_.front();
00372   return std::make_pair(ts.stamp, ts.frame_id_);
00373 }
00374 
00375 /** Get latest timestamp from cache.
00376  * @return latest timestamp
00377  */
00378 fawkes::Time
00379 TimeCache::get_latest_timestamp() const
00380 {
00381   if (storage_.empty()) return fawkes::Time(); //empty list case
00382   return storage_.front().stamp;
00383 }
00384 
00385 /** Get oldest timestamp from cache.
00386  * @return oldest time stamp.
00387  */
00388 fawkes::Time
00389 TimeCache::get_oldest_timestamp() const
00390 {
00391   if (storage_.empty()) return fawkes::Time(); //empty list case
00392   return storage_.back().stamp;
00393 }
00394 
00395 /** Prune storage list based on maximum cache lifetime. */
00396 void
00397 TimeCache::prune_list()
00398 {
00399   fawkes::Time latest_time = storage_.begin()->stamp;
00400   
00401   while(!storage_.empty() &&
00402         storage_.back().stamp + max_storage_time_ < latest_time)
00403   {
00404     storage_.pop_back();
00405   }
00406 
00407 }
00408 
00409 
00410 } // end namespace tf
00411 } // end namespace fawkes