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 <deque>
38 
39 #include <ros/message_forward.h>
40 #include <ros/time.h>
41 
42 #include <boost/shared_ptr.hpp>
43 
44 namespace geometry_msgs
45 {
46 ROS_DECLARE_MESSAGE(TransformStamped);
47 }
48 
49 namespace tf2
50 {
51 
52 typedef std::pair<ros::Time, CompactFrameID> P_TimeAndFrameID;
53 
55 {
56 public:
58  virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0)=0; //returns false if data unavailable (should be thrown as lookup exception
59 
61  virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0)=0;
62 
64  virtual void clearList()=0;
65 
67  virtual CompactFrameID getParent(ros::Time time, std::string* error_str) = 0;
68 
73 
74 
76 
77  virtual unsigned int getListLength()=0;
78 
80  virtual ros::Time getLatestTimestamp()=0;
81 
83  virtual ros::Time getOldestTimestamp()=0;
84 };
85 
87 
93 {
94  public:
95  static const int MIN_INTERPOLATION_DISTANCE = 5;
96  static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000;
97  static const int64_t DEFAULT_MAX_STORAGE_TIME = 10ULL * 1000000000LL;
98 
99  TimeCache(ros::Duration max_storage_time = ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME));
100 
101 
103 
104  virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0);
105  virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0);
106  virtual void clearList();
107  virtual CompactFrameID getParent(ros::Time time, std::string* error_str);
109 
111  virtual unsigned int getListLength();
112  virtual ros::Time getLatestTimestamp();
113  virtual ros::Time getOldestTimestamp();
114 
115 
116 private:
117  typedef std::deque<TransformStorage> L_TransformStorage;
119 
121 
122 
124  //Assumes storage is already locked for it
125  inline uint8_t findClosest(TransformStorage*& one, TransformStorage*& two, ros::Time target_time, std::string* error_str);
126 
127  inline void interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output);
128 
129 
130  void pruneList();
131 
132 
133 
134 };
135 
137 {
138  public:
140 
141  virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0); //returns false if data unavailable (should be thrown as lookup exception
142  virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0);
143  virtual void clearList();
144  virtual CompactFrameID getParent(ros::Time time, std::string* error_str);
146 
147 
149  virtual unsigned int getListLength();
150  virtual ros::Time getLatestTimestamp();
151  virtual ros::Time getOldestTimestamp();
152 
153 
154 private:
156 };
157 
158 }
159 
160 #endif // TF2_TIME_CACHE_H
tf2::TimeCache::max_storage_time_
ros::Duration max_storage_time_
Definition: time_cache.h:120
tf2::TimeCache::MIN_INTERPOLATION_DISTANCE
static const int MIN_INTERPOLATION_DISTANCE
Number of nano-seconds to not interpolate below.
Definition: time_cache.h:95
tf2::TimeCache::getOldestTimestamp
virtual ros::Time getOldestTimestamp()
Get the oldest timestamp cached.
Definition: cache.cpp:322
tf2::StaticCache::storage_
TransformStorage storage_
Definition: time_cache.h:155
tf2::StaticCache::insertData
virtual bool insertData(const TransformStorage &new_data, std::string *error_str=0)
Insert data into the cache.
Definition: static_cache.cpp:48
boost::shared_ptr
tf2::TimeCacheInterface::getListLength
virtual unsigned int getListLength()=0
Debugging information methods.
tf2::StaticCache::getListLength
virtual unsigned int getListLength()
Debugging information methods.
Definition: static_cache.cpp:59
tf2::TimeCacheInterfacePtr
boost::shared_ptr< TimeCacheInterface > TimeCacheInterfacePtr
Definition: buffer_core.h:61
time.h
geometry_msgs
Definition: time_cache.h:44
tf2::TimeCache::L_TransformStorage
std::deque< TransformStorage > L_TransformStorage
Definition: time_cache.h:117
tf2::TimeCache::insertData
virtual bool insertData(const TransformStorage &new_data, std::string *error_str=0)
Insert data into the cache.
Definition: cache.cpp:255
tf2::TimeCacheInterface::getParent
virtual CompactFrameID getParent(ros::Time time, std::string *error_str)=0
Retrieve the parent at a specific time.
tf2::TimeCache::clearList
virtual void clearList()
Clear the list of stored values.
Definition: cache.cpp:295
tf2::StaticCache::getData
virtual bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
Virtual methods.
Definition: static_cache.cpp:41
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:92
tf2::TimeCache::pruneList
void pruneList()
Definition: cache.cpp:328
tf2::CompactFrameID
uint32_t CompactFrameID
Definition: transform_storage.h:50
tf2::TimeCache::getParent
virtual CompactFrameID getParent(ros::Time time, std::string *error_str)
Retrieve the parent at a specific time.
Definition: cache.cpp:241
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:96
tf2::TransformStorage
Storage for transforms and their parent.
Definition: transform_storage.h:53
tf2::StaticCache::getLatestTimestamp
virtual ros::Time getLatestTimestamp()
Get the latest timestamp cached.
Definition: static_cache.cpp:71
tf2::TimeCache::interpolate
void interpolate(const TransformStorage &one, const TransformStorage &two, ros::Time time, TransformStorage &output)
Definition: cache.cpp:186
tf2::TimeCache::DEFAULT_MAX_STORAGE_TIME
static const int64_t DEFAULT_MAX_STORAGE_TIME
default value of 10 seconds storage
Definition: time_cache.h:97
tf2::TimeCacheInterface::insertData
virtual bool insertData(const TransformStorage &new_data, std::string *error_str=0)=0
Insert data into the cache.
tf2::TimeCacheInterface::getOldestTimestamp
virtual ros::Time getOldestTimestamp()=0
Get the oldest timestamp cached.
tf2::TimeCache::getListLength
virtual unsigned int getListLength()
Debugging information methods.
Definition: cache.cpp:300
message_forward.h
tf2::StaticCache
Definition: time_cache.h:136
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...
Definition: static_cache.cpp:66
tf2::TimeCache::findClosest
uint8_t findClosest(TransformStorage *&one, TransformStorage *&two, ros::Time target_time, std::string *error_str)
A helper function for getData.
Definition: cache.cpp:111
tf2::TimeCache::storage_
L_TransformStorage storage_
Definition: time_cache.h:118
tf2::TimeCacheInterface::clearList
virtual void clearList()=0
Clear the list of stored values.
transform_storage.h
geometry_msgs::ROS_DECLARE_MESSAGE
ROS_DECLARE_MESSAGE(TransformStamped)
tf2::TimeCacheInterface::getLatestTimestamp
virtual ros::Time getLatestTimestamp()=0
Get the latest timestamp cached.
tf2::TimeCacheInterface
Definition: time_cache.h:54
tf2::TimeCache::getData
virtual bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
Virtual methods.
Definition: cache.cpp:208
tf2::StaticCache::getParent
virtual CompactFrameID getParent(ros::Time time, std::string *error_str)
Retrieve the parent at a specific time.
Definition: static_cache.cpp:61
tf2::P_TimeAndFrameID
std::pair< ros::Time, CompactFrameID > P_TimeAndFrameID
Definition: buffer_core.h:57
tf2::TimeCache::getLatestTimestamp
virtual ros::Time getLatestTimestamp()
Get the latest timestamp cached.
Definition: cache.cpp:316
ros::Time
tf2::TimeCacheInterface::getData
virtual bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)=0
Access data from the cache.
tf2
Definition: buffer_core.h:54
tf2::TimeCache::TimeCache
TimeCache(ros::Duration max_storage_time=ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME))
Definition: cache.cpp:59
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::clearList
virtual void clearList()
Clear the list of stored values.
Definition: static_cache.cpp:57
tf2::StaticCache::getOldestTimestamp
virtual ros::Time getOldestTimestamp()
Get the oldest timestamp cached.
Definition: static_cache.cpp:76
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...
Definition: cache.cpp:305
ros::Duration


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Sun Feb 4 2024 03:18:11