Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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. */
73 TransformStorage::TransformStorage()
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  */
82 TransformStorage::TransformStorage(const StampedTransform& data,
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 /** Copy constructor.
94  * @param rhs storage to copy
95  */
97 {
98  *this = rhs;
99 }
100 
101 
102 /** @class TimeCache <tf/time_cache.h>
103  * Time based transform cache.
104  * A class to keep a sorted linked list in time. This builds and
105  * maintains a list of timestamped data. And provides lookup
106  * functions to get data out as a function of time.
107  */
108 
109 /** Constructor.
110  * @param max_storage_time maximum time in seconds to cache, defaults to 10 seconds
111  */
112 TimeCache::TimeCache(float max_storage_time)
113 : max_storage_time_(max_storage_time)
114 {}
115 
116 /** Create extrapolation error string.
117  * @param t0 requested time
118  * @param t1 available time
119  * @param error_str upon return contains the error string.
120  */
121 // hoisting these into separate functions causes an ~8% speedup.
122 // Removing calling them altogether adds another ~10%
123 void
124 create_extrapolation_exception1(fawkes::Time t0, fawkes::Time t1, std::string* error_str)
125 {
126  if (error_str)
127  {
128  char *tmp;
129  if (asprintf(&tmp, "Lookup would require extrapolation at time %li.%li, "
130  "but only time %li.%li is in the buffer", t0.get_sec(), t0.get_nsec(),
131  t1.get_sec(), t1.get_nsec()) != -1)
132  {
133  *error_str = tmp;
134  free(tmp);
135  }
136  }
137 }
138 
139 /** Create extrapolation error string.
140  * @param t0 requested time
141  * @param t1 available time
142  * @param error_str upon return contains the error string.
143  */
144 void
145 create_extrapolation_exception2(fawkes::Time t0, fawkes::Time t1, std::string* error_str)
146 {
147  if (error_str)
148  {
149  char *tmp;
150  if (asprintf(&tmp,"Lookup would require extrapolation into the future. "
151  "Requested time %s, but the latest data is at time %s",
152  t0.str(), t1.str()) != -1)
153  {
154  *error_str = tmp;
155  free(tmp);
156  }
157  }
158 }
159 
160 /** Create extrapolation error string.
161  * @param t0 requested time
162  * @param t1 available time
163  * @param error_str upon return contains the error string.
164  */
165 void
166 create_extrapolation_exception3(fawkes::Time t0, fawkes::Time t1, std::string* error_str)
167 {
168  if (error_str)
169  {
170  char *tmp;
171  if (asprintf(&tmp,"Lookup would require extrapolation into the past. "
172  "Requested time %s, but the latest data is at time %s",
173  t0.str(), t1.str()) != -1)
174  {
175  *error_str = tmp;
176  free(tmp);
177  }
178  }
179 }
180 
181 /// A helper function for getData
182 //Assumes storage is already locked for it
183 uint8_t
184 TimeCache::find_closest(TransformStorage*& one, TransformStorage*& two,
185  fawkes::Time target_time, std::string* error_str)
186 {
187  //No values stored
188  if (storage_.empty()) {
189  return 0;
190  }
191 
192  //If time == 0 return the latest
193  if (target_time.is_zero()) {
194  one = &storage_.front();
195  return 1;
196  }
197 
198  // One value stored
199  if (++storage_.begin() == storage_.end()) {
200  TransformStorage& ts = *storage_.begin();
201  if (ts.stamp == target_time) {
202  one = &ts;
203  return 1;
204  } else {
205  create_extrapolation_exception1(target_time, ts.stamp, error_str);
206  return 0;
207  }
208  }
209 
210  fawkes::Time latest_time = (*storage_.begin()).stamp;
211  fawkes::Time earliest_time = (*(storage_.rbegin())).stamp;
212 
213  if (target_time == latest_time) {
214  one = &(*storage_.begin());
215  return 1;
216  } else if (target_time == earliest_time) {
217  one = &(*storage_.rbegin());
218  return 1;
219  } else if (target_time > latest_time) {
220  // Catch cases that would require extrapolation
221  create_extrapolation_exception2(target_time, latest_time, error_str);
222  return 0;
223  } else if (target_time < earliest_time) {
224  create_extrapolation_exception3(target_time, earliest_time, error_str);
225  return 0;
226  }
227 
228  //At least 2 values stored
229  //Find the first value less than the target value
230  L_TransformStorage::iterator storage_it = storage_.begin();
231  while(storage_it != storage_.end()) {
232  if (storage_it->stamp <= target_time) break;
233  storage_it++;
234  }
235 
236  //Finally the case were somewhere in the middle Guarenteed no extrapolation :-)
237  one = &*(storage_it); //Older
238  two = &*(--storage_it); //Newer
239  return 2;
240 }
241 
242 
243 void
244 TimeCache::interpolate(const TransformStorage& one,
245  const TransformStorage& two,
246  fawkes::Time time, TransformStorage& output)
247 {
248  // Check for zero distance case
249  if( two.stamp == one.stamp ) {
250  output = two;
251  return;
252  }
253  //Calculate the ratio
254  btScalar ratio =
255  (time.in_sec() - one.stamp.in_sec()) /
256  (two.stamp.in_sec() - one.stamp.in_sec());
257 
258  //Interpolate translation
259  output.translation_.setInterpolate3(one.translation_, two.translation_, ratio);
260 
261  //Interpolate rotation
262  output.rotation_ = slerp( one.rotation_, two.rotation_, ratio);
263 
264  output.stamp = one.stamp;
265  output.frame_id_ = one.frame_id_;
266  output.child_frame_id_ = one.child_frame_id_;
267 }
268 
269 /** Get data.
270  * @param time time for which go get data
271  * @param data_out upon return contains requested data
272  * @param error_str error stirng
273 * @return false if data not available
274 */
275 bool
277  std::string* error_str)
278 {
279  TransformStorage* p_temp_1 = NULL;
280  TransformStorage* p_temp_2 = NULL;
281 
282  int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
283  if (num_nodes == 0) {
284  return false;
285  } else if (num_nodes == 1) {
286  data_out = *p_temp_1;
287  } else if (num_nodes == 2) {
288  if( p_temp_1->frame_id_ == p_temp_2->frame_id_) {
289  interpolate(*p_temp_1, *p_temp_2, time, data_out);
290  } else {
291  data_out = *p_temp_1;
292  }
293  }
294 
295  return true;
296 }
297 
298 /** Get parent frame number
299  * @param time point in time
300  * @param error_str error string
301  * @return frame number
302  */
303 CompactFrameID
304 TimeCache::get_parent(fawkes::Time time, std::string* error_str)
305 {
306  TransformStorage* p_temp_1 = NULL;
307  TransformStorage* p_temp_2 = NULL;
308 
309  int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str);
310  if (num_nodes == 0) {
311  return 0;
312  }
313 
314  return p_temp_1->frame_id_;
315 }
316 
317 
318 /** Insert data.
319  * @param new_data data to insert
320  * @return true on success, false otherwise
321  */
322 bool
324 {
325  L_TransformStorage::iterator storage_it = storage_.begin();
326 
327  if(storage_it != storage_.end()) {
328  if (storage_it->stamp > new_data.stamp + max_storage_time_) {
329  return false;
330  }
331  }
332 
333 
334  while(storage_it != storage_.end()) {
335  if (storage_it->stamp <= new_data.stamp)
336  break;
337  storage_it++;
338  }
339  storage_.insert(storage_it, new_data);
340 
341  prune_list();
342  return true;
343 }
344 
345 /** Clear storage. */
346 void
348 {
349  storage_.clear();
350 }
351 
352 /** Get storage list length.
353  * @return storage list length
354  */
355 unsigned int
357 {
358  return storage_.size();
359 }
360 
361 /** Get latest time and parent frame number.
362  * @return latest time and parent frame number
363  */
364 P_TimeAndFrameID
366 {
367  if (storage_.empty()) {
368  return std::make_pair(fawkes::Time(), 0);
369  }
370 
371  const TransformStorage& ts = storage_.front();
372  return std::make_pair(ts.stamp, ts.frame_id_);
373 }
374 
375 /** Get latest timestamp from cache.
376  * @return latest timestamp
377  */
380 {
381  if (storage_.empty()) return fawkes::Time(); //empty list case
382  return storage_.front().stamp;
383 }
384 
385 /** Get oldest timestamp from cache.
386  * @return oldest time stamp.
387  */
390 {
391  if (storage_.empty()) return fawkes::Time(); //empty list case
392  return storage_.back().stamp;
393 }
394 
395 /** Prune storage list based on maximum cache lifetime. */
396 void
397 TimeCache::prune_list()
398 {
399  fawkes::Time latest_time = storage_.begin()->stamp;
400 
401  while(!storage_.empty() &&
402  storage_.back().stamp + max_storage_time_ < latest_time)
403  {
404  storage_.pop_back();
405  }
406 
407 }
408 
409 
410 } // end namespace tf
411 } // end namespace fawkes