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 
59 /** @class TransformStorage <tf/time_cache.h>
60  * Storage for transforms and their parent.
61  *
62  * @fn TransformStorage & TransformStorage::operator=(const TransformStorage& rhs)
63  * Assignment operator.
64  * @param rhs storage to assign
65  * @return reference to this instance
66  */
67 
68 /** Constructor. */
70 {
71 }
72 
73 /** Constructor.
74  * @param data initial stamped transform data
75  * @param frame_id parent frame ID
76  * @param child_frame_id child frame ID
77  */
79  CompactFrameID frame_id,
80  CompactFrameID child_frame_id)
81 : rotation(data.getRotation()),
82  translation(data.getOrigin()),
83  stamp(data.stamp),
84  frame_id(frame_id),
85  child_frame_id(child_frame_id)
86 {
87 }
88 
89 /** @class TimeCacheInterface <tf/time_cache.h>
90  * Interface for transform time caches.
91  *
92  * @fn virtual TimeCacheInterfacePtr TimeCacheInterface::clone(const fawkes::Time &look_back_until = fawkes::Time(0,0)) const = 0
93  * Create a copy of this time cache.
94  * @param look_back_until if non-zero time is passed only
95  * include transforms younger than the given time.
96  * @return shared pointer to copy of this time cache
97  *
98  * @fn virtual bool TimeCacheInterface::get_data(fawkes::Time time, TransformStorage & data_out, std::string* error_str = 0) = 0
99  * Get data.
100  * @param time time for which go get data
101  * @param data_out upon return contains requested data
102  * @param error_str error stirng
103  * @return false if data not available
104  *
105  * @fn virtual bool TimeCacheInterface::insert_data(const TransformStorage& new_data) = 0
106  * Insert data.
107  * @param new_data data to insert
108  * @return true on success, false otherwise
109  *
110  * @fn virtual void TimeCacheInterface::clear_list() = 0
111  * Clear storage.
112  *
113  * @fn virtual CompactFrameID TimeCacheInterface::get_parent(fawkes::Time time, std::string* error_str) = 0
114  * Get parent frame number.
115  * @param time point in time
116  * @param error_str error string
117  * @return frame number
118  *
119  * @fn virtual P_TimeAndFrameID TimeCacheInterface::get_latest_time_and_parent() = 0
120  * Get latest time and parent frame number.
121  * @return latest time and parent frame number
122  *
123  * @fn virtual unsigned int TimeCacheInterface::get_list_length() = 0 const
124  * Get storage list length.
125  * @return storage list length
126  *
127  * @fn virtual fawkes::Time TimeCacheInterface::get_latest_timestamp() = 0 const
128  * Get latest timestamp from cache.
129  * @return latest timestamp
130  *
131  * @fn virtual fawkes::Time TimeCacheInterface::get_oldest_timestamp() = 0 const
132  * Get oldest timestamp from cache.
133  * @return oldest time stamp.
134  *
135  * @fn virtual const L_TransformStorage & TimeCacheInterface::get_storage() const = 0
136  * Get storage list.
137  * @return reference to list of storage elements
138  *
139  * @fn L_TransformStorage TimeCacheInterface::get_storage_copy() const = 0
140  * Get copy of storage elements.
141  * @return copied list of storage elements
142  */
143 
144 /** @class TimeCache <tf/time_cache.h>
145  * Time based transform cache.
146  * A class to keep a sorted linked list in time. This builds and
147  * maintains a list of timestamped data. And provides lookup
148  * functions to get data out as a function of time.
149  */
150 
151 /** Constructor.
152  * @param max_storage_time maximum time in seconds to cache, defaults to 10 seconds
153  */
154 TimeCache::TimeCache(float max_storage_time) : max_storage_time_(max_storage_time)
155 {
156 }
157 
158 /** Destructor. */
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  char *tmp;
175  if (asprintf(&tmp,
176  "Lookup would require extrapolation at time %li.%li, "
177  "but only time %li.%li is in the buffer",
178  t0.get_sec(),
179  t0.get_nsec(),
180  t1.get_sec(),
181  t1.get_usec())
182  != -1) {
183  *error_str = tmp;
184  free(tmp);
185  }
186  }
187 }
188 
189 /** Create extrapolation error string.
190  * @param t0 requested time
191  * @param t1 available time
192  * @param error_str upon return contains the error string.
193  */
194 void
195 create_extrapolation_exception2(fawkes::Time t0, fawkes::Time t1, std::string *error_str)
196 {
197  if (error_str) {
198  char *tmp;
199  if (asprintf(&tmp,
200  "Lookup would require extrapolation into the future. "
201  "Requested time %li.%li, but the latest data is at time %li.%li",
202  t0.get_sec(),
203  t0.get_usec(),
204  t1.get_sec(),
205  t1.get_usec())
206  != -1) {
207  *error_str = tmp;
208  free(tmp);
209  }
210  }
211 }
212 
213 /** Create extrapolation error string.
214  * @param t0 requested time
215  * @param t1 available time
216  * @param error_str upon return contains the error string.
217  */
218 void
219 create_extrapolation_exception3(fawkes::Time t0, fawkes::Time t1, std::string *error_str)
220 {
221  if (error_str) {
222  char *tmp;
223  if (asprintf(&tmp,
224  "Lookup would require extrapolation into the past. "
225  "Requested time %li.%li, but the latest data is at time %li.%li",
226  t0.get_sec(),
227  t0.get_usec(),
228  t1.get_sec(),
229  t1.get_usec())
230  != -1) {
231  *error_str = tmp;
232  free(tmp);
233  }
234  }
235 }
236 
237 /// A helper function for getData
238 //Assumes storage is already locked for it
239 uint8_t
240 TimeCache::find_closest(TransformStorage *&one,
241  TransformStorage *&two,
242  fawkes::Time target_time,
243  std::string * error_str)
244 {
245  //No values stored
246  if (storage_.empty()) {
247  if (error_str)
248  *error_str = "Transform cache storage is empty";
249  return 0;
250  }
251 
252  //If time == 0 return the latest
253  if (target_time.is_zero()) {
254  one = &storage_.front();
255  return 1;
256  }
257 
258  // One value stored
259  if (++storage_.begin() == storage_.end()) {
260  TransformStorage &ts = *storage_.begin();
261  if (ts.stamp == target_time) {
262  one = &ts;
263  return 1;
264  } else {
265  create_extrapolation_exception1(target_time, ts.stamp, error_str);
266  return 0;
267  }
268  }
269 
270  fawkes::Time latest_time = (*storage_.begin()).stamp;
271  fawkes::Time earliest_time = (*(storage_.rbegin())).stamp;
272 
273  if (target_time == latest_time) {
274  one = &(*storage_.begin());
275  return 1;
276  } else if (target_time == earliest_time) {
277  one = &(*storage_.rbegin());
278  return 1;
279  } else if (target_time > latest_time) {
280  // Catch cases that would require extrapolation
281  create_extrapolation_exception2(target_time, latest_time, error_str);
282  return 0;
283  } else if (target_time < earliest_time) {
284  create_extrapolation_exception3(target_time, earliest_time, error_str);
285  return 0;
286  }
287 
288  //At least 2 values stored
289  //Find the first value less than the target value
290  L_TransformStorage::iterator storage_it = storage_.begin();
291  while (storage_it != storage_.end()) {
292  if (storage_it->stamp <= target_time)
293  break;
294  storage_it++;
295  }
296 
297  //Finally the case were somewhere in the middle Guarenteed no extrapolation :-)
298  one = &*(storage_it); //Older
299  two = &*(--storage_it); //Newer
300  return 2;
301 }
302 
303 void
304 TimeCache::interpolate(const TransformStorage &one,
305  const TransformStorage &two,
306  fawkes::Time time,
307  TransformStorage & output)
308 {
309  // Check for zero distance case
310  if (two.stamp == one.stamp) {
311  output = two;
312  return;
313  }
314  //Calculate the ratio
315  btScalar ratio = (time.in_sec() - one.stamp.in_sec()) / (two.stamp.in_sec() - one.stamp.in_sec());
316 
317  //Interpolate translation
318  output.translation.setInterpolate3(one.translation, two.translation, ratio);
319 
320  //Interpolate rotation
321  output.rotation = slerp(one.rotation, two.rotation, ratio);
322 
323  output.stamp = one.stamp;
324  output.frame_id = one.frame_id;
325  output.child_frame_id = one.child_frame_id;
326 }
327 
328 TimeCacheInterfacePtr
329 TimeCache::clone(const fawkes::Time &look_back_until) const
330 {
331  TimeCache *copy = new TimeCache(max_storage_time_);
332  if (look_back_until.is_zero()) {
333  copy->storage_ = storage_;
334  } else {
335  L_TransformStorage::const_iterator storage_it = storage_.begin();
336  for (storage_it = storage_.begin(); storage_it != storage_.end(); ++storage_it) {
337  if (storage_it->stamp <= look_back_until)
338  break;
339  copy->storage_.push_back(*storage_it);
340  }
341  }
342  return std::shared_ptr<TimeCacheInterface>(copy);
343 }
344 
345 bool
346 TimeCache::get_data(fawkes::Time time, TransformStorage &data_out, std::string *error_str)
347 {
348  TransformStorage *p_temp_1 = NULL;
349  TransformStorage *p_temp_2 = NULL;
350 
351  int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
352  if (num_nodes == 0) {
353  return false;
354  } else if (num_nodes == 1) {
355  data_out = *p_temp_1;
356  } else if (num_nodes == 2) {
357  if (p_temp_1->frame_id == p_temp_2->frame_id) {
358  interpolate(*p_temp_1, *p_temp_2, time, data_out);
359  } else {
360  data_out = *p_temp_1;
361  }
362  }
363 
364  return true;
365 }
366 
367 CompactFrameID
368 TimeCache::get_parent(fawkes::Time time, std::string *error_str)
369 {
370  TransformStorage *p_temp_1 = NULL;
371  TransformStorage *p_temp_2 = NULL;
372 
373  int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
374  if (num_nodes == 0) {
375  return 0;
376  }
377 
378  return p_temp_1->frame_id;
379 }
380 
381 bool
383 {
384  L_TransformStorage::iterator storage_it = storage_.begin();
385 
386  if (storage_it != storage_.end()) {
387  if (storage_it->stamp > new_data.stamp + max_storage_time_) {
388  return false;
389  }
390  }
391 
392  while (storage_it != storage_.end()) {
393  if (storage_it->stamp <= new_data.stamp)
394  break;
395  storage_it++;
396  }
397  storage_.insert(storage_it, new_data);
398 
399  prune_list();
400  return true;
401 }
402 
403 void
405 {
406  storage_.clear();
407 }
408 
409 unsigned int
411 {
412  return storage_.size();
413 }
414 
417 {
418  return storage_;
419 }
420 
423 {
424  return storage_;
425 }
426 
427 P_TimeAndFrameID
429 {
430  if (storage_.empty()) {
431  return std::make_pair(fawkes::Time(), 0);
432  }
433 
434  const TransformStorage &ts = storage_.front();
435  return std::make_pair(ts.stamp, ts.frame_id);
436 }
437 
440 {
441  if (storage_.empty())
442  return fawkes::Time(0, 0); //empty list case
443  return storage_.front().stamp;
444 }
445 
448 {
449  if (storage_.empty())
450  return fawkes::Time(0, 0); //empty list case
451  return storage_.back().stamp;
452 }
453 
454 /** Prune storage list based on maximum cache lifetime. */
455 void
456 TimeCache::prune_list()
457 {
458  fawkes::Time latest_time = storage_.begin()->stamp;
459 
460  while (!storage_.empty() && storage_.back().stamp + max_storage_time_ < latest_time) {
461  storage_.pop_back();
462  }
463 }
464 
465 } // end namespace tf
466 } // end namespace fawkes
A class for handling time.
Definition: time.h:93
long get_usec() const
Get microseconds.
Definition: time.h:127
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
bool is_zero() const
Check if time is zero.
Definition: time.h:143
double in_sec() const
Convet time to seconds.
Definition: time.cpp:219
long get_nsec() const
Get nanoseconds.
Definition: time.h:132
long get_sec() const
Get seconds.
Definition: time.h:117
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
std::list< TransformStorage > L_TransformStorage
List of stored transforms.
Definition: time_cache.h:74
Time based transform cache.
Definition: time_cache.h:95
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:329
virtual unsigned int get_list_length() const
Debugging information methods.
Definition: time_cache.cpp:410
virtual CompactFrameID get_parent(fawkes::Time time, std::string *error_str)
Get parent frame number.
Definition: time_cache.cpp:368
virtual fawkes::Time get_oldest_timestamp() const
Get oldest timestamp from cache.
Definition: time_cache.cpp:447
virtual fawkes::Time get_latest_timestamp() const
Get latest timestamp from cache.
Definition: time_cache.cpp:439
virtual L_TransformStorage get_storage_copy() const
Get copy of storage elements.
Definition: time_cache.cpp:422
TimeCache(float max_storage_time=DEFAULT_MAX_STORAGE_TIME)
Constructor.
Definition: time_cache.cpp:154
virtual ~TimeCache()
Destructor.
Definition: time_cache.cpp:159
virtual bool insert_data(const TransformStorage &new_data)
Insert data.
Definition: time_cache.cpp:382
virtual P_TimeAndFrameID get_latest_time_and_parent()
Get latest time and parent frame number.
Definition: time_cache.cpp:428
virtual void clear_list()
Clear storage.
Definition: time_cache.cpp:404
virtual const L_TransformStorage & get_storage() const
Get storage list.
Definition: time_cache.cpp:416
virtual bool get_data(fawkes::Time time, TransformStorage &data_out, std::string *error_str=0)
Get data.
Definition: time_cache.cpp:346
Storage for transforms and their parent.
CompactFrameID frame_id
parent/reference frame number
TransformStorage()
Constructor.
Definition: time_cache.cpp:69
fawkes::Time stamp
time stamp
Fawkes library namespace.