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