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 "tf2/time_cache.h"
33 #include "tf2/exceptions.h"
34 
35 #include <tf2/LinearMath/Vector3.h>
38 #include <geometry_msgs/TransformStamped.h>
39 #include <assert.h>
40 
41 namespace tf2 {
42 
44 {
45 }
46 
47 TransformStorage::TransformStorage(const geometry_msgs::TransformStamped& data, CompactFrameID frame_id,
48  CompactFrameID child_frame_id)
49 : stamp_(data.header.stamp)
50 , frame_id_(frame_id)
51 , child_frame_id_(child_frame_id)
52 {
53  const geometry_msgs::Quaternion& o = data.transform.rotation;
54  rotation_ = tf2::Quaternion(o.x, o.y, o.z, o.w);
55  const geometry_msgs::Vector3& v = data.transform.translation;
56  translation_ = tf2::Vector3(v.x, v.y, v.z);
57 }
58 
60 : max_storage_time_(max_storage_time)
61 {}
62 
63 namespace cache { // Avoid ODR collisions https://github.com/ros/geometry2/issues/175
64 // hoisting these into separate functions causes an ~8% speedup. Removing calling them altogether adds another ~10%
65 void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string* error_str)
66 {
67  if (error_str)
68  {
69  char str[116]; // Text without formatting strings has 76, each timestamp has up to 20
70  snprintf(str, sizeof(str), "Lookup would require extrapolation at time %.09f, but only time %.09f is in the buffer", t0.toSec(), t1.toSec());
71  *error_str = str;
72  }
73 }
74 
75 void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string* error_str)
76 {
77  if (error_str)
78  {
79  ros::Duration tdiff = t1 - t0;
80  char str[163]; // Text without formatting strings has 102, each timestamp has up to 20
81  snprintf(
82  str, sizeof(str),
83  "Lookup would require extrapolation %.09fs into the future. Requested time %.09f but the latest data is at time %.09f",
84  tdiff.toSec(), t0.toSec(), t1.toSec());
85  *error_str = str;
86  }
87 }
88 
89 void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string* error_str)
90 {
91  if (error_str)
92  {
93  ros::Duration tdiff = t1 - t0;
94  char str[163]; // Text without formatting strings has 102, each timestamp has up to 20
95  snprintf(
96  str, sizeof(str),
97  "Lookup would require extrapolation %.09fs into the past. Requested time %.09f but the earliest data is at time %.09f",
98  tdiff.toSec(), t0.toSec(), t1.toSec());
99  *error_str = str;
100  }
101 }
102 } // namespace cache
103 
104 bool operator>(const TransformStorage& lhs, const TransformStorage& rhs)
105 {
106  return lhs.stamp_ > rhs.stamp_;
107 }
108 
109 uint8_t TimeCache::findClosest(TransformStorage*& one, TransformStorage*& two, ros::Time target_time, std::string* error_str)
110 {
111  //No values stored
112  if (storage_.empty())
113  {
114  return 0;
115  }
116 
117  //If time == 0 return the latest
118  if (target_time.isZero())
119  {
120  one = &storage_.front();
121  return 1;
122  }
123 
124  // One value stored
125  if (++storage_.begin() == storage_.end())
126  {
127  TransformStorage& ts = *storage_.begin();
128  if (ts.stamp_ == target_time)
129  {
130  one = &ts;
131  return 1;
132  }
133  else
134  {
135  cache::createExtrapolationException1(target_time, ts.stamp_, error_str);
136  return 0;
137  }
138  }
139 
140  ros::Time latest_time = (*storage_.begin()).stamp_;
141  ros::Time earliest_time = (*(storage_.rbegin())).stamp_;
142 
143  if (target_time == latest_time)
144  {
145  one = &(*storage_.begin());
146  return 1;
147  }
148  else if (target_time == earliest_time)
149  {
150  one = &(*storage_.rbegin());
151  return 1;
152  }
153  // Catch cases that would require extrapolation
154  else if (target_time > latest_time)
155  {
156  cache::createExtrapolationException2(target_time, latest_time, error_str);
157  return 0;
158  }
159  else if (target_time < earliest_time)
160  {
161  cache::createExtrapolationException3(target_time, earliest_time, error_str);
162  return 0;
163  }
164 
165  //At least 2 values stored
166  //Find the first value less than the target value
167  L_TransformStorage::iterator storage_it;
168  TransformStorage storage_target_time;
169  storage_target_time.stamp_ = target_time;
170 
171  storage_it = std::lower_bound(
172  storage_.begin(),
173  storage_.end(),
174  storage_target_time, std::greater<TransformStorage>());
175 
176  //Finally the case were somewhere in the middle Guarenteed no extrapolation :-)
177  one = &*(storage_it); //Older
178  two = &*(--storage_it); //Newer
179  return 2;
180 
181 
182 }
183 
185 {
186  // Check for zero distance case
187  if( two.stamp_ == one.stamp_ )
188  {
189  output = two;
190  return;
191  }
192  //Calculate the ratio
193  tf2Scalar ratio = (time - one.stamp_).toSec() / (two.stamp_ - one.stamp_).toSec();
194 
195  //Interpolate translation
196  output.translation_.setInterpolate3(one.translation_, two.translation_, ratio);
197 
198  //Interpolate rotation
199  output.rotation_ = slerp( one.rotation_, two.rotation_, ratio);
200 
201  output.stamp_ = time;
202  output.frame_id_ = one.frame_id_;
203  output.child_frame_id_ = one.child_frame_id_;
204 }
205 
206 bool TimeCache::getData(ros::Time time, TransformStorage & data_out, std::string* error_str) //returns false if data not available
207 {
208  TransformStorage* p_temp_1;
209  TransformStorage* p_temp_2;
210 
211  int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str);
212  if (num_nodes == 0)
213  {
214  return false;
215  }
216  else if (num_nodes == 1)
217  {
218  data_out = *p_temp_1;
219  }
220  else if (num_nodes == 2)
221  {
222  if( p_temp_1->frame_id_ == p_temp_2->frame_id_)
223  {
224  interpolate(*p_temp_1, *p_temp_2, time, data_out);
225  }
226  else
227  {
228  data_out = *p_temp_1;
229  }
230  }
231  else
232  {
233  assert(0);
234  }
235 
236  return true;
237 }
238 
239 CompactFrameID TimeCache::getParent(ros::Time time, std::string* error_str)
240 {
241  TransformStorage* p_temp_1;
242  TransformStorage* p_temp_2;
243 
244  int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str);
245  if (num_nodes == 0)
246  {
247  return 0;
248  }
249 
250  return p_temp_1->frame_id_;
251 }
252 
253 bool TimeCache::insertData(const TransformStorage& new_data, std::string* error_str)
254 {
255  L_TransformStorage::iterator storage_it = storage_.begin();
256 
257  if(storage_it != storage_.end())
258  {
259  if (storage_it->stamp_ > new_data.stamp_ + max_storage_time_)
260  {
261  if (error_str)
262  {
263  *error_str = "TF_OLD_DATA ignoring data from the past (Possible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained)";
264  }
265  return false;
266  }
267  }
268 
269 
270  while(storage_it != storage_.end())
271  {
272  if (storage_it->stamp_ <= new_data.stamp_)
273  break;
274  storage_it++;
275  }
276  if (storage_it != storage_.end() && storage_it->stamp_ == new_data.stamp_)
277  {
278  if (error_str)
279  {
280  *error_str = "TF_REPEATED_DATA ignoring data with redundant timestamp";
281  }
282  return false;
283  }
284  else
285  {
286  storage_.insert(storage_it, new_data);
287  }
288 
289  pruneList();
290  return true;
291 }
292 
294 {
295  storage_.clear();
296 }
297 
299 {
300  return storage_.size();
301 }
302 
304 {
305  if (storage_.empty())
306  {
307  return std::make_pair(ros::Time(), 0);
308  }
309 
310  const TransformStorage& ts = storage_.front();
311  return std::make_pair(ts.stamp_, ts.frame_id_);
312 }
313 
315 {
316  if (storage_.empty()) return ros::Time(); //empty list case
317  return storage_.front().stamp_;
318 }
319 
321 {
322  if (storage_.empty()) return ros::Time(); //empty list case
323  return storage_.back().stamp_;
324 }
325 
327 {
328  ros::Time latest_time = storage_.begin()->stamp_;
329 
330  while(!storage_.empty() && storage_.back().stamp_ + max_storage_time_ < latest_time)
331  {
332  storage_.pop_back();
333  }
334 
335 } // namespace tf2
336 }
tf2::TimeCache::max_storage_time_
ros::Duration max_storage_time_
Definition: time_cache.h:120
tf2::TransformStorage::TransformStorage
TransformStorage()
Definition: cache.cpp:43
tf2::TimeCache::getOldestTimestamp
virtual ros::Time getOldestTimestamp()
Get the oldest timestamp cached.
Definition: cache.cpp:320
exceptions.h
Transform.h
TimeBase< Time, Duration >::toSec
double toSec() const
tf2::TransformStorage::child_frame_id_
CompactFrameID child_frame_id_
Definition: transform_storage.h:80
tf2::TimeCache::insertData
virtual bool insertData(const TransformStorage &new_data, std::string *error_str=0)
Insert data into the cache.
Definition: cache.cpp:253
tf2::TimeCache::clearList
virtual void clearList()
Clear the list of stored values.
Definition: cache.cpp:293
tf2::TimeCache::pruneList
void pruneList()
Definition: cache.cpp:326
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:239
tf2::TransformStorage::rotation_
tf2::Quaternion rotation_
Definition: transform_storage.h:76
tf2::TransformStorage
Storage for transforms and their parent.
Definition: transform_storage.h:53
tf2::TimeCache::interpolate
void interpolate(const TransformStorage &one, const TransformStorage &two, ros::Time time, TransformStorage &output)
Definition: cache.cpp:184
tf2::TimeCache::getListLength
virtual unsigned int getListLength()
Debugging information methods.
Definition: cache.cpp:298
TimeBase< Time, Duration >::isZero
bool isZero() const
tf2::operator>
bool operator>(const TransformStorage &lhs, const TransformStorage &rhs)
Definition: cache.cpp:104
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:109
tf2::TimeCache::storage_
L_TransformStorage storage_
Definition: time_cache.h:118
tf2::TransformStorage::stamp_
ros::Time stamp_
Definition: transform_storage.h:78
Quaternion.h
time_cache.h
tf2::TransformStorage::translation_
tf2::Vector3 translation_
Definition: transform_storage.h:77
tf2::TransformStorage::frame_id_
CompactFrameID frame_id_
Definition: transform_storage.h:79
tf2::cache::createExtrapolationException1
void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string *error_str)
Definition: cache.cpp:65
tf2::TimeCache::getData
virtual bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
Virtual methods.
Definition: cache.cpp:206
header
std_msgs::Header const * header(const M &m)
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:314
ros::Time
tf2::cache::createExtrapolationException3
void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string *error_str)
Definition: cache.cpp:89
Vector3.h
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::Quaternion
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30
tf2::slerp
TF2SIMD_FORCE_INLINE Quaternion slerp(const Quaternion &q1, const Quaternion &q2, const tf2Scalar &t)
Return the result of spherical linear interpolation betwen two quaternions.
Definition: Quaternion.h:432
DurationBase< Duration >::toSec
double toSec() const
tf2Scalar
double tf2Scalar
The tf2Scalar type abstracts floating point numbers, to easily switch between double and single float...
Definition: Scalar.h:159
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:303
ros::Duration
tf2::cache::createExtrapolationException2
void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string *error_str)
Definition: cache.cpp:75


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Sep 4 2020 03:15:36