Fawkes API  Fawkes Development Version
transformer.h
1 /***************************************************************************
2  * transformer.h - Fawkes tf transformer (based on ROS tf)
3  *
4  * Created: Tue Oct 18 17:03:47 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_TRANSFORMER_H_
53 #define __LIBS_TF_TRANSFORMER_H_
54 
55 #include <tf/types.h>
56 #include <stdint.h>
57 
58 #include <map>
59 #ifdef __FreeBSD__
60 # include <tr1/unordered_map>
61 #else
62 # include <unordered_map>
63 #endif
64 #include <vector>
65 #include <string>
66 
67 namespace fawkes {
68 
69  class Mutex;
70 
71  namespace tf {
72 #if 0 /* just to make Emacs auto-indent happy */
73  }
74 }
75 #endif
76 
77 class TimeCache;
78 
80 {
81  public:
82  static const unsigned int MAX_GRAPH_DEPTH = 100UL;
84 
85  Transformer(float cache_time_sec = 10.0);
86  virtual ~Transformer(void);
87 
88  void clear();
89 
90  bool set_transform(const StampedTransform &transform,
91  const std::string &authority = "default_authority");
92 
93  bool frame_exists(const std::string& frame_id_str) const;
94 
95 
96  void lookup_transform(const std::string& target_frame,
97  const std::string& source_frame,
98  const fawkes::Time& time,
99  StampedTransform& transform) const;
100 
101  void lookup_transform(const std::string& target_frame,
102  const std::string& source_frame,
103  StampedTransform& transform) const;
104 
105  void lookup_transform(const std::string& target_frame,
106  const fawkes::Time& target_time,
107  const std::string& source_frame,
108  const fawkes::Time& source_time,
109  const std::string& fixed_frame,
110  StampedTransform& transform) const;
111 
112  bool can_transform(const std::string& target_frame,
113  const std::string& source_frame,
114  const fawkes::Time& time) const;
115 
116  bool can_transform(const std::string& target_frame,
117  const fawkes::Time& target_time,
118  const std::string& source_frame,
119  const fawkes::Time& source_time,
120  const std::string& fixed_frame) const;
121 
122  const TimeCache * get_frame_cache(const std::string &frame_id) const;
123 
124  void set_enabled(bool enabled);
125  bool is_enabled() const { return enabled_; };
126 
127  int get_latest_common_time(const std::string &source_frame, const std::string &target_frame,
128  fawkes::Time& time, std::string* error_string = 0) const;
129 
130  void transform_quaternion(const std::string& target_frame,
131  const Stamped<Quaternion>& stamped_in,
132  Stamped<Quaternion>& stamped_out) const;
133  void transform_vector(const std::string& target_frame,
134  const Stamped<Vector3>& stamped_in,
135  Stamped<Vector3>& stamped_out) const;
136  void transform_point(const std::string& target_frame,
137  const Stamped<Point>& stamped_in, Stamped<Point>& stamped_out) const;
138  void transform_pose(const std::string& target_frame,
139  const Stamped<Pose>& stamped_in, Stamped<Pose>& stamped_out) const;
140 
141  void transform_quaternion(const std::string& target_frame, const fawkes::Time& target_time,
142  const Stamped<Quaternion>& stamped_in,
143  const std::string& fixed_frame,
144  Stamped<Quaternion>& stamped_out) const;
145  void transform_vector(const std::string& target_frame, const fawkes::Time& target_time,
146  const Stamped<Vector3>& stamped_in,
147  const std::string& fixed_frame,
148  Stamped<Vector3>& stamped_out) const;
149  void transform_point(const std::string& target_frame, const fawkes::Time& target_time,
150  const Stamped<Point>& stamped_in,
151  const std::string& fixed_frame,
152  Stamped<Point>& stamped_out) const;
153  void transform_pose(const std::string& target_frame, const fawkes::Time& target_time,
154  const Stamped<Pose>& stamped_in,
155  const std::string& fixed_frame,
156  Stamped<Pose>& stamped_out) const;
157 
158  protected: /* methods */
159  TimeCache * get_frame(unsigned int frame_number) const;
160 
161  CompactFrameID lookup_frame_number(const std::string &frameid_str) const;
162  CompactFrameID lookup_or_insert_frame_number(const std::string &frameid_str);
163  std::string lookup_frame_string(unsigned int frame_id_num) const;
164 
165  protected:
166  /// Flag to mark the transformer as disabled
167  bool enabled_;
168  /// Map from string frame ids to CompactFrameID.
169 #ifdef __FreeBSD__
170  typedef std::tr1::unordered_map<std::string, CompactFrameID> M_StringToCompactFrameID;
171 #else
172  typedef std::unordered_map<std::string, CompactFrameID> M_StringToCompactFrameID;
173 #endif
174  /// Map from frame IDs to frame numbers
175  M_StringToCompactFrameID frameIDs_;
176  /// Map from CompactFrameID frame_id_numbers to string for debugging and output.
177  std::vector<std::string> frameIDs_reverse;
178  /// Map to lookup the most recent authority for a given frame.
179  std::map<CompactFrameID, std::string> frame_authority_;
180 
181  /** \brief The pointers to potential frames that the tree can be made of.
182  * The frames will be dynamically allocated at run time when set the first time. */
183  std::vector<TimeCache*> frames_;
184 
185  /** \brief A mutex to protect testing and allocating new frames on the above vector. */
186  mutable Mutex *frame_mutex_;
187 
188  /// How long to cache transform history
189  float cache_time_;
190 
191  /// whether or not to allow extrapolation
193 
194  /// transform prefix to apply as necessary
195  std::string tf_prefix_;
196 
197  /// Set to true to allow falling back to wall time
199 
200  private:
201  /**Return the latest time which is common across the spanning set.
202  * @return zero if fails to cross */
203  int get_latest_common_time(CompactFrameID target_frame, CompactFrameID source_frame,
204  fawkes::Time& time, std::string* error_string) const;
205 
206  bool can_transform_no_lock(CompactFrameID target_id, CompactFrameID source_id,
207  const fawkes::Time& time) const;
208  void create_connectivity_error_string(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const;
209 
210  template<typename F>
211  int walk_to_top_parent(F& f, fawkes::Time time,
212  CompactFrameID target_id, CompactFrameID source_id,
213  std::string* error_string) const;
214 
215 };
216 
217 
218 } // end namespace tf
219 } // end namespace fawkes
220 
221 #endif
std::map< CompactFrameID, std::string > frame_authority_
Map to lookup the most recent authority for a given frame.
Definition: transformer.h:179
bool is_enabled() const
Check if transformer is enabled.
Definition: transformer.h:125
std::unordered_map< std::string, CompactFrameID > M_StringToCompactFrameID
Map from string frame ids to CompactFrameID.
Definition: transformer.h:172
Fawkes library namespace.
std::vector< TimeCache * > frames_
The pointers to potential frames that the tree can be made of.
Definition: transformer.h:183
float max_extrapolation_distance_
whether or not to allow extrapolation
Definition: transformer.h:192
A class for handling time.
Definition: time.h:91
Mutex * frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
Definition: transformer.h:186
std::string tf_prefix_
transform prefix to apply as necessary
Definition: transformer.h:195
bool fall_back_to_wall_time_
Set to true to allow falling back to wall time.
Definition: transformer.h:198
Time based transform cache.
Definition: time_cache.h:104
bool enabled_
Flag to mark the transformer as disabled.
Definition: transformer.h:167
std::vector< std::string > frameIDs_reverse
Map from CompactFrameID frame_id_numbers to string for debugging and output.
Definition: transformer.h:177
static const float DEFAULT_MAX_EXTRAPOLATION_DISTANCE
The default amount of time to extrapolate.
Definition: transformer.h:83
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:129
M_StringToCompactFrameID frameIDs_
Map from frame IDs to frame numbers.
Definition: transformer.h:175
Mutex mutual exclusion lock.
Definition: mutex.h:32
Coordinate transforms between any two frames in a system.
Definition: transformer.h:79
float cache_time_
How long to cache transform history.
Definition: transformer.h:189