cache.cpp
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 #include "tf/time_cache.h"
33 #include "tf/exceptions.h"
34 
36 #include <geometry_msgs/TransformStamped.h>
37 
38 #include "ros/assert.h"
39 
40 using namespace tf;
41 
43 {
44 }
45 
47  CompactFrameID child_frame_id)
48 : rotation_(data.getRotation())
49 , translation_(data.getOrigin())
50 , stamp_(data.stamp_)
51 , frame_id_(frame_id)
52 , child_frame_id_(child_frame_id)
53 { }
54 
56 : max_storage_time_(max_storage_time)
57 {}
58 
59 // hoisting these into separate functions causes an ~8% speedup. Removing calling them altogether adds another ~10%
60 void createEmptyException(std::string *error_str)
61 {
62  if (error_str)
63  {
64  *error_str = "Unable to lookup transform, cache is empty";
65  }
66 }
67 
68 void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string* error_str)
69 {
70  if (error_str)
71  {
72  std::stringstream ss;
73  ss << "Lookup would require extrapolation at time " << t0 << ", but only time " << t1 << " is in the buffer";
74  *error_str = ss.str();
75  }
76 }
77 
78 void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string* error_str)
79 {
80  if (error_str)
81  {
82  std::stringstream ss;
83  ss << "Lookup would require extrapolation into the future. Requested time " << t0 << " but the latest data is at time " << t1;
84  *error_str = ss.str();
85  }
86 }
87 
88 void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string* error_str)
89 {
90  if (error_str)
91  {
92  std::stringstream ss;
93  ss << "Lookup would require extrapolation into the past. Requested time " << t0 << " but the earliest data is at time " << t1;
94  *error_str = ss.str();
95  }
96 }
97 
98 uint8_t TimeCache::findClosest(const TransformStorage*& one, const TransformStorage*& two, ros::Time target_time, std::string* error_str)
99 {
100  //No values stored
101  if (storage_.empty())
102  {
103  createEmptyException(error_str);
104  return 0;
105  }
106 
107  //If time == 0 return the latest
108  if (target_time.isZero())
109  {
110  one = &(*storage_.rbegin());
111  return 1;
112  }
113 
114  // One value stored
115  if (++storage_.begin() == storage_.end())
116  {
117  const TransformStorage& ts = *storage_.begin();
118  if (ts.stamp_ == target_time)
119  {
120  one = &ts;
121  return 1;
122  }
123  else
124  {
125  createExtrapolationException1(target_time, ts.stamp_, error_str);
126  return 0;
127  }
128  }
129 
130  ros::Time latest_time = (*storage_.rbegin()).stamp_;
131  ros::Time earliest_time = (*(storage_.begin())).stamp_;
132 
133  if (target_time == latest_time)
134  {
135  one = &(*storage_.rbegin());
136  return 1;
137  }
138  else if (target_time == earliest_time)
139  {
140  one = &(*storage_.begin());
141  return 1;
142  }
143  // Catch cases that would require extrapolation
144  else if (target_time > latest_time)
145  {
146  createExtrapolationException2(target_time, latest_time, error_str);
147  return 0;
148  }
149  else if (target_time < earliest_time)
150  {
151  createExtrapolationException3(target_time, earliest_time, error_str);
152  return 0;
153  }
154 
155  //Create a temporary object to compare to when searching the lower bound via std::set
156  TransformStorage tmp;
157  tmp.stamp_ = target_time;
158 
159  //Find the first value equal or higher than the target value
160  L_TransformStorage::iterator storage_it = storage_.upper_bound(tmp);
161 
162  //Finally the case were somewhere in the middle Guarenteed no extrapolation :-)
163  two = &*(storage_it); //Newer
164  one = &*(--storage_it); //Older
165 
166  return 2;
167 
168 }
169 
171 {
172  // Check for zero distance case
173  if( two.stamp_ == one.stamp_ )
174  {
175  output = two;
176  return;
177  }
178  //Calculate the ratio
179  tfScalar ratio = (time.toSec() - one.stamp_.toSec()) / (two.stamp_.toSec() - one.stamp_.toSec());
180 
181  //Interpolate translation
182  output.translation_.setInterpolate3(one.translation_, two.translation_, ratio);
183 
184  //Interpolate rotation
185  output.rotation_ = slerp( one.rotation_, two.rotation_, ratio);
186 
187  output.stamp_ = one.stamp_;
188  output.frame_id_ = one.frame_id_;
189  output.child_frame_id_ = one.child_frame_id_;
190 }
191 
192 bool TimeCache::getData(ros::Time time, TransformStorage & data_out, std::string* error_str) //returns false if data not available
193 {
194  const TransformStorage* p_temp_1 = NULL;
195  const TransformStorage* p_temp_2 = NULL;
196 
197  int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str);
198  if (num_nodes == 0)
199  {
200  return false;
201  }
202  else if (num_nodes == 1)
203  {
204  data_out = *p_temp_1;
205  }
206  else if (num_nodes == 2)
207  {
208  if( p_temp_1->frame_id_ == p_temp_2->frame_id_)
209  {
210  interpolate(*p_temp_1, *p_temp_2, time, data_out);
211  }
212  else
213  {
214  data_out = *p_temp_1;
215  }
216  }
217  else
218  {
219  ROS_BREAK();
220  }
221 
222  return true;
223 }
224 
225 CompactFrameID TimeCache::getParent(ros::Time time, std::string* error_str)
226 {
227  const TransformStorage* p_temp_1 = NULL;
228  const TransformStorage* p_temp_2 = NULL;
229 
230  int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str);
231  if (num_nodes == 0)
232  {
233  return 0;
234  }
235 
236  return p_temp_1->frame_id_;
237 }
238 
240 {
241 
242  if (storage_.begin() != storage_.end())
243  {
244  // trying to add data that dates back longer than we want to keep history
245  if (storage_.rbegin()->stamp_ > new_data.stamp_ + max_storage_time_)
246  return false;
247 
248  // if we already have data at that exact time, delete it to ensure the latest data is stored
249  if (storage_.rbegin()->stamp_ >= new_data.stamp_)
250  {
251  L_TransformStorage::iterator storage_it = storage_.find(new_data);
252  if (storage_it != storage_.end())
253  storage_.erase(storage_it);
254  }
255  }
256 
257  storage_.insert(storage_.end(), new_data);
258 
259  pruneList();
260 
261  return true;
262 }
263 
265 {
266  storage_.clear();
267 }
268 
270 {
271  return storage_.size();
272 }
273 
275 {
276  if (storage_.empty())
277  {
278  return std::make_pair(ros::Time(), 0);
279  }
280 
281  const TransformStorage& ts = *storage_.rbegin();
282  return std::make_pair(ts.stamp_, ts.frame_id_);
283 }
284 
286 {
287  if (storage_.empty()) return ros::Time(); //empty list case
288  return storage_.rbegin()->stamp_;
289 }
290 
292 {
293  if (storage_.empty()) return ros::Time(); //empty list case
294  return storage_.begin()->stamp_;
295 }
296 
298 {
299  ros::Time latest_time = storage_.rbegin()->stamp_;
300 
301  while(!storage_.empty() && storage_.begin()->stamp_ + max_storage_time_ < latest_time)
302  {
303  storage_.erase(storage_.begin());
304  }
305 
306 }
ros::Duration max_storage_time_
Definition: time_cache.h:122
double tfScalar
The tfScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: Scalar.h:160
TimeCache(ros::Duration max_storage_time=ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME))
Definition: cache.cpp:55
unsigned int getListLength()
Debugging information methods.
Definition: cache.cpp:269
CompactFrameID frame_id_
Definition: time_cache.h:86
void clearList()
Definition: cache.cpp:264
void pruneList()
Definition: cache.cpp:297
void interpolate(const TransformStorage &one, const TransformStorage &two, ros::Time time, TransformStorage &output)
Definition: cache.cpp:170
Definition: exceptions.h:38
ros::Time getLatestTimestamp()
Definition: cache.cpp:285
TFSIMD_FORCE_INLINE Quaternion slerp(const Quaternion &q1, const Quaternion &q2, const tfScalar &t)
Return the result of spherical linear interpolation betwen two quaternions.
Definition: Quaternion.h:430
tf::Vector3 translation_
Definition: time_cache.h:84
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
void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string *error_str)
Definition: cache.cpp:88
void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string *error_str)
Definition: cache.cpp:68
TFSIMD_FORCE_INLINE void setInterpolate3(const Vector3 &v0, const Vector3 &v1, tfScalar rt)
Definition: Vector3.h:220
std::pair< ros::Time, CompactFrameID > P_TimeAndFrameID
Definition: time_cache.h:51
bool insertData(const TransformStorage &new_data)
Definition: cache.cpp:239
void createEmptyException(std::string *error_str)
Definition: cache.cpp:60
CompactFrameID getParent(ros::Time time, std::string *error_str)
Definition: cache.cpp:225
bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
Definition: cache.cpp:192
P_TimeAndFrameID getLatestTimeAndParent()
Definition: cache.cpp:274
L_TransformStorage storage_
Definition: time_cache.h:120
void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string *error_str)
Definition: cache.cpp:78
Storage for transforms and their parent.
Definition: time_cache.h:54
tf::Quaternion rotation_
Definition: time_cache.h:83
ros::Time getOldestTimestamp()
Definition: cache.cpp:291
CompactFrameID child_frame_id_
Definition: time_cache.h:87
#define ROS_BREAK()
uint32_t CompactFrameID
Definition: time_cache.h:50
The Stamped Transform datatype used by tf.


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Tue Jan 2 2018 03:17:40