Fawkes API Fawkes Development Version
buffer_core.h
1/***************************************************************************
2 * buffer_core.h - Fawkes tf buffer core (based on ROS tf2)
3 *
4 * Created: Mon Oct 26 11:02:22 2015
5 * Copyright 2015 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 tf2 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_BUFFER_CORE_H_
53#define _LIBS_TF_BUFFER_CORE_H_
54
55#include <tf/transform_storage.h>
56#include <tf/types.h>
57#include <utils/time/time.h>
58
59#include <map>
60#include <memory>
61#include <mutex>
62#include <string>
63#include <unordered_map>
64#include <vector>
65
66namespace fawkes {
67namespace tf {
68
69class TimeCacheInterface;
70typedef std::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
71
72/// Internal error values
73enum ErrorValues {
74 NO_ERROR = 0, ///< No error occured.
75 LOOKUP_ERROR = 1, ///< Frame ID lookup error
76 CONNECTIVITY_ERROR = 2, ///< No connection between frames found
77 EXTRAPOLATION_ERROR = 3, ///< Extrapolation required out of limits.
78 INVALID_ARGUMENT_ERROR = 4, ///< Invalid argument
79 TIMEOUT_ERROR = 5, ///< timeout
80 TRANSFORM_ERROR = 6 ///< generic error
81};
82
83/** \brief A Class which provides coordinate transforms between any two frames in a system.
84 *
85 * This class provides a simple interface to allow recording and
86 * lookup of relationships between arbitrary frames of the system.
87 *
88 * libTF assumes that there is a tree of coordinate frame transforms
89 * which define the relationship between all coordinate frames. For
90 * example your typical robot would have a transform from global to
91 * real world. And then from base to hand, and from base to head.
92 * But Base to Hand really is composed of base to shoulder to elbow to
93 * wrist to hand. libTF is designed to take care of all the
94 * intermediate steps for you.
95 *
96 * Internal Representation
97 * libTF will store frames with the parameters necessary for
98 * generating the transform into that frame from it's parent and a
99 * reference to the parent frame. Frames are designated using an
100 * std::string 0 is a frame without a parent (the top of a tree) The
101 * positions of frames over time must be pushed in.
102 *
103 * All function calls which pass frame ids can potentially throw the
104 * exception tf::LookupException
105 */
107{
108public:
109 static const int DEFAULT_CACHE_TIME = 10; //!< The default amount of time to cache data in seconds
110 static const uint32_t MAX_GRAPH_DEPTH = 1000UL; //!< Maximum number of times to recurse before
111 //! assuming the tree has a loop
112
113 BufferCore(float cache_time = DEFAULT_CACHE_TIME);
114 virtual ~BufferCore(void);
115
116 void clear();
117
118 bool set_transform(const StampedTransform &transform,
119 const std::string & authority,
120 bool is_static = false);
121
122 /*********** Accessors *************/
123 void lookup_transform(const std::string & target_frame,
124 const std::string & source_frame,
125 const fawkes::Time &time,
126 StampedTransform & transform) const;
127
128 void lookup_transform(const std::string & target_frame,
129 const fawkes::Time &target_time,
130 const std::string & source_frame,
131 const fawkes::Time &source_time,
132 const std::string & fixed_frame,
133 StampedTransform & transform) const;
134
135 bool can_transform(const std::string & target_frame,
136 const std::string & source_frame,
137 const fawkes::Time &time,
138 std::string * error_msg = NULL) const;
139
140 bool can_transform(const std::string & target_frame,
141 const fawkes::Time &target_time,
142 const std::string & source_frame,
143 const fawkes::Time &source_time,
144 const std::string & fixed_frame,
145 std::string * error_msg = NULL) const;
146
147 std::string all_frames_as_YAML(double current_time) const;
148 std::string all_frames_as_YAML() const;
149 std::string all_frames_as_string() const;
150
151 /** Get the duration over which this transformer will cache.
152 * @return cache length in seconds */
153 float
155 {
156 return cache_time_;
157 }
158
159protected:
160 /** A way to see what frames have been cached.
161 * Useful for debugging. Use this call internally.
162 */
163 std::string all_frames_as_string_no_lock() const;
164
165 /******************** Internal Storage ****************/
166
167 /// Vector data type for frame caches.
168 typedef std::vector<TimeCacheInterfacePtr> V_TimeCacheInterface;
169 /** The pointers to potential frames that the tree can be made of.
170 * The frames will be dynamically allocated at run time when set the
171 * first time. */
173
174 /** \brief A mutex to protect testing and allocating new frames on the above vector. */
175 mutable std::mutex frame_mutex_;
176
177 /** \brief A map from string frame ids to CompactFrameID */
178 typedef std::unordered_map<std::string, CompactFrameID> M_StringToCompactFrameID;
179 /// Mapping from frame string IDs to compact IDs.
181 /** \brief A map from CompactFrameID frame_id_numbers to string for debugging and output */
182 std::vector<std::string> frameIDs_reverse;
183 /** \brief A map to lookup the most recent authority for a given frame */
184 std::map<CompactFrameID, std::string> frame_authority_;
185
186 /// How long to cache transform history
188
189 /************************* Internal Functions ****************************/
190
191 TimeCacheInterfacePtr get_frame(CompactFrameID c_frame_id) const;
192
193 TimeCacheInterfacePtr allocate_frame(CompactFrameID cfid, bool is_static);
194
195 bool warn_frame_id(const char *function_name_arg, const std::string &frame_id) const;
196 CompactFrameID validate_frame_id(const char * function_name_arg,
197 const std::string &frame_id) const;
198
199 /// String to number for frame lookup with dynamic allocation of new frames
200 CompactFrameID lookup_frame_number(const std::string &frameid_str) const;
201
202 /// String to number for frame lookup with dynamic allocation of new frames
203 CompactFrameID lookup_or_insert_frame_number(const std::string &frameid_str);
204
205 ///Number to string frame lookup may throw LookupException if number invalid
206 const std::string &lookup_frame_string(CompactFrameID frame_id_num) const;
207
208 void create_connectivity_error_string(CompactFrameID source_frame,
209 CompactFrameID target_frame,
210 std::string * out) const;
211
212 int get_latest_common_time(CompactFrameID target_frame,
213 CompactFrameID source_frame,
214 fawkes::Time & time,
215 std::string * error_string) const;
216
217 template <typename F>
218 int walk_to_top_parent(F & f,
219 fawkes::Time time,
220 CompactFrameID target_id,
221 CompactFrameID source_id,
222 std::string * error_string) const;
223
224 template <typename F>
225 int walk_to_top_parent(F & f,
226 fawkes::Time time,
227 CompactFrameID target_id,
228 CompactFrameID source_id,
229 std::string * error_string,
230 std::vector<CompactFrameID> *frame_chain) const;
231
232 bool can_transform_internal(CompactFrameID target_id,
233 CompactFrameID source_id,
234 const fawkes::Time &time,
235 std::string * error_msg) const;
236 bool can_transform_no_lock(CompactFrameID target_id,
237 CompactFrameID source_id,
238 const fawkes::Time &time,
239 std::string * error_msg) const;
240};
241
242} // end namespace tf
243} // end namespace fawkes
244
245#endif
A class for handling time.
Definition: time.h:93
A Class which provides coordinate transforms between any two frames in a system.
Definition: buffer_core.h:107
float cache_time_
How long to cache transform history.
Definition: buffer_core.h:187
bool can_transform_no_lock(CompactFrameID target_id, CompactFrameID source_id, const fawkes::Time &time, std::string *error_msg) const
Test if a transform is possible.
int walk_to_top_parent(F &f, fawkes::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string *error_string) const
Traverse transform tree: walk from frame to top-parent of both.
CompactFrameID lookup_frame_number(const std::string &frameid_str) const
String to number for frame lookup with dynamic allocation of new frames.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
std::map< CompactFrameID, std::string > frame_authority_
A map to lookup the most recent authority for a given frame.
Definition: buffer_core.h:184
M_StringToCompactFrameID frameIDs_
Mapping from frame string IDs to compact IDs.
Definition: buffer_core.h:180
static const uint32_t MAX_GRAPH_DEPTH
Maximum number of times to recurse before assuming the tree has a loop.
Definition: buffer_core.h:110
const std::string & lookup_frame_string(CompactFrameID frame_id_num) const
Number to string frame lookup may throw LookupException if number invalid.
float get_cache_length()
Get the duration over which this transformer will cache.
Definition: buffer_core.h:154
void clear()
Clear all data.
int get_latest_common_time(CompactFrameID target_frame, CompactFrameID source_frame, fawkes::Time &time, std::string *error_string) const
Get latest common time of two frames.
std::mutex frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
Definition: buffer_core.h:175
bool can_transform_internal(CompactFrameID target_id, CompactFrameID source_id, const fawkes::Time &time, std::string *error_msg) const
Test if a transform is possible.
static const int DEFAULT_CACHE_TIME
The default amount of time to cache data in seconds.
Definition: buffer_core.h:109
void create_connectivity_error_string(CompactFrameID source_frame, CompactFrameID target_frame, std::string *out) const
Create error string.
bool warn_frame_id(const char *function_name_arg, const std::string &frame_id) const
Warn if an illegal frame_id was passed.
Definition: buffer_core.cpp:74
TimeCacheInterfacePtr get_frame(CompactFrameID c_frame_id) const
Accessor to get frame cache.
std::string all_frames_as_string() const
A way to see what frames have been cached.
V_TimeCacheInterface frames_
The pointers to potential frames that the tree can be made of.
Definition: buffer_core.h:172
CompactFrameID validate_frame_id(const char *function_name_arg, const std::string &frame_id) const
Check if frame ID is valid and return compact ID.
bool can_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, std::string *error_msg=NULL) const
Test if a transform is possible.
std::vector< std::string > frameIDs_reverse
A map from CompactFrameID frame_id_numbers to string for debugging and output.
Definition: buffer_core.h:182
std::unordered_map< std::string, CompactFrameID > M_StringToCompactFrameID
A map from string frame ids to CompactFrameID.
Definition: buffer_core.h:178
CompactFrameID lookup_or_insert_frame_number(const std::string &frameid_str)
String to number for frame lookup with dynamic allocation of new frames.
std::string all_frames_as_YAML() const
Get latest frames as YAML.
BufferCore(float cache_time=DEFAULT_CACHE_TIME)
Constructor.
bool set_transform(const StampedTransform &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
std::vector< TimeCacheInterfacePtr > V_TimeCacheInterface
Vector data type for frame caches.
Definition: buffer_core.h:168
std::string all_frames_as_string_no_lock() const
A way to see what frames have been cached.
TimeCacheInterfacePtr allocate_frame(CompactFrameID cfid, bool is_static)
Allocate a new frame cache.
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Fawkes library namespace.