tf2  master
tf2 maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.
time_cache.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
32 #ifndef TF2_TIME_CACHE_H
33 #define TF2_TIME_CACHE_H
34 
35 #include "transform_storage.h"
36 
37 #include <memory>
38 #include <list>
39 #include <sstream>
40 
41 #include <tf2/visibility_control.h>
42 
43 namespace tf2
44 {
45 
47 
49 {
50 public:
52  virtual ~TimeCacheInterface() = default;
53 
56  virtual bool getData(TimePoint time, TransformStorage & data_out, std::string* error_str = 0)=0; //returns false if data unavailable (should be thrown as lookup exception
57 
60  virtual bool insertData(const TransformStorage& new_data)=0;
61 
64  virtual void clearList()=0;
65 
68  virtual CompactFrameID getParent(TimePoint time, std::string* error_str) = 0;
69 
75 
76 
78 
80  virtual unsigned int getListLength()=0;
81 
84  virtual TimePoint getLatestTimestamp()=0;
85 
88  virtual TimePoint getOldestTimestamp()=0;
89 };
90 
92 
94 
100 {
101  public:
102  TF2_PUBLIC
103  static const int MIN_INTERPOLATION_DISTANCE = 5;
104  TF2_PUBLIC
105  static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000;
106 
107  TF2_PUBLIC
109 
110 
112 
113  TF2_PUBLIC
114  virtual bool getData(TimePoint time, TransformStorage & data_out, std::string* error_str = 0);
115  TF2_PUBLIC
116  virtual bool insertData(const TransformStorage& new_data);
117  TF2_PUBLIC
118  virtual void clearList();
119  TF2_PUBLIC
120  virtual CompactFrameID getParent(TimePoint time, std::string* error_str);
121  TF2_PUBLIC
123 
125  TF2_PUBLIC
126  virtual unsigned int getListLength();
127  TF2_PUBLIC
128  virtual TimePoint getLatestTimestamp();
129  TF2_PUBLIC
130  virtual TimePoint getOldestTimestamp();
131 
132 
133 private:
135  L_TransformStorage storage_;
136 
137  tf2::Duration max_storage_time_;
138 
139 
141  //Assumes storage is already locked for it
142  inline uint8_t findClosest(TransformStorage*& one, TransformStorage*& two, TimePoint target_time, std::string* error_str);
143 
144  inline void interpolate(const TransformStorage& one, const TransformStorage& two, TimePoint time, TransformStorage& output);
145 
146 
147  void pruneList();
148 
149 
150 
151 };
152 
154 {
155  public:
157 
158  TF2_PUBLIC
159  virtual bool getData(TimePoint time, TransformStorage & data_out, std::string* error_str = 0); //returns false if data unavailable (should be thrown as lookup exception
160  TF2_PUBLIC
161  virtual bool insertData(const TransformStorage& new_data);
162  TF2_PUBLIC
163  virtual void clearList();
164  TF2_PUBLIC
165  virtual CompactFrameID getParent(TimePoint time, std::string* error_str);
166  TF2_PUBLIC
168 
169 
171  TF2_PUBLIC
172  virtual unsigned int getListLength();
173  TF2_PUBLIC
174  virtual TimePoint getLatestTimestamp();
175  TF2_PUBLIC
176  virtual TimePoint getOldestTimestamp();
177 
178 
179 private:
180  TransformStorage storage_;
181 };
182 
183 }
184 
185 #endif // TF2_TIME_CACHE_H
tf2::StaticCache::getLatestTimeAndParent
virtual P_TimeAndFrameID getLatestTimeAndParent()
Get the latest time stored in this cache, and the parent associated with it. Returns parent = 0 if no...
tf2::TimeCache::MIN_INTERPOLATION_DISTANCE
static const int MIN_INTERPOLATION_DISTANCE
Number of nano-seconds to not interpolate below.
Definition: time_cache.h:103
tf2::StaticCache::getParent
virtual CompactFrameID getParent(TimePoint time, std::string *error_str)
Retrieve the parent at a specific time.
std::string
std::shared_ptr
tf2::TimeCacheInterface::~TimeCacheInterface
virtual ~TimeCacheInterface()=default
std::list< TransformStorage >
tf2::TimeCacheInterface::getListLength
virtual unsigned int getListLength()=0
Debugging information methods.
std::pair
tf2::TimeCache::getData
virtual bool getData(TimePoint time, TransformStorage &data_out, std::string *error_str=0)
Virtual methods.
std::chrono::nanoseconds
tf2::TimeCache::getParent
virtual CompactFrameID getParent(TimePoint time, std::string *error_str)
Retrieve the parent at a specific time.
tf2::TimeCache::clearList
virtual void clearList()
Clear the list of stored values.
tf2::TimeCacheInterface::getParent
virtual CompactFrameID getParent(TimePoint time, std::string *error_str)=0
Retrieve the parent at a specific time.
tf2::StaticCache::insertData
virtual bool insertData(const TransformStorage &new_data)
Insert data into the cache.
tf2::TimeCache
A class to keep a sorted linked list in time This builds and maintains a list of timestamped data....
Definition: time_cache.h:99
tf2::TimeCache::getOldestTimestamp
virtual TimePoint getOldestTimestamp()
Get the oldest timestamp cached.
tf2::CompactFrameID
uint32_t CompactFrameID
Definition: transform_storage.h:46
tf2::TimeCache::MAX_LENGTH_LINKED_LIST
static const unsigned int MAX_LENGTH_LINKED_LIST
Maximum length of linked list, to make sure not to be able to use unlimited memory.
Definition: time_cache.h:105
tf2::TransformStorage
Storage for transforms and their parent.
Definition: transform_storage.h:49
tf2::TimeCacheInterface::getOldestTimestamp
virtual TimePoint getOldestTimestamp()=0
Get the oldest timestamp cached.
tf2::TimeCacheInterface::getData
virtual bool getData(TimePoint time, TransformStorage &data_out, std::string *error_str=0)=0
Access data from the cache.
tf2::TimeCache::getLatestTimeAndParent
virtual P_TimeAndFrameID getLatestTimeAndParent()
Get the latest time stored in this cache, and the parent associated with it. Returns parent = 0 if no...
tf2::StaticCache
Definition: time_cache.h:153
tf2::P_TimeAndFrameID
std::pair< TimePoint, CompactFrameID > P_TimeAndFrameID
Definition: buffer_core.h:58
tf2::TimeCache::insertData
virtual bool insertData(const TransformStorage &new_data)
Insert data into the cache.
tf2::TimeCacheInterface::clearList
virtual void clearList()=0
Clear the list of stored values.
tf2::StaticCache::getData
virtual bool getData(TimePoint time, TransformStorage &data_out, std::string *error_str=0)
Virtual methods.
tf2::TIMECACHE_DEFAULT_MAX_STORAGE_TIME
constexpr tf2::Duration TIMECACHE_DEFAULT_MAX_STORAGE_TIME
default value of 10 seconds storage
Definition: time_cache.h:93
std::chrono::time_point< std::chrono::system_clock, Duration >
transform_storage.h
tf2::TimeCacheInterface
Definition: time_cache.h:48
tf2::StaticCache::getLatestTimestamp
virtual TimePoint getLatestTimestamp()
Get the latest timestamp cached.
tf2::StaticCache::getOldestTimestamp
virtual TimePoint getOldestTimestamp()
Get the oldest timestamp cached.
tf2::TimeCacheInterface::getLatestTimestamp
virtual TimePoint getLatestTimestamp()=0
Get the latest timestamp cached.
visibility_control.h
tf2::TimeCache::getLatestTimestamp
virtual TimePoint getLatestTimestamp()
Get the latest timestamp cached.
tf2::StaticCache::clearList
virtual void clearList()
Clear the list of stored values.
tf2
Definition: buffer_core.h:55
tf2::TimeCache::TimeCache
TimeCache(tf2::Duration max_storage_time=TIMECACHE_DEFAULT_MAX_STORAGE_TIME)
TF2_PUBLIC
#define TF2_PUBLIC
Definition: visibility_control.h:58
tf2::TimeCacheInterface::insertData
virtual bool insertData(const TransformStorage &new_data)=0
Insert data into the cache.
tf2::TimeCache::getListLength
virtual unsigned int getListLength()
Debugging information methods.
tf2::TimeCacheInterface::getLatestTimeAndParent
virtual P_TimeAndFrameID getLatestTimeAndParent()=0
Get the latest time stored in this cache, and the parent associated with it. Returns parent = 0 if no...
tf2::StaticCache::getListLength
virtual unsigned int getListLength()
Debugging information methods.