Fawkes API  Fawkes Development Version
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 #include <tf/transform_storage.h>
57 
58 #include <list>
59 #include <cstdint>
60 #include <memory>
61 
62 namespace fawkes {
63  namespace tf {
64 #if 0 /* just to make Emacs auto-indent happy */
65  }
66 }
67 #endif
68 
69 typedef std::pair<fawkes::Time, CompactFrameID> P_TimeAndFrameID;
70 
71 class TimeCacheInterface;
72 typedef std::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
73 
75 {
76  public:
77  /** List of stored transforms. */
78  typedef std::list<TransformStorage> L_TransformStorage;
79 
80  virtual TimeCacheInterfacePtr clone(const fawkes::Time &look_back_until = fawkes::Time(0,0)) const = 0;
81  virtual bool get_data(fawkes::Time time, TransformStorage & data_out,
82  std::string* error_str = 0) = 0;
83  virtual bool insert_data(const TransformStorage& new_data) = 0;
84  virtual void clear_list() = 0;
85  virtual CompactFrameID get_parent(fawkes::Time time, std::string* error_str) = 0;
86  virtual P_TimeAndFrameID get_latest_time_and_parent() = 0;
87 
88  /// Debugging information methods
89  virtual unsigned int get_list_length() const = 0;
90  virtual fawkes::Time get_latest_timestamp() const = 0;
91  virtual fawkes::Time get_oldest_timestamp() const = 0;
92 
93  virtual const L_TransformStorage & get_storage() const = 0;
94  virtual L_TransformStorage get_storage_copy() const = 0;
95 };
96 
97 
99 {
100  public:
101 
102  /// Number of nano-seconds to not interpolate below.
103  static const int MIN_INTERPOLATION_DISTANCE = 5;
104  /// Maximum length of linked list, to make sure not to be able to use unlimited memory.
105  static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000;
106  /// default value of 10 seconds storage
107  static const int64_t DEFAULT_MAX_STORAGE_TIME = 1ULL * 1000000000LL; //!< default value of 10 seconds storage
108 
109  TimeCache(float max_storage_time = DEFAULT_MAX_STORAGE_TIME);
110 
111  virtual TimeCacheInterfacePtr clone(const fawkes::Time &look_back_until = fawkes::Time(0,0)) const;
112  virtual bool get_data(fawkes::Time time, TransformStorage & data_out,
113  std::string* error_str = 0);
114  virtual bool insert_data(const TransformStorage& new_data);
115  virtual void clear_list();
116  virtual CompactFrameID get_parent(fawkes::Time time, std::string* error_str);
117  virtual P_TimeAndFrameID get_latest_time_and_parent();
118 
119  virtual const L_TransformStorage & get_storage() const;
120  virtual L_TransformStorage get_storage_copy() const;
121 
122  virtual unsigned int get_list_length() const;
123  virtual fawkes::Time get_latest_timestamp() const;
124  virtual fawkes::Time get_oldest_timestamp() const;
125 
126  private:
127  L_TransformStorage storage_;
128 
129  float max_storage_time_;
130 
131 
132  inline uint8_t find_closest(TransformStorage*& one, TransformStorage*& two,
133  fawkes::Time target_time, std::string* error_str);
134 
135  inline void interpolate(const TransformStorage& one, const TransformStorage& two,
136  fawkes::Time time, TransformStorage& output);
137 
138  void prune_list();
139 };
140 
141 
143 {
144  public:
145  StaticCache();
146 
147  virtual TimeCacheInterfacePtr clone(const fawkes::Time &look_back_until = fawkes::Time(0,0)) const;
148  virtual bool get_data(fawkes::Time time, TransformStorage & data_out,
149  std::string* error_str = 0);
150  virtual bool insert_data(const TransformStorage& new_data);
151  virtual void clear_list();
152  virtual CompactFrameID get_parent(fawkes::Time time, std::string* error_str);
153  virtual P_TimeAndFrameID get_latest_time_and_parent();
154 
155  virtual unsigned int get_list_length() const;
156  virtual fawkes::Time get_latest_timestamp() const;
157  virtual fawkes::Time get_oldest_timestamp() const;
158 
159  virtual const L_TransformStorage & get_storage() const;
160  virtual L_TransformStorage get_storage_copy() const;
161 
162  private:
163  TransformStorage storage_;
164  L_TransformStorage storage_as_list_;
165 };
166 
167 
168 } // end namespace tf
169 } // end namespace fawkes
170 
171 #endif
Interface for transform time caches.
Definition: time_cache.h:74
virtual unsigned int get_list_length() const =0
Debugging information methods.
Fawkes library namespace.
Storage for transforms and their parent.
A class for handling time.
Definition: time.h:91
virtual CompactFrameID get_parent(fawkes::Time time, std::string *error_str)=0
Get parent frame number.
std::list< TransformStorage > L_TransformStorage
List of stored transforms.
Definition: time_cache.h:78
Time based transform cache.
Definition: time_cache.h:98
virtual const L_TransformStorage & get_storage() const =0
Get storage list.
virtual P_TimeAndFrameID get_latest_time_and_parent()=0
Get latest time and parent frame number.
virtual fawkes::Time get_latest_timestamp() const =0
Get latest timestamp from cache.
virtual bool insert_data(const TransformStorage &new_data)=0
Insert data.
Transform cache for static transforms.
Definition: time_cache.h:142
virtual fawkes::Time get_oldest_timestamp() const =0
Get oldest timestamp from cache.
virtual void clear_list()=0
Clear storage.
virtual TimeCacheInterfacePtr clone(const fawkes::Time &look_back_until=fawkes::Time(0, 0)) const =0
Create a copy of this time cache.
virtual L_TransformStorage get_storage_copy() const =0
Get copy of storage elements.
virtual bool get_data(fawkes::Time time, TransformStorage &data_out, std::string *error_str=0)=0
Get data.