Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
time_cache.h
1 /***************************************************************************
2  * time_cache.h - Fawkes tf time cache (based on ROS tf)
3  *
4  * Created: Thu Oct 20 11:09:58 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 #ifndef __LIBS_TF_TIME_CACHE_H_
53 #define __LIBS_TF_TIME_CACHE_H_
54 
55 #include <tf/types.h>
56 
57 #include <LinearMath/btTransform.h>
58 #include <list>
59 #include <stdint.h>
60 
61 namespace fawkes {
62  namespace tf {
63 #if 0 /* just to make Emacs auto-indent happy */
64  }
65 }
66 #endif
67 
68 enum ExtrapolationMode {
69  ONE_VALUE,
70  INTERPOLATE,
71  EXTRAPOLATE_BACK,
72  EXTRAPOLATE_FORWARD
73 };
74 
75 typedef std::pair<fawkes::Time, CompactFrameID> P_TimeAndFrameID;
76 
78 {
79  public:
81  TransformStorage(const StampedTransform& data, CompactFrameID frame_id,
82  CompactFrameID child_frame_id);
84 
85  TransformStorage& operator=(const TransformStorage& rhs)
86  {
87  rotation_ = rhs.rotation_;
88  translation_ = rhs.translation_;
89  stamp = rhs.stamp;
90  frame_id_ = rhs.frame_id_;
91  child_frame_id_ = rhs.child_frame_id_;
92  return *this;
93  }
94 
95  btQuaternion rotation_; ///< rotation quaternio
96  btVector3 translation_; ///< translation vector
97  fawkes::Time stamp; ///< time stamp
98  CompactFrameID frame_id_; ///< parent/reference frame number
99  CompactFrameID child_frame_id_; ///< child frame number
100 };
101 
102 
103 
105 {
106  public:
107  /// Number of nano-seconds to not interpolate below.
108  static const int MIN_INTERPOLATION_DISTANCE = 5;
109  /// Maximum length of linked list, to make sure not to be able to use unlimited memory.
110  static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000;
111 
112  TimeCache(float max_storage_time = 10.0);
113 
114  bool get_data(fawkes::Time time, TransformStorage & data_out,
115  std::string* error_str = 0);
116  bool insert_data(const TransformStorage& new_data);
117  void clear_list();
118  CompactFrameID get_parent(fawkes::Time time, std::string* error_str);
119  P_TimeAndFrameID get_latest_time_and_parent() const;
120 
121  /// Debugging information methods
122  unsigned int get_list_length() const;
123  fawkes::Time get_latest_timestamp() const;
124  fawkes::Time get_oldest_timestamp() const;
125 
126  private:
127  typedef std::list<TransformStorage> L_TransformStorage;
128  L_TransformStorage storage_;
129 
130  float max_storage_time_;
131 
132 
133  inline uint8_t find_closest(TransformStorage*& one, TransformStorage*& two,
134  fawkes::Time target_time, std::string* error_str);
135 
136  inline void interpolate(const TransformStorage& one, const TransformStorage& two,
137  fawkes::Time time, TransformStorage& output);
138 
139  void prune_list();
140 };
141 
142 
143 } // end namespace tf
144 } // end namespace fawkes
145 
146 #endif