Fawkes API  Fawkes Development Version
time_cache.cpp
1 /***************************************************************************
2  * time_cache.cpp - Fawkes tf time cache (based on ROS tf)
3  *
4  * Created: Thu Oct 20 11:26:40 2011
5  * Copyright 2011 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version. A runtime exception applies to
12  * this software (see LICENSE.GPL_WRE file mentioned below for details).
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
20  */
21 
22 /* This code is based on ROS tf with the following copyright and license:
23  *
24  * Copyright (c) 2008, Willow Garage, Inc.
25  * All rights reserved.
26  *
27  * Redistribution and use in source and binary forms, with or without
28  * modification, are permitted provided that the following conditions are met:
29  *
30  * * Redistributions of source code must retain the above copyright
31  * notice, this list of conditions and the following disclaimer.
32  * * Redistributions in binary form must reproduce the above copyright
33  * notice, this list of conditions and the following disclaimer in the
34  * documentation and/or other materials provided with the distribution.
35  * * Neither the name of the Willow Garage, Inc. nor the names of its
36  * contributors may be used to endorse or promote products derived from
37  * this software without specific prior written permission.
38  *
39  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
40  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
41  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
42  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
43  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
44  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
45  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
46  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
47  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49  * POSSIBILITY OF SUCH DAMAGE.
50  */
51 
52 #include <tf/time_cache.h>
53 
54 #include <cstdio>
55 
56 namespace fawkes {
57  namespace tf {
58 #if 0 /* just to make Emacs auto-indent happy */
59  }
60 }
61 #endif
62 
63 /** @class TransformStorage <tf/time_cache.h>
64  * Storage for transforms and their parent.
65  *
66  * @fn TransformStorage & TransformStorage::operator=(const TransformStorage& rhs)
67  * Assignment operator.
68  * @param rhs storage to assign
69  * @return reference to this instance
70  */
71 
72 /** Constructor. */
74 {
75 }
76 
77 /** Constructor.
78  * @param data initial stamped transform data
79  * @param frame_id parent frame ID
80  * @param child_frame_id child frame ID
81  */
83  CompactFrameID frame_id,
84  CompactFrameID child_frame_id)
85 : rotation(data.getRotation())
86 , translation(data.getOrigin())
87 , stamp(data.stamp)
88 , frame_id(frame_id)
89 , child_frame_id(child_frame_id)
90 { }
91 
92 
93 /** @class TimeCacheInterface <tf/time_cache.h>
94  * Interface for transform time caches.
95  *
96  * @fn virtual TimeCacheInterfacePtr TimeCacheInterface::clone(const fawkes::Time &look_back_until = fawkes::Time(0,0)) const = 0
97  * Create a copy of this time cache.
98  * @param look_back_until if non-zero time is passed only
99  * include transforms younger than the given time.
100  * @return shared pointer to copy of this time cache
101  *
102  * @fn virtual bool TimeCacheInterface::get_data(fawkes::Time time, TransformStorage & data_out, std::string* error_str = 0) = 0
103  * Get data.
104  * @param time time for which go get data
105  * @param data_out upon return contains requested data
106  * @param error_str error stirng
107  * @return false if data not available
108  *
109  * @fn virtual bool TimeCacheInterface::insert_data(const TransformStorage& new_data) = 0
110  * Insert data.
111  * @param new_data data to insert
112  * @return true on success, false otherwise
113  *
114  * @fn virtual void TimeCacheInterface::clear_list() = 0
115  * Clear storage.
116  *
117  * @fn virtual CompactFrameID TimeCacheInterface::get_parent(fawkes::Time time, std::string* error_str) = 0
118  * Get parent frame number.
119  * @param time point in time
120  * @param error_str error string
121  * @return frame number
122  *
123  * @fn virtual P_TimeAndFrameID TimeCacheInterface::get_latest_time_and_parent() = 0
124  * Get latest time and parent frame number.
125  * @return latest time and parent frame number
126  *
127  * @fn virtual unsigned int TimeCacheInterface::get_list_length() = 0 const
128  * Get storage list length.
129  * @return storage list length
130  *
131  * @fn virtual fawkes::Time TimeCacheInterface::get_latest_timestamp() = 0 const
132  * Get latest timestamp from cache.
133  * @return latest timestamp
134  *
135  * @fn virtual fawkes::Time TimeCacheInterface::get_oldest_timestamp() = 0 const
136  * Get oldest timestamp from cache.
137  * @return oldest time stamp.
138  *
139  * @fn virtual const L_TransformStorage & TimeCacheInterface::get_storage() const = 0
140  * Get storage list.
141  * @return reference to list of storage elements
142  *
143  * @fn L_TransformStorage TimeCacheInterface::get_storage_copy() const = 0
144  * Get copy of storage elements.
145  * @return copied list of storage elements
146  */
147 
148 /** @class TimeCache <tf/time_cache.h>
149  * Time based transform cache.
150  * A class to keep a sorted linked list in time. This builds and
151  * maintains a list of timestamped data. And provides lookup
152  * functions to get data out as a function of time.
153  */
154 
155 /** Constructor.
156  * @param max_storage_time maximum time in seconds to cache, defaults to 10 seconds
157  */
158 TimeCache::TimeCache(float max_storage_time)
159 : max_storage_time_(max_storage_time)
160 {}
161 
162 
163 /** Create extrapolation error string.
164  * @param t0 requested time
165  * @param t1 available time
166  * @param error_str upon return contains the error string.
167  */
168 // hoisting these into separate functions causes an ~8% speedup.
169 // Removing calling them altogether adds another ~10%
170 void
171 create_extrapolation_exception1(fawkes::Time t0, fawkes::Time t1, std::string* error_str)
172 {
173  if (error_str)
174  {
175  char *tmp;
176  if (asprintf(&tmp, "Lookup would require extrapolation at time %li.%li, "
177  "but only time %li.%li is in the buffer", t0.get_sec(), t0.get_nsec(),
178  t1.get_sec(), t1.get_usec()) != -1)
179  {
180  *error_str = tmp;
181  free(tmp);
182  }
183  }
184 }
185 
186 /** Create extrapolation error string.
187  * @param t0 requested time
188  * @param t1 available time
189  * @param error_str upon return contains the error string.
190  */
191 void
192 create_extrapolation_exception2(fawkes::Time t0, fawkes::Time t1, std::string* error_str)
193 {
194  if (error_str)
195  {
196  char *tmp;
197  if (asprintf(&tmp,"Lookup would require extrapolation into the future. "
198  "Requested time %li.%li, but the latest data is at time %li.%li",
199  t0.get_sec(), t0.get_usec(), t1.get_sec(), t1.get_usec()) != -1)
200  {
201  *error_str = tmp;
202  free(tmp);
203  }
204  }
205 }
206 
207 /** Create extrapolation error string.
208  * @param t0 requested time
209  * @param t1 available time
210  * @param error_str upon return contains the error string.
211  */
212 void
213 create_extrapolation_exception3(fawkes::Time t0, fawkes::Time t1, std::string* error_str)
214 {
215  if (error_str)
216  {
217  char *tmp;
218  if (asprintf(&tmp,"Lookup would require extrapolation into the past. "
219  "Requested time %li.%li, but the latest data is at time %li.%li",
220  t0.get_sec(), t0.get_usec(), t1.get_sec(), t1.get_usec()) != -1)
221  {
222  *error_str = tmp;
223  free(tmp);
224  }
225  }
226 }
227 
228 /// A helper function for getData
229 //Assumes storage is already locked for it
230 uint8_t
231 TimeCache::find_closest(TransformStorage*& one, TransformStorage*& two,
232  fawkes::Time target_time, std::string* error_str)
233 {
234  //No values stored
235  if (storage_.empty()) {
236  if (error_str) *error_str = "Transform cache storage is empty";
237  return 0;
238  }
239 
240  //If time == 0 return the latest
241  if (target_time.is_zero()) {
242  one = &storage_.front();
243  return 1;
244  }
245 
246  // One value stored
247  if (++storage_.begin() == storage_.end()) {
248  TransformStorage& ts = *storage_.begin();
249  if (ts.stamp == target_time) {
250  one = &ts;
251  return 1;
252  } else {
253  create_extrapolation_exception1(target_time, ts.stamp, error_str);
254  return 0;
255  }
256  }
257 
258  fawkes::Time latest_time = (*storage_.begin()).stamp;
259  fawkes::Time earliest_time = (*(storage_.rbegin())).stamp;
260 
261  if (target_time == latest_time) {
262  one = &(*storage_.begin());
263  return 1;
264  } else if (target_time == earliest_time) {
265  one = &(*storage_.rbegin());
266  return 1;
267  } else if (target_time > latest_time) {
268  // Catch cases that would require extrapolation
269  create_extrapolation_exception2(target_time, latest_time, error_str);
270  return 0;
271  } else if (target_time < earliest_time) {
272  create_extrapolation_exception3(target_time, earliest_time, error_str);
273  return 0;
274  }
275 
276  //At least 2 values stored
277  //Find the first value less than the target value
278  L_TransformStorage::iterator storage_it = storage_.begin();
279  while(storage_it != storage_.end()) {
280  if (storage_it->stamp <= target_time) break;
281  storage_it++;
282  }
283 
284  //Finally the case were somewhere in the middle Guarenteed no extrapolation :-)
285  one = &*(storage_it); //Older
286  two = &*(--storage_it); //Newer
287  return 2;
288 }
289 
290 
291 void
292 TimeCache::interpolate(const TransformStorage& one,
293  const TransformStorage& two,
294  fawkes::Time time, TransformStorage& output)
295 {
296  // Check for zero distance case
297  if( two.stamp == one.stamp ) {
298  output = two;
299  return;
300  }
301  //Calculate the ratio
302  btScalar ratio =
303  (time.in_sec() - one.stamp.in_sec()) /
304  (two.stamp.in_sec() - one.stamp.in_sec());
305 
306  //Interpolate translation
307  output.translation.setInterpolate3(one.translation, two.translation, ratio);
308 
309  //Interpolate rotation
310  output.rotation = slerp( one.rotation, two.rotation, ratio);
311 
312  output.stamp = one.stamp;
313  output.frame_id = one.frame_id;
314  output.child_frame_id = one.child_frame_id;
315 }
316 
317 TimeCacheInterfacePtr
318 TimeCache::clone(const fawkes::Time &look_back_until) const
319 {
320  TimeCache *copy = new TimeCache(max_storage_time_);
321  if (look_back_until.is_zero()) {
322  copy->storage_ = storage_;
323  } else {
324  L_TransformStorage::const_iterator storage_it = storage_.begin();
325  for (storage_it = storage_.begin(); storage_it != storage_.end(); ++storage_it) {
326  if (storage_it->stamp <= look_back_until) break;
327  copy->storage_.push_back(*storage_it);
328  }
329  }
330  return std::shared_ptr<TimeCacheInterface>(copy);
331 }
332 
333 
334 bool
336  std::string* error_str)
337 {
338  TransformStorage* p_temp_1 = NULL;
339  TransformStorage* p_temp_2 = NULL;
340 
341  int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
342  if (num_nodes == 0) {
343  return false;
344  } else if (num_nodes == 1) {
345  data_out = *p_temp_1;
346  } else if (num_nodes == 2) {
347  if( p_temp_1->frame_id == p_temp_2->frame_id) {
348  interpolate(*p_temp_1, *p_temp_2, time, data_out);
349  } else {
350  data_out = *p_temp_1;
351  }
352  }
353 
354  return true;
355 }
356 
357 CompactFrameID
358 TimeCache::get_parent(fawkes::Time time, std::string* error_str)
359 {
360  TransformStorage* p_temp_1 = NULL;
361  TransformStorage* p_temp_2 = NULL;
362 
363  int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
364  if (num_nodes == 0) {
365  return 0;
366  }
367 
368  return p_temp_1->frame_id;
369 }
370 
371 
372 bool
374 {
375  L_TransformStorage::iterator storage_it = storage_.begin();
376 
377  if(storage_it != storage_.end()) {
378  if (storage_it->stamp > new_data.stamp + max_storage_time_) {
379  return false;
380  }
381  }
382 
383 
384  while(storage_it != storage_.end()) {
385  if (storage_it->stamp <= new_data.stamp)
386  break;
387  storage_it++;
388  }
389  storage_.insert(storage_it, new_data);
390 
391  prune_list();
392  return true;
393 }
394 
395 void
397 {
398  storage_.clear();
399 }
400 
401 unsigned int
403 {
404  return storage_.size();
405 }
406 
407 
410 {
411  return storage_;
412 }
413 
416 {
417  return storage_;
418 }
419 
420 P_TimeAndFrameID
422 {
423  if (storage_.empty()) {
424  return std::make_pair(fawkes::Time(), 0);
425  }
426 
427  const TransformStorage& ts = storage_.front();
428  return std::make_pair(ts.stamp, ts.frame_id);
429 }
430 
433 {
434  if (storage_.empty()) return fawkes::Time(0,0); //empty list case
435  return storage_.front().stamp;
436 }
437 
440 {
441  if (storage_.empty()) return fawkes::Time(0,0); //empty list case
442  return storage_.back().stamp;
443 }
444 
445 /** Prune storage list based on maximum cache lifetime. */
446 void
447 TimeCache::prune_list()
448 {
449  fawkes::Time latest_time = storage_.begin()->stamp;
450 
451  while(!storage_.empty() &&
452  storage_.back().stamp + max_storage_time_ < latest_time)
453  {
454  storage_.pop_back();
455  }
456 
457 }
458 
459 
460 } // end namespace tf
461 } // end namespace fawkes
virtual L_TransformStorage get_storage_copy() const
Get copy of storage elements.
Definition: time_cache.cpp:415
virtual bool insert_data(const TransformStorage &new_data)
Insert data.
Definition: time_cache.cpp:373
double in_sec() const
Convet time to seconds.
Definition: time.cpp:232
virtual void clear_list()
Clear storage.
Definition: time_cache.cpp:396
virtual bool get_data(fawkes::Time time, TransformStorage &data_out, std::string *error_str=0)
Get data.
Definition: time_cache.cpp:335
virtual CompactFrameID get_parent(fawkes::Time time, std::string *error_str)
Get parent frame number.
Definition: time_cache.cpp:358
virtual const L_TransformStorage & get_storage() const
Get storage list.
Definition: time_cache.cpp:409
Fawkes library namespace.
Storage for transforms and their parent.
A class for handling time.
Definition: time.h:91
virtual fawkes::Time get_latest_timestamp() const
Get latest timestamp from cache.
Definition: time_cache.cpp:432
virtual TimeCacheInterfacePtr clone(const fawkes::Time &look_back_until=fawkes::Time(0, 0)) const
Create a copy of this time cache.
Definition: time_cache.cpp:318
CompactFrameID child_frame_id
child frame number
std::list< TransformStorage > L_TransformStorage
List of stored transforms.
Definition: time_cache.h:78
Time based transform cache.
Definition: time_cache.h:98
CompactFrameID frame_id
parent/reference frame number
long get_nsec() const
Get nanoseconds.
Definition: time.h:113
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
bool is_zero() const
Check if time is zero.
Definition: time.h:116
virtual unsigned int get_list_length() const
Debugging information methods.
Definition: time_cache.cpp:402
TimeCache(float max_storage_time=DEFAULT_MAX_STORAGE_TIME)
Constructor.
Definition: time_cache.cpp:158
long get_sec() const
Get seconds.
Definition: time.h:110
virtual P_TimeAndFrameID get_latest_time_and_parent()
Get latest time and parent frame number.
Definition: time_cache.cpp:421
long get_usec() const
Get microseconds.
Definition: time.h:112
Quaternion rotation
rotation quaternion
Vector3 translation
translation vector
fawkes::Time stamp
time stamp
virtual fawkes::Time get_oldest_timestamp() const
Get oldest timestamp from cache.
Definition: time_cache.cpp:439
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
TransformStorage()
Constructor.
Definition: time_cache.cpp:73