00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00032 #ifndef TF_TIME_CACHE_H
00033 #define TF_TIME_CACHE_H
00034
00035 #include <list>
00036 #include <boost/thread/mutex.hpp>
00037
00038 #include "tf/transform_datatypes.h"
00039 #include "tf/exceptions.h"
00040
00041 #include "LinearMath/btTransform.h"
00042
00043 #include <sstream>
00044
00045 namespace tf
00046 {
00047 enum ExtrapolationMode { ONE_VALUE, INTERPOLATE, EXTRAPOLATE_BACK, EXTRAPOLATE_FORWARD };
00048
00050 class TransformStorage : public StampedTransform
00051 {
00052 public:
00053 TransformStorage(){};
00054 TransformStorage(const StampedTransform& data, unsigned int frame_id_num): StampedTransform(data), frame_id_num_(frame_id_num){};
00055 unsigned int frame_id_num_;
00056 ExtrapolationMode mode_;
00057 };
00058
00059
00064 class TimeCache
00065 {
00066 public:
00067 static const int MIN_INTERPOLATION_DISTANCE = 5;
00068 static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000;
00069 static const int64_t DEFAULT_MAX_STORAGE_TIME = 1ULL * 1000000000LL;
00070 static const int64_t DEFAULT_MAX_EXTRAPOLATION_TIME = 0LL;
00071
00072
00073 TimeCache(bool interpolating = true, ros::Duration max_storage_time = ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME),
00074 ros::Duration max_extrapolation_time = ros::Duration().fromNSec(DEFAULT_MAX_EXTRAPOLATION_TIME)):
00075 interpolating_(interpolating),
00076 max_storage_time_(max_storage_time),
00077 max_extrapolation_time_(max_extrapolation_time)
00078 {};
00079
00080
00081 bool getData(ros::Time time, TransformStorage & data_out);
00082
00083 bool insertData(const TransformStorage& new_data)
00084 {
00085
00086 boost::mutex::scoped_lock lock(storage_lock_);
00087
00088 std::list<TransformStorage >::iterator storage_it = storage_.begin();
00089
00090 if(storage_it != storage_.end())
00091 {
00092 if (storage_it->stamp_ > new_data.stamp_ + max_storage_time_)
00093 {
00094 return false;
00095 }
00096 }
00097
00098
00099 while(storage_it != storage_.end())
00100 {
00101 if (storage_it->stamp_ <= new_data.stamp_)
00102 break;
00103 storage_it++;
00104 }
00105 storage_.insert(storage_it, new_data);
00106
00107 pruneList();
00108 return true;
00109 };
00110
00111
00112 void interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output);
00113
00115 void clearList() { boost::mutex::scoped_lock lock(storage_lock_); storage_.clear(); };
00116
00118 unsigned int getListLength() { boost::mutex::scoped_lock lock(storage_lock_); return storage_.size(); };
00119
00121 ros::Time getLatestTimestamp()
00122 {
00123 boost::mutex::scoped_lock lock(storage_lock_);
00124 if (storage_.empty()) return ros::Time();
00125 return storage_.front().stamp_;
00126 };
00127
00129 ros::Time getOldestTimestamp()
00130 {
00131 boost::mutex::scoped_lock lock(storage_lock_);
00132 if (storage_.empty()) return ros::Time();
00133 return storage_.back().stamp_;
00134 };
00135 private:
00136 std::list<TransformStorage > storage_;
00137
00138 bool interpolating_;
00139 ros::Duration max_storage_time_;
00140 ros::Duration max_extrapolation_time_;
00141
00142 boost::mutex storage_lock_;
00143
00144
00145
00147
00148 uint8_t findClosest(TransformStorage& one, TransformStorage& two, ros::Time target_time, ExtrapolationMode& mode);
00149
00150 void pruneList()
00151 {
00152 ros::Time latest_time = storage_.begin()->stamp_;
00153
00154 while(!storage_.empty() && storage_.back().stamp_ + max_storage_time_ < latest_time)
00155 {
00156 storage_.pop_back();
00157 }
00158
00159 };
00160
00161
00162
00163 };
00164
00165
00166 }
00167 #endif // TF_TIME_CACHE_H