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 
86  CompactFrameID frame_id_;
87  CompactFrameID child_frame_id_;
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);
110  P_TimeAndFrameID getLatestTimeAndParent();
111 
113  unsigned int getListLength();
114  ros::Time getLatestTimestamp();
115  ros::Time getOldestTimestamp();
116 
117 
118 private:
119  typedef std::set<TransformStorage> L_TransformStorage;
120  L_TransformStorage storage_;
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
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
ros::Duration max_storage_time_
Definition: time_cache.h:122
TransformStorage(const TransformStorage &rhs)
Definition: time_cache.h:60
CompactFrameID frame_id_
Definition: time_cache.h:86
ExtrapolationMode
Definition: time_cache.h:47
std::set< TransformStorage > L_TransformStorage
Definition: time_cache.h:119
A class to keep a sorted linked list in time This builds and maintains a list of timestamped data...
Definition: time_cache.h:97
Definition: exceptions.h:38
tf::Vector3 translation_
Definition: time_cache.h:84
std::pair< ros::Time, CompactFrameID > P_TimeAndFrameID
Definition: time_cache.h:51
bool operator<(const TransformStorage &b) const
Definition: time_cache.h:77
L_TransformStorage storage_
Definition: time_cache.h:120
Storage for transforms and their parent.
Definition: time_cache.h:54
tf::Quaternion rotation_
Definition: time_cache.h:83
CompactFrameID child_frame_id_
Definition: time_cache.h:87
TransformStorage & operator=(const TransformStorage &rhs)
Definition: time_cache.h:65
uint32_t CompactFrameID
Definition: time_cache.h:50
The Stamped Transform datatype used by tf.
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
Definition: Vector3.h:38


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Jun 7 2019 22:00:28