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 TF_TIME_CACHE_H
33 #define TF_TIME_CACHE_H
34 
35 #include <set>
36 #include <boost/thread/mutex.hpp>
37 
38 #include "tf/transform_datatypes.h"
39 #include "tf/exceptions.h"
40 
42 
43 #include <sstream>
44 
45 namespace tf
46 {
48 
49 
50 typedef uint32_t CompactFrameID;
51 typedef std::pair<ros::Time, CompactFrameID> P_TimeAndFrameID;
52 
55 {
56 public:
58  TransformStorage(const StampedTransform& data, CompactFrameID frame_id, CompactFrameID child_frame_id);
59 
61  {
62  *this = rhs;
63  }
64 
66  {
67 #if 01
68  rotation_ = rhs.rotation_;
70  stamp_ = rhs.stamp_;
71  frame_id_ = rhs.frame_id_;
73 #endif
74  return *this;
75  }
76 
77  bool operator< (const TransformStorage &b) const
78  {
79  return this->stamp_ < b.stamp_;
80  }
81 
82 
84  tf::Vector3 translation_;
88 };
89 
90 
91 
92 
97 class TimeCache
98 {
99  public:
100  static const int MIN_INTERPOLATION_DISTANCE = 5;
101  static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000;
102  static const int64_t DEFAULT_MAX_STORAGE_TIME = 1ULL * 1000000000LL;
103 
104  TimeCache(ros::Duration max_storage_time = ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME));
105 
106  bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0);
107  bool insertData(const TransformStorage& new_data);
108  void clearList();
109  CompactFrameID getParent(ros::Time time, std::string* error_str);
111 
113  unsigned int getListLength();
116 
117 
118 private:
119  typedef std::set<TransformStorage> L_TransformStorage;
121 
123 
124 
126  //Assumes storage is already locked for it
127  inline uint8_t findClosest(const TransformStorage*& one, const TransformStorage*& two, ros::Time target_time, std::string* error_str);
128 
129  inline void interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output);
130 
131 
132  void pruneList();
133 
134 
135 
136 };
137 
138 
139 }
140 #endif // TF_TIME_CACHE_H
tf::TimeCache::max_storage_time_
ros::Duration max_storage_time_
Definition: time_cache.h:122
tf::P_TimeAndFrameID
std::pair< ros::Time, CompactFrameID > P_TimeAndFrameID
Definition: time_cache.h:51
tf::TimeCache::storage_
L_TransformStorage storage_
Definition: time_cache.h:120
exceptions.h
tf::EXTRAPOLATE_FORWARD
@ EXTRAPOLATE_FORWARD
Definition: time_cache.h:47
tf::TimeCache::MIN_INTERPOLATION_DISTANCE
static const int MIN_INTERPOLATION_DISTANCE
Number of nano-seconds to not interpolate below.
Definition: time_cache.h:100
tf::TimeCache::getParent
CompactFrameID getParent(ros::Time time, std::string *error_str)
Definition: cache.cpp:225
tf::TimeCache::pruneList
void pruneList()
Definition: cache.cpp:297
Transform.h
tf::TransformStorage
Storage for transforms and their parent.
Definition: time_cache.h:54
tf::TransformStorage::operator=
TransformStorage & operator=(const TransformStorage &rhs)
Definition: time_cache.h:65
tf::TimeCache::getListLength
unsigned int getListLength()
Debugging information methods.
Definition: cache.cpp:269
tf::TimeCache::getLatestTimeAndParent
P_TimeAndFrameID getLatestTimeAndParent()
Definition: cache.cpp:274
tf::TransformStorage::operator<
bool operator<(const TransformStorage &b) const
Definition: time_cache.h:77
tf::TransformStorage::rotation_
tf::Quaternion rotation_
Definition: time_cache.h:83
tf::TransformStorage::child_frame_id_
CompactFrameID child_frame_id_
Definition: time_cache.h:87
tf::StampedTransform
The Stamped Transform datatype used by tf.
Definition: transform_datatypes.h:81
tf::TimeCache::getOldestTimestamp
ros::Time getOldestTimestamp()
Definition: cache.cpp:291
tf::ExtrapolationMode
ExtrapolationMode
Definition: time_cache.h:47
tf::TimeCache::L_TransformStorage
std::set< TransformStorage > L_TransformStorage
Definition: time_cache.h:119
tf::TimeCache::interpolate
void interpolate(const TransformStorage &one, const TransformStorage &two, ros::Time time, TransformStorage &output)
Definition: cache.cpp:170
tf::EXTRAPOLATE_BACK
@ EXTRAPOLATE_BACK
Definition: time_cache.h:47
tf::CompactFrameID
uint32_t CompactFrameID
Definition: time_cache.h:50
tf::TimeCache
A class to keep a sorted linked list in time This builds and maintains a list of timestamped data....
Definition: time_cache.h:97
tf::TransformStorage::stamp_
ros::Time stamp_
Definition: time_cache.h:85
tf::TimeCache::getData
bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
Definition: cache.cpp:192
tf::TransformStorage::TransformStorage
TransformStorage(const TransformStorage &rhs)
Definition: time_cache.h:60
tf::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:101
tf::TimeCache::insertData
bool insertData(const TransformStorage &new_data)
Definition: cache.cpp:239
tf::TransformStorage::frame_id_
CompactFrameID frame_id_
Definition: time_cache.h:86
tf::TimeCache::TimeCache
TimeCache(ros::Duration max_storage_time=ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME))
Definition: cache.cpp:55
tf::TimeCache::findClosest
uint8_t findClosest(const TransformStorage *&one, const TransformStorage *&two, ros::Time target_time, std::string *error_str)
A helper function for getData.
Definition: cache.cpp:98
tf::TimeCache::clearList
void clearList()
Definition: cache.cpp:264
ros::Time
tf::TransformStorage::translation_
tf::Vector3 translation_
Definition: time_cache.h:84
tf::TimeCache::getLatestTimestamp
ros::Time getLatestTimestamp()
Definition: cache.cpp:285
tf
Definition: exceptions.h:38
tf::TimeCache::DEFAULT_MAX_STORAGE_TIME
static const int64_t DEFAULT_MAX_STORAGE_TIME
default value of 10 seconds storage
Definition: time_cache.h:102
transform_datatypes.h
tf::INTERPOLATE
@ INTERPOLATE
Definition: time_cache.h:47
tf::ONE_VALUE
@ ONE_VALUE
Definition: time_cache.h:47
ros::Duration
tf::TransformStorage::TransformStorage
TransformStorage()
Definition: cache.cpp:42
tf::Quaternion
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Sun Aug 16 2020 03:38:53