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 
66 namespace fawkes {
67 namespace tf {
68 
69 class TimeCacheInterface;
70 typedef std::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
71 
72 /// Internal error values
73 enum 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  */
106 class BufferCore
107 {
108 public:
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 
159 protected:
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
187  float cache_time_;
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
fawkes::tf::StampedTransform
Transform that contains a timestamp and frame IDs.
Definition: types.h:98
fawkes::tf::BufferCore::create_connectivity_error_string
void create_connectivity_error_string(CompactFrameID source_frame, CompactFrameID target_frame, std::string *out) const
Create error string.
Definition: buffer_core.cpp:852
fawkes::tf::BufferCore::frameIDs_reverse
std::vector< std::string > frameIDs_reverse
A map from CompactFrameID frame_id_numbers to string for debugging and output.
Definition: buffer_core.h:189
fawkes::tf::BufferCore::can_transform_no_lock
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.
Definition: buffer_core.cpp:683
fawkes::tf::BufferCore::frames_
V_TimeCacheInterface frames_
The pointers to potential frames that the tree can be made of.
Definition: buffer_core.h:179
fawkes::tf::BufferCore::allocate_frame
TimeCacheInterfacePtr allocate_frame(CompactFrameID cfid, bool is_static)
Allocate a new frame cache.
Definition: buffer_core.cpp:256
fawkes::tf::BufferCore::lookup_or_insert_frame_number
CompactFrameID lookup_or_insert_frame_number(const std::string &frameid_str)
String to number for frame lookup with dynamic allocation of new frames.
Definition: buffer_core.cpp:817
fawkes::tf::BufferCore::lookup_transform
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
Definition: buffer_core.cpp:561
fawkes::tf::BufferCore::validate_frame_id
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.
Definition: buffer_core.cpp:107
fawkes::tf::BufferCore::warn_frame_id
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:81
fawkes::tf::BufferCore::frame_mutex_
std::mutex frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
Definition: buffer_core.h:182
fawkes::tf::BufferCore::get_frame
TimeCacheInterfacePtr get_frame(CompactFrameID c_frame_id) const
Accessor to get frame cache.
Definition: buffer_core.cpp:787
fawkes::tf::BufferCore::BufferCore
BufferCore(float cache_time=DEFAULT_CACHE_TIME)
Constructor.
Definition: buffer_core.cpp:134
fawkes::tf::BufferCore::get_cache_length
float get_cache_length()
Get the duration over which this transformer will cache.
Definition: buffer_core.h:161
fawkes::tf::BufferCore::all_frames_as_string
std::string all_frames_as_string() const
A way to see what frames have been cached.
Definition: buffer_core.cpp:871
fawkes::tf::BufferCore::can_transform
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.
Definition: buffer_core.cpp:730
fawkes::tf::BufferCore::M_StringToCompactFrameID
std::unordered_map< std::string, CompactFrameID > M_StringToCompactFrameID
A map from string frame ids to CompactFrameID.
Definition: buffer_core.h:185
fawkes
fawkes::tf::BufferCore::clear
void clear()
Clear all data.
Definition: buffer_core.cpp:147
fawkes::tf::BufferCore::cache_time_
float cache_time_
How long to cache transform history.
Definition: buffer_core.h:194
fawkes::tf::BufferCore::get_latest_common_time
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.
Definition: buffer_core.cpp:933
fawkes::tf::BufferCore::all_frames_as_string_no_lock
std::string all_frames_as_string_no_lock() const
A way to see what frames have been cached.
Definition: buffer_core.cpp:882
fawkes::tf::BufferCore::frameIDs_
M_StringToCompactFrameID frameIDs_
Mapping from frame string IDs to compact IDs.
Definition: buffer_core.h:187
fawkes::tf::BufferCore::V_TimeCacheInterface
std::vector< TimeCacheInterfacePtr > V_TimeCacheInterface
Vector data type for frame caches.
Definition: buffer_core.h:175
fawkes::tf::BufferCore::walk_to_top_parent
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.
Definition: buffer_core.cpp:285
fawkes::Time
Definition: time.h:96
fawkes::tf::BufferCore::lookup_frame_number
CompactFrameID lookup_frame_number(const std::string &frameid_str) const
String to number for frame lookup with dynamic allocation of new frames.
Definition: buffer_core.cpp:801
fawkes::tf::BufferCore::can_transform_internal
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.
Definition: buffer_core.cpp:713
fawkes::tf::BufferCore::set_transform
bool set_transform(const StampedTransform &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
Definition: buffer_core.cpp:170
fawkes::tf::BufferCore::lookup_frame_string
const std::string & lookup_frame_string(CompactFrameID frame_id_num) const
Number to string frame lookup may throw LookupException if number invalid.
Definition: buffer_core.cpp:838
fawkes::tf::BufferCore::all_frames_as_YAML
std::string all_frames_as_YAML() const
Get latest frames as YAML.
Definition: buffer_core.cpp:1164
fawkes::tf::BufferCore::DEFAULT_CACHE_TIME
static const int DEFAULT_CACHE_TIME
The default amount of time to cache data in seconds.
Definition: buffer_core.h:116
fawkes::tf::BufferCore::frame_authority_
std::map< CompactFrameID, std::string > frame_authority_
A map to lookup the most recent authority for a given frame.
Definition: buffer_core.h:191
fawkes::tf::BufferCore
A Class which provides coordinate transforms between any two frames in a system.
Definition: buffer_core.h:113
fawkes::tf::BufferCore::MAX_GRAPH_DEPTH
static const uint32_t MAX_GRAPH_DEPTH
Maximum number of times to recurse before assuming the tree has a loop.
Definition: buffer_core.h:117