Go to the documentation of this file.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 TF2_TIME_CACHE_H
00033 #define TF2_TIME_CACHE_H
00034
00035 #include "transform_storage.h"
00036
00037 #include <list>
00038
00039 #include <sstream>
00040
00041 #include <ros/message_forward.h>
00042 #include <ros/time.h>
00043
00044 #include <boost/shared_ptr.hpp>
00045
00046 namespace geometry_msgs
00047 {
00048 ROS_DECLARE_MESSAGE(TransformStamped);
00049 }
00050
00051 namespace tf2
00052 {
00053
00054 typedef std::pair<ros::Time, CompactFrameID> P_TimeAndFrameID;
00055
00056 class TimeCacheInterface
00057 {
00058 public:
00060 virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0)=0;
00061
00063 virtual bool insertData(const TransformStorage& new_data)=0;
00064
00066 virtual void clearList()=0;
00067
00069 virtual CompactFrameID getParent(ros::Time time, std::string* error_str) = 0;
00070
00074 virtual P_TimeAndFrameID getLatestTimeAndParent() = 0;
00075
00076
00078
00079 virtual unsigned int getListLength()=0;
00080
00082 virtual ros::Time getLatestTimestamp()=0;
00083
00085 virtual ros::Time getOldestTimestamp()=0;
00086 };
00087
00088 typedef boost::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
00089
00094 class TimeCache : public TimeCacheInterface
00095 {
00096 public:
00097 static const int MIN_INTERPOLATION_DISTANCE = 5;
00098 static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000;
00099 static const int64_t DEFAULT_MAX_STORAGE_TIME = 1ULL * 1000000000LL;
00100
00101 TimeCache(ros::Duration max_storage_time = ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME));
00102
00103
00105
00106 virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0);
00107 virtual bool insertData(const TransformStorage& new_data);
00108 virtual void clearList();
00109 virtual CompactFrameID getParent(ros::Time time, std::string* error_str);
00110 virtual P_TimeAndFrameID getLatestTimeAndParent();
00111
00113 virtual unsigned int getListLength();
00114 virtual ros::Time getLatestTimestamp();
00115 virtual ros::Time getOldestTimestamp();
00116
00117
00118 private:
00119 typedef std::list<TransformStorage> L_TransformStorage;
00120 L_TransformStorage storage_;
00121
00122 ros::Duration max_storage_time_;
00123
00124
00126
00127 inline uint8_t findClosest(TransformStorage*& one, TransformStorage*& two, ros::Time target_time, std::string* error_str);
00128
00129 inline void interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output);
00130
00131
00132 void pruneList();
00133
00134
00135
00136 };
00137
00138 class StaticCache : public TimeCacheInterface
00139 {
00140 public:
00142
00143 virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0);
00144 virtual bool insertData(const TransformStorage& new_data);
00145 virtual void clearList();
00146 virtual CompactFrameID getParent(ros::Time time, std::string* error_str);
00147 virtual P_TimeAndFrameID getLatestTimeAndParent();
00148
00149
00151 virtual unsigned int getListLength();
00152 virtual ros::Time getLatestTimestamp();
00153 virtual ros::Time getOldestTimestamp();
00154
00155
00156 private:
00157 TransformStorage storage_;
00158 };
00159
00160 }
00161
00162 #endif // TF2_TIME_CACHE_H