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  std::stringstream ss;
70  ss << "Lookup would require extrapolation at time " << t0 << ", but only time " << t1 << " is in the buffer";
71  *error_str = ss.str();
72  }
73 }
74 
75 void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string* error_str)
76 {
77  if (error_str)
78  {
79  std::stringstream ss;
80  ss << "Lookup would require extrapolation into the future. Requested time " << t0 << " but the latest data is at time " << t1;
81  *error_str = ss.str();
82  }
83 }
84 
85 void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string* error_str)
86 {
87  if (error_str)
88  {
89  std::stringstream ss;
90  ss << "Lookup would require extrapolation into the past. Requested time " << t0 << " but the earliest data is at time " << t1;
91  *error_str = ss.str();
92  }
93 }
94 } // namespace cache
95 
96 bool operator>(const TransformStorage& lhs, const TransformStorage& rhs)
97 {
98  return lhs.stamp_ > rhs.stamp_;
99 }
100 
101 uint8_t TimeCache::findClosest(TransformStorage*& one, TransformStorage*& two, ros::Time target_time, std::string* error_str)
102 {
103  //No values stored
104  if (storage_.empty())
105  {
106  return 0;
107  }
108 
109  //If time == 0 return the latest
110  if (target_time.isZero())
111  {
112  one = &storage_.front();
113  return 1;
114  }
115 
116  // One value stored
117  if (++storage_.begin() == storage_.end())
118  {
119  TransformStorage& ts = *storage_.begin();
120  if (ts.stamp_ == target_time)
121  {
122  one = &ts;
123  return 1;
124  }
125  else
126  {
127  cache::createExtrapolationException1(target_time, ts.stamp_, error_str);
128  return 0;
129  }
130  }
131 
132  ros::Time latest_time = (*storage_.begin()).stamp_;
133  ros::Time earliest_time = (*(storage_.rbegin())).stamp_;
134 
135  if (target_time == latest_time)
136  {
137  one = &(*storage_.begin());
138  return 1;
139  }
140  else if (target_time == earliest_time)
141  {
142  one = &(*storage_.rbegin());
143  return 1;
144  }
145  // Catch cases that would require extrapolation
146  else if (target_time > latest_time)
147  {
148  cache::createExtrapolationException2(target_time, latest_time, error_str);
149  return 0;
150  }
151  else if (target_time < earliest_time)
152  {
153  cache::createExtrapolationException3(target_time, earliest_time, error_str);
154  return 0;
155  }
156 
157  //At least 2 values stored
158  //Find the first value less than the target value
159  L_TransformStorage::iterator storage_it;
160  TransformStorage storage_target_time;
161  storage_target_time.stamp_ = target_time;
162 
163  storage_it = std::lower_bound(
164  storage_.begin(),
165  storage_.end(),
166  storage_target_time, std::greater<TransformStorage>());
167 
168  //Finally the case were somewhere in the middle Guarenteed no extrapolation :-)
169  one = &*(storage_it); //Older
170  two = &*(--storage_it); //Newer
171  return 2;
172 
173 
174 }
175 
177 {
178  // Check for zero distance case
179  if( two.stamp_ == one.stamp_ )
180  {
181  output = two;
182  return;
183  }
184  //Calculate the ratio
185  tf2Scalar ratio = (time.toSec() - one.stamp_.toSec()) / (two.stamp_.toSec() - one.stamp_.toSec());
186 
187  //Interpolate translation
188  output.translation_.setInterpolate3(one.translation_, two.translation_, ratio);
189 
190  //Interpolate rotation
191  output.rotation_ = slerp( one.rotation_, two.rotation_, ratio);
192 
193  output.stamp_ = time;
194  output.frame_id_ = one.frame_id_;
195  output.child_frame_id_ = one.child_frame_id_;
196 }
197 
198 bool TimeCache::getData(ros::Time time, TransformStorage & data_out, std::string* error_str) //returns false if data not available
199 {
200  TransformStorage* p_temp_1;
201  TransformStorage* p_temp_2;
202 
203  int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str);
204  if (num_nodes == 0)
205  {
206  return false;
207  }
208  else if (num_nodes == 1)
209  {
210  data_out = *p_temp_1;
211  }
212  else if (num_nodes == 2)
213  {
214  if( p_temp_1->frame_id_ == p_temp_2->frame_id_)
215  {
216  interpolate(*p_temp_1, *p_temp_2, time, data_out);
217  }
218  else
219  {
220  data_out = *p_temp_1;
221  }
222  }
223  else
224  {
225  assert(0);
226  }
227 
228  return true;
229 }
230 
231 CompactFrameID TimeCache::getParent(ros::Time time, std::string* error_str)
232 {
233  TransformStorage* p_temp_1;
234  TransformStorage* p_temp_2;
235 
236  int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str);
237  if (num_nodes == 0)
238  {
239  return 0;
240  }
241 
242  return p_temp_1->frame_id_;
243 }
244 
246 {
247  L_TransformStorage::iterator storage_it = storage_.begin();
248 
249  if(storage_it != storage_.end())
250  {
251  if (storage_it->stamp_ > new_data.stamp_ + max_storage_time_)
252  {
253  return false;
254  }
255  }
256 
257 
258  while(storage_it != storage_.end())
259  {
260  if (storage_it->stamp_ <= new_data.stamp_)
261  break;
262  storage_it++;
263  }
264  storage_.insert(storage_it, new_data);
265 
266  pruneList();
267  return true;
268 }
269 
271 {
272  storage_.clear();
273 }
274 
276 {
277  return storage_.size();
278 }
279 
281 {
282  if (storage_.empty())
283  {
284  return std::make_pair(ros::Time(), 0);
285  }
286 
287  const TransformStorage& ts = storage_.front();
288  return std::make_pair(ts.stamp_, ts.frame_id_);
289 }
290 
292 {
293  if (storage_.empty()) return ros::Time(); //empty list case
294  return storage_.front().stamp_;
295 }
296 
298 {
299  if (storage_.empty()) return ros::Time(); //empty list case
300  return storage_.back().stamp_;
301 }
302 
304 {
305  ros::Time latest_time = storage_.begin()->stamp_;
306 
307  while(!storage_.empty() && storage_.back().stamp_ + max_storage_time_ < latest_time)
308  {
309  storage_.pop_back();
310  }
311 
312 } // namespace tf2
313 }
uint32_t CompactFrameID
ros::Duration max_storage_time_
Definition: time_cache.h:122
bool operator>(const TransformStorage &lhs, const TransformStorage &rhs)
Definition: cache.cpp:96
void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string *error_str)
Definition: cache.cpp:85
double tf2Scalar
The tf2Scalar type abstracts floating point numbers, to easily switch between double and single float...
Definition: Scalar.h:159
virtual ros::Time getLatestTimestamp()
Get the latest timestamp cached.
Definition: cache.cpp:291
L_TransformStorage storage_
Definition: time_cache.h:120
std_msgs::Header * header(M &m)
virtual bool insertData(const TransformStorage &new_data)
Insert data into the cache.
Definition: cache.cpp:245
CompactFrameID child_frame_id_
void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string *error_str)
Definition: cache.cpp:65
tf2::Quaternion rotation_
std::pair< ros::Time, CompactFrameID > P_TimeAndFrameID
Definition: buffer_core.h:57
uint8_t findClosest(TransformStorage *&one, TransformStorage *&two, ros::Time target_time, std::string *error_str)
A helper function for getData.
Definition: cache.cpp:101
virtual ros::Time getOldestTimestamp()
Get the oldest timestamp cached.
Definition: cache.cpp:297
void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string *error_str)
Definition: cache.cpp:75
virtual unsigned int getListLength()
Debugging information methods.
Definition: cache.cpp:275
Storage for transforms and their parent.
CompactFrameID frame_id_
void interpolate(const TransformStorage &one, const TransformStorage &two, ros::Time time, TransformStorage &output)
Definition: cache.cpp:176
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:280
void pruneList()
Definition: cache.cpp:303
virtual void clearList()
Clear the list of stored values.
Definition: cache.cpp:270
TimeCache(ros::Duration max_storage_time=ros::Duration().fromNSec(DEFAULT_MAX_STORAGE_TIME))
Definition: cache.cpp:59
virtual CompactFrameID getParent(ros::Time time, std::string *error_str)
Retrieve the parent at a specific time.
Definition: cache.cpp:231
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
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:430
virtual bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
Virtual methods.
Definition: cache.cpp:198


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Feb 22 2018 03:32:23