Fawkes API
Fawkes Development Version
|
00001 /*************************************************************************** 00002 * time_cache.cpp - Fawkes tf time cache (based on ROS tf) 00003 * 00004 * Created: Thu Oct 20 11:26:40 2011 00005 * Copyright 2011 Tim Niemueller [www.niemueller.de] 00006 ****************************************************************************/ 00007 00008 /* This program is free software; you can redistribute it and/or modify 00009 * it under the terms of the GNU General Public License as published by 00010 * the Free Software Foundation; either version 2 of the License, or 00011 * (at your option) any later version. A runtime exception applies to 00012 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00020 */ 00021 00022 /* This code is based on ROS tf with the following copyright and license: 00023 * 00024 * Copyright (c) 2008, Willow Garage, Inc. 00025 * All rights reserved. 00026 * 00027 * Redistribution and use in source and binary forms, with or without 00028 * modification, are permitted provided that the following conditions are met: 00029 * 00030 * * Redistributions of source code must retain the above copyright 00031 * notice, this list of conditions and the following disclaimer. 00032 * * Redistributions in binary form must reproduce the above copyright 00033 * notice, this list of conditions and the following disclaimer in the 00034 * documentation and/or other materials provided with the distribution. 00035 * * Neither the name of the Willow Garage, Inc. nor the names of its 00036 * contributors may be used to endorse or promote products derived from 00037 * this software without specific prior written permission. 00038 * 00039 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00040 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00041 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00042 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00043 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00044 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00045 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00046 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00047 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00048 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00049 * POSSIBILITY OF SUCH DAMAGE. 00050 */ 00051 00052 #include <tf/time_cache.h> 00053 00054 #include <cstdio> 00055 00056 namespace fawkes { 00057 namespace tf { 00058 #if 0 /* just to make Emacs auto-indent happy */ 00059 } 00060 } 00061 #endif 00062 00063 /** @class TransformStorage <tf/time_cache.h> 00064 * Storage for transforms and their parent. 00065 * 00066 * @fn TransformStorage & TransformStorage::operator=(const TransformStorage& rhs) 00067 * Assignment operator. 00068 * @param rhs storage to assign 00069 * @return reference to this instance 00070 */ 00071 00072 /** Constructor. */ 00073 TransformStorage::TransformStorage() 00074 { 00075 } 00076 00077 /** Constructor. 00078 * @param data initial stamped transform data 00079 * @param frame_id parent frame ID 00080 * @param child_frame_id child frame ID 00081 */ 00082 TransformStorage::TransformStorage(const StampedTransform& data, 00083 CompactFrameID frame_id, 00084 CompactFrameID child_frame_id) 00085 : rotation_(data.getRotation()) 00086 , translation_(data.getOrigin()) 00087 , stamp(data.stamp) 00088 , frame_id_(frame_id) 00089 , child_frame_id_(child_frame_id) 00090 { } 00091 00092 00093 /** Copy constructor. 00094 * @param rhs storage to copy 00095 */ 00096 TransformStorage::TransformStorage(const TransformStorage& rhs) 00097 { 00098 *this = rhs; 00099 } 00100 00101 00102 /** @class TimeCache <tf/time_cache.h> 00103 * Time based transform cache. 00104 * A class to keep a sorted linked list in time. This builds and 00105 * maintains a list of timestamped data. And provides lookup 00106 * functions to get data out as a function of time. 00107 */ 00108 00109 /** Constructor. 00110 * @param max_storage_time maximum time in seconds to cache, defaults to 10 seconds 00111 */ 00112 TimeCache::TimeCache(float max_storage_time) 00113 : max_storage_time_(max_storage_time) 00114 {} 00115 00116 /** Create extrapolation error string. 00117 * @param t0 requested time 00118 * @param t1 available time 00119 * @param error_str upon return contains the error string. 00120 */ 00121 // hoisting these into separate functions causes an ~8% speedup. 00122 // Removing calling them altogether adds another ~10% 00123 void 00124 create_extrapolation_exception1(fawkes::Time t0, fawkes::Time t1, std::string* error_str) 00125 { 00126 if (error_str) 00127 { 00128 char *tmp; 00129 if (asprintf(&tmp, "Lookup would require extrapolation at time %li.%li, " 00130 "but only time %li.%li is in the buffer", t0.get_sec(), t0.get_nsec(), 00131 t1.get_sec(), t1.get_nsec()) != -1) 00132 { 00133 *error_str = tmp; 00134 free(tmp); 00135 } 00136 } 00137 } 00138 00139 /** Create extrapolation error string. 00140 * @param t0 requested time 00141 * @param t1 available time 00142 * @param error_str upon return contains the error string. 00143 */ 00144 void 00145 create_extrapolation_exception2(fawkes::Time t0, fawkes::Time t1, std::string* error_str) 00146 { 00147 if (error_str) 00148 { 00149 char *tmp; 00150 if (asprintf(&tmp,"Lookup would require extrapolation into the future. " 00151 "Requested time %s, but the latest data is at time %s", 00152 t0.str(), t1.str()) != -1) 00153 { 00154 *error_str = tmp; 00155 free(tmp); 00156 } 00157 } 00158 } 00159 00160 /** Create extrapolation error string. 00161 * @param t0 requested time 00162 * @param t1 available time 00163 * @param error_str upon return contains the error string. 00164 */ 00165 void 00166 create_extrapolation_exception3(fawkes::Time t0, fawkes::Time t1, std::string* error_str) 00167 { 00168 if (error_str) 00169 { 00170 char *tmp; 00171 if (asprintf(&tmp,"Lookup would require extrapolation into the past. " 00172 "Requested time %s, but the latest data is at time %s", 00173 t0.str(), t1.str()) != -1) 00174 { 00175 *error_str = tmp; 00176 free(tmp); 00177 } 00178 } 00179 } 00180 00181 /// A helper function for getData 00182 //Assumes storage is already locked for it 00183 uint8_t 00184 TimeCache::find_closest(TransformStorage*& one, TransformStorage*& two, 00185 fawkes::Time target_time, std::string* error_str) 00186 { 00187 //No values stored 00188 if (storage_.empty()) { 00189 return 0; 00190 } 00191 00192 //If time == 0 return the latest 00193 if (target_time.is_zero()) { 00194 one = &storage_.front(); 00195 return 1; 00196 } 00197 00198 // One value stored 00199 if (++storage_.begin() == storage_.end()) { 00200 TransformStorage& ts = *storage_.begin(); 00201 if (ts.stamp == target_time) { 00202 one = &ts; 00203 return 1; 00204 } else { 00205 create_extrapolation_exception1(target_time, ts.stamp, error_str); 00206 return 0; 00207 } 00208 } 00209 00210 fawkes::Time latest_time = (*storage_.begin()).stamp; 00211 fawkes::Time earliest_time = (*(storage_.rbegin())).stamp; 00212 00213 if (target_time == latest_time) { 00214 one = &(*storage_.begin()); 00215 return 1; 00216 } else if (target_time == earliest_time) { 00217 one = &(*storage_.rbegin()); 00218 return 1; 00219 } else if (target_time > latest_time) { 00220 // Catch cases that would require extrapolation 00221 create_extrapolation_exception2(target_time, latest_time, error_str); 00222 return 0; 00223 } else if (target_time < earliest_time) { 00224 create_extrapolation_exception3(target_time, earliest_time, error_str); 00225 return 0; 00226 } 00227 00228 //At least 2 values stored 00229 //Find the first value less than the target value 00230 L_TransformStorage::iterator storage_it = storage_.begin(); 00231 while(storage_it != storage_.end()) { 00232 if (storage_it->stamp <= target_time) break; 00233 storage_it++; 00234 } 00235 00236 //Finally the case were somewhere in the middle Guarenteed no extrapolation :-) 00237 one = &*(storage_it); //Older 00238 two = &*(--storage_it); //Newer 00239 return 2; 00240 } 00241 00242 00243 void 00244 TimeCache::interpolate(const TransformStorage& one, 00245 const TransformStorage& two, 00246 fawkes::Time time, TransformStorage& output) 00247 { 00248 // Check for zero distance case 00249 if( two.stamp == one.stamp ) { 00250 output = two; 00251 return; 00252 } 00253 //Calculate the ratio 00254 btScalar ratio = 00255 (time.in_sec() - one.stamp.in_sec()) / 00256 (two.stamp.in_sec() - one.stamp.in_sec()); 00257 00258 //Interpolate translation 00259 output.translation_.setInterpolate3(one.translation_, two.translation_, ratio); 00260 00261 //Interpolate rotation 00262 output.rotation_ = slerp( one.rotation_, two.rotation_, ratio); 00263 00264 output.stamp = one.stamp; 00265 output.frame_id_ = one.frame_id_; 00266 output.child_frame_id_ = one.child_frame_id_; 00267 } 00268 00269 /** Get data. 00270 * @param time time for which go get data 00271 * @param data_out upon return contains requested data 00272 * @param error_str error stirng 00273 * @return false if data not available 00274 */ 00275 bool 00276 TimeCache::get_data(fawkes::Time time, TransformStorage & data_out, 00277 std::string* error_str) 00278 { 00279 TransformStorage* p_temp_1 = NULL; 00280 TransformStorage* p_temp_2 = NULL; 00281 00282 int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str); 00283 if (num_nodes == 0) { 00284 return false; 00285 } else if (num_nodes == 1) { 00286 data_out = *p_temp_1; 00287 } else if (num_nodes == 2) { 00288 if( p_temp_1->frame_id_ == p_temp_2->frame_id_) { 00289 interpolate(*p_temp_1, *p_temp_2, time, data_out); 00290 } else { 00291 data_out = *p_temp_1; 00292 } 00293 } 00294 00295 return true; 00296 } 00297 00298 /** Get parent frame number 00299 * @param time point in time 00300 * @param error_str error string 00301 * @return frame number 00302 */ 00303 CompactFrameID 00304 TimeCache::get_parent(fawkes::Time time, std::string* error_str) 00305 { 00306 TransformStorage* p_temp_1 = NULL; 00307 TransformStorage* p_temp_2 = NULL; 00308 00309 int num_nodes = find_closest(p_temp_1, p_temp_2, time, error_str); 00310 if (num_nodes == 0) { 00311 return 0; 00312 } 00313 00314 return p_temp_1->frame_id_; 00315 } 00316 00317 00318 /** Insert data. 00319 * @param new_data data to insert 00320 * @return true on success, false otherwise 00321 */ 00322 bool 00323 TimeCache::insert_data(const TransformStorage& new_data) 00324 { 00325 L_TransformStorage::iterator storage_it = storage_.begin(); 00326 00327 if(storage_it != storage_.end()) { 00328 if (storage_it->stamp > new_data.stamp + max_storage_time_) { 00329 return false; 00330 } 00331 } 00332 00333 00334 while(storage_it != storage_.end()) { 00335 if (storage_it->stamp <= new_data.stamp) 00336 break; 00337 storage_it++; 00338 } 00339 storage_.insert(storage_it, new_data); 00340 00341 prune_list(); 00342 return true; 00343 } 00344 00345 /** Clear storage. */ 00346 void 00347 TimeCache::clear_list() 00348 { 00349 storage_.clear(); 00350 } 00351 00352 /** Get storage list length. 00353 * @return storage list length 00354 */ 00355 unsigned int 00356 TimeCache::get_list_length() const 00357 { 00358 return storage_.size(); 00359 } 00360 00361 /** Get latest time and parent frame number. 00362 * @return latest time and parent frame number 00363 */ 00364 P_TimeAndFrameID 00365 TimeCache::get_latest_time_and_parent() const 00366 { 00367 if (storage_.empty()) { 00368 return std::make_pair(fawkes::Time(), 0); 00369 } 00370 00371 const TransformStorage& ts = storage_.front(); 00372 return std::make_pair(ts.stamp, ts.frame_id_); 00373 } 00374 00375 /** Get latest timestamp from cache. 00376 * @return latest timestamp 00377 */ 00378 fawkes::Time 00379 TimeCache::get_latest_timestamp() const 00380 { 00381 if (storage_.empty()) return fawkes::Time(); //empty list case 00382 return storage_.front().stamp; 00383 } 00384 00385 /** Get oldest timestamp from cache. 00386 * @return oldest time stamp. 00387 */ 00388 fawkes::Time 00389 TimeCache::get_oldest_timestamp() const 00390 { 00391 if (storage_.empty()) return fawkes::Time(); //empty list case 00392 return storage_.back().stamp; 00393 } 00394 00395 /** Prune storage list based on maximum cache lifetime. */ 00396 void 00397 TimeCache::prune_list() 00398 { 00399 fawkes::Time latest_time = storage_.begin()->stamp; 00400 00401 while(!storage_.empty() && 00402 storage_.back().stamp + max_storage_time_ < latest_time) 00403 { 00404 storage_.pop_back(); 00405 } 00406 00407 } 00408 00409 00410 } // end namespace tf 00411 } // end namespace fawkes