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