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
62namespace fawkes {
63namespace tf {
64
65typedef std::pair<fawkes::Time, CompactFrameID> P_TimeAndFrameID;
66
67class TimeCacheInterface;
68typedef std::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
69
71{
72public:
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;
89
90 virtual const L_TransformStorage &get_storage() const = 0;
92};
93
95{
96public:
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
123private:
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{
143public:
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
162private:
163 TransformStorage storage_;
164 L_TransformStorage storage_as_list_;
165};
166
167} // end namespace tf
168} // end namespace fawkes
169
170#endif
A class for handling time.
Definition: time.h:93
Transform cache for static transforms.
Definition: time_cache.h:142
virtual bool get_data(fawkes::Time time, TransformStorage &data_out, std::string *error_str=0)
Get data.
virtual void clear_list()
Clear storage.
virtual bool insert_data(const TransformStorage &new_data)
Insert data.
virtual unsigned int get_list_length() const
Debugging information methods.
virtual P_TimeAndFrameID get_latest_time_and_parent()
Get latest time and parent frame number.
virtual ~StaticCache()
Destructor .
StaticCache()
Constructor.
virtual L_TransformStorage get_storage_copy() const
Get copy of storage elements.
virtual const L_TransformStorage & get_storage() const
Get storage list.
virtual TimeCacheInterfacePtr clone(const fawkes::Time &look_back_until=fawkes::Time(0, 0)) const
Create a copy of this time cache.
virtual fawkes::Time get_latest_timestamp() const
Get latest timestamp from cache.
virtual CompactFrameID get_parent(fawkes::Time time, std::string *error_str)
Get parent frame number.
virtual fawkes::Time get_oldest_timestamp() const
Get oldest timestamp from cache.
Interface for transform time caches.
Definition: time_cache.h:71
virtual const L_TransformStorage & get_storage() const =0
Get storage list.
virtual CompactFrameID get_parent(fawkes::Time time, std::string *error_str)=0
Get parent frame number.
virtual fawkes::Time get_latest_timestamp() const =0
Get latest timestamp from cache.
virtual TimeCacheInterfacePtr clone(const fawkes::Time &look_back_until=fawkes::Time(0, 0)) const =0
Create a copy of this time cache.
virtual fawkes::Time get_oldest_timestamp() const =0
Get oldest timestamp from cache.
virtual void clear_list()=0
Clear storage.
virtual bool insert_data(const TransformStorage &new_data)=0
Insert data.
virtual P_TimeAndFrameID get_latest_time_and_parent()=0
Get latest time and parent frame number.
virtual unsigned int get_list_length() const =0
Debugging information methods.
virtual bool get_data(fawkes::Time time, TransformStorage &data_out, std::string *error_str=0)=0
Get data.
std::list< TransformStorage > L_TransformStorage
List of stored transforms.
Definition: time_cache.h:74
virtual L_TransformStorage get_storage_copy() const =0
Get copy of storage elements.
Time based transform cache.
Definition: time_cache.h:95
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
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
virtual unsigned int get_list_length() const
Debugging information methods.
Definition: time_cache.cpp:410
virtual CompactFrameID get_parent(fawkes::Time time, std::string *error_str)
Get parent frame number.
Definition: time_cache.cpp:368
virtual fawkes::Time get_oldest_timestamp() const
Get oldest timestamp from cache.
Definition: time_cache.cpp:447
virtual fawkes::Time get_latest_timestamp() const
Get latest timestamp from cache.
Definition: time_cache.cpp:439
virtual L_TransformStorage get_storage_copy() const
Get copy of storage elements.
Definition: time_cache.cpp:422
TimeCache(float max_storage_time=DEFAULT_MAX_STORAGE_TIME)
Constructor.
Definition: time_cache.cpp:154
static const int MIN_INTERPOLATION_DISTANCE
Number of nano-seconds to not interpolate below.
Definition: time_cache.h:98
virtual ~TimeCache()
Destructor.
Definition: time_cache.cpp:159
static const int64_t DEFAULT_MAX_STORAGE_TIME
default value of 10 seconds storage
Definition: time_cache.h:102
virtual bool insert_data(const TransformStorage &new_data)
Insert data.
Definition: time_cache.cpp:382
virtual P_TimeAndFrameID get_latest_time_and_parent()
Get latest time and parent frame number.
Definition: time_cache.cpp:428
virtual void clear_list()
Clear storage.
Definition: time_cache.cpp:404
virtual const L_TransformStorage & get_storage() const
Get storage list.
Definition: time_cache.cpp:416
virtual bool get_data(fawkes::Time time, TransformStorage &data_out, std::string *error_str=0)
Get data.
Definition: time_cache.cpp:346
Storage for transforms and their parent.
Fawkes library namespace.