Fawkes API  Fawkes Development Version
time_cache.h
00001 /***************************************************************************
00002  *  time_cache.h - Fawkes tf time cache (based on ROS tf)
00003  *
00004  *  Created: Thu Oct 20 11:09:58 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 #ifndef __LIBS_TF_TIME_CACHE_H_
00053 #define __LIBS_TF_TIME_CACHE_H_
00054 
00055 #include <tf/types.h>
00056 
00057 #include <LinearMath/btTransform.h>
00058 #include <list>
00059 #include <stdint.h>
00060 
00061 namespace fawkes {
00062   namespace tf {
00063 #if 0 /* just to make Emacs auto-indent happy */
00064   }
00065 }
00066 #endif
00067 
00068 enum ExtrapolationMode {
00069   ONE_VALUE,
00070   INTERPOLATE,
00071   EXTRAPOLATE_BACK,
00072   EXTRAPOLATE_FORWARD
00073 };
00074 
00075 typedef std::pair<fawkes::Time, CompactFrameID> P_TimeAndFrameID;
00076 
00077 class TransformStorage
00078 {
00079  public:
00080   TransformStorage();
00081   TransformStorage(const StampedTransform& data, CompactFrameID frame_id,
00082                    CompactFrameID child_frame_id);
00083   TransformStorage(const TransformStorage& rhs);
00084 
00085   TransformStorage& operator=(const TransformStorage& rhs)
00086   {
00087     rotation_ = rhs.rotation_;
00088     translation_ = rhs.translation_;
00089     stamp = rhs.stamp;
00090     frame_id_ = rhs.frame_id_;
00091     child_frame_id_ = rhs.child_frame_id_;
00092     return *this;
00093   }
00094 
00095   btQuaternion rotation_;       ///< rotation quaternio
00096   btVector3 translation_;       ///< translation vector
00097   fawkes::Time stamp;           ///< time stamp
00098   CompactFrameID frame_id_;     ///< parent/reference frame number
00099   CompactFrameID child_frame_id_;       ///< child frame number
00100 };
00101 
00102 
00103 
00104 class TimeCache
00105 {
00106  public:
00107   /// Number of nano-seconds to not interpolate below.
00108   static const int MIN_INTERPOLATION_DISTANCE = 5;
00109   /// Maximum length of linked list, to make sure not to be able to use unlimited memory.
00110   static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000;
00111 
00112   TimeCache(float max_storage_time = 10.0);
00113 
00114   bool get_data(fawkes::Time time, TransformStorage & data_out,
00115                 std::string* error_str = 0);
00116   bool insert_data(const TransformStorage& new_data);
00117   void clear_list();
00118   CompactFrameID get_parent(fawkes::Time time, std::string* error_str);
00119   P_TimeAndFrameID get_latest_time_and_parent() const;
00120 
00121   /// Debugging information methods
00122   unsigned int get_list_length() const;
00123   fawkes::Time get_latest_timestamp() const;
00124   fawkes::Time get_oldest_timestamp() const;
00125 
00126  private:
00127   typedef std::list<TransformStorage> L_TransformStorage;
00128   L_TransformStorage storage_;
00129 
00130   float max_storage_time_;
00131 
00132 
00133   inline uint8_t find_closest(TransformStorage*& one, TransformStorage*& two,
00134                               fawkes::Time target_time, std::string* error_str);
00135 
00136   inline void interpolate(const TransformStorage& one, const TransformStorage& two,
00137                           fawkes::Time time, TransformStorage& output);
00138 
00139   void prune_list();
00140 };
00141 
00142 
00143 } // end namespace tf
00144 } // end namespace fawkes
00145 
00146 #endif