Fawkes API
Fawkes Development Version
|
00001 /*************************************************************************** 00002 * time_cache.h - Fawkes tf time cache (based on ROS tf) 00003 * 00004 * Created: Thu Oct 20 11:09:58 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 #ifndef __LIBS_TF_TIME_CACHE_H_ 00053 #define __LIBS_TF_TIME_CACHE_H_ 00054 00055 #include <tf/types.h> 00056 00057 #include <LinearMath/btTransform.h> 00058 #include <list> 00059 #include <stdint.h> 00060 00061 namespace fawkes { 00062 namespace tf { 00063 #if 0 /* just to make Emacs auto-indent happy */ 00064 } 00065 } 00066 #endif 00067 00068 enum ExtrapolationMode { 00069 ONE_VALUE, 00070 INTERPOLATE, 00071 EXTRAPOLATE_BACK, 00072 EXTRAPOLATE_FORWARD 00073 }; 00074 00075 typedef std::pair<fawkes::Time, CompactFrameID> P_TimeAndFrameID; 00076 00077 class TransformStorage 00078 { 00079 public: 00080 TransformStorage(); 00081 TransformStorage(const StampedTransform& data, CompactFrameID frame_id, 00082 CompactFrameID child_frame_id); 00083 TransformStorage(const TransformStorage& rhs); 00084 00085 TransformStorage& operator=(const TransformStorage& rhs) 00086 { 00087 rotation_ = rhs.rotation_; 00088 translation_ = rhs.translation_; 00089 stamp = rhs.stamp; 00090 frame_id_ = rhs.frame_id_; 00091 child_frame_id_ = rhs.child_frame_id_; 00092 return *this; 00093 } 00094 00095 btQuaternion rotation_; ///< rotation quaternio 00096 btVector3 translation_; ///< translation vector 00097 fawkes::Time stamp; ///< time stamp 00098 CompactFrameID frame_id_; ///< parent/reference frame number 00099 CompactFrameID child_frame_id_; ///< child frame number 00100 }; 00101 00102 00103 00104 class TimeCache 00105 { 00106 public: 00107 /// Number of nano-seconds to not interpolate below. 00108 static const int MIN_INTERPOLATION_DISTANCE = 5; 00109 /// Maximum length of linked list, to make sure not to be able to use unlimited memory. 00110 static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000; 00111 00112 TimeCache(float max_storage_time = 10.0); 00113 00114 bool get_data(fawkes::Time time, TransformStorage & data_out, 00115 std::string* error_str = 0); 00116 bool insert_data(const TransformStorage& new_data); 00117 void clear_list(); 00118 CompactFrameID get_parent(fawkes::Time time, std::string* error_str); 00119 P_TimeAndFrameID get_latest_time_and_parent() const; 00120 00121 /// Debugging information methods 00122 unsigned int get_list_length() const; 00123 fawkes::Time get_latest_timestamp() const; 00124 fawkes::Time get_oldest_timestamp() const; 00125 00126 private: 00127 typedef std::list<TransformStorage> L_TransformStorage; 00128 L_TransformStorage storage_; 00129 00130 float max_storage_time_; 00131 00132 00133 inline uint8_t find_closest(TransformStorage*& one, TransformStorage*& two, 00134 fawkes::Time target_time, std::string* error_str); 00135 00136 inline void interpolate(const TransformStorage& one, const TransformStorage& two, 00137 fawkes::Time time, TransformStorage& output); 00138 00139 void prune_list(); 00140 }; 00141 00142 00143 } // end namespace tf 00144 } // end namespace fawkes 00145 00146 #endif