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
56namespace fawkes {
57namespace 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 */
154TimeCache::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%
170void
171create_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 */
194void
195create_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 */
218void
219create_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
239uint8_t
240TimeCache::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
303void
304TimeCache::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
328TimeCacheInterfacePtr
329TimeCache::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
345bool
346TimeCache::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
367CompactFrameID
368TimeCache::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
381bool
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
403void
405{
406 storage_.clear();
407}
408
409unsigned int
411{
412 return storage_.size();
413}
414
417{
418 return storage_;
419}
420
423{
424 return storage_;
425}
426
427P_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. */
455void
456TimeCache::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.