buffer_core.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
32 #ifndef TF2_BUFFER_CORE_H
33 #define TF2_BUFFER_CORE_H
34 
35 #include "transform_storage.h"
36 
37 #include <boost/signals2.hpp>
38 
39 #include <string>
40 
41 #include "ros/duration.h"
42 #include "ros/time.h"
43 //#include "geometry_msgs/TwistStamped.h"
44 #include "geometry_msgs/TransformStamped.h"
45 
47 //#include "tf/tf.h"
48 
49 #include <boost/unordered_map.hpp>
50 #include <boost/thread/mutex.hpp>
51 #include <boost/function.hpp>
52 #include <boost/shared_ptr.hpp>
53 
54 namespace tf2
55 {
56 
57 typedef std::pair<ros::Time, CompactFrameID> P_TimeAndFrameID;
58 typedef uint32_t TransformableCallbackHandle;
59 typedef uint64_t TransformableRequestHandle;
60 
63 
65 {
68 };
69 
89 {
90 public:
91  /************* Constants ***********************/
92  static const int DEFAULT_CACHE_TIME = 10;
93  static const uint32_t MAX_GRAPH_DEPTH = 1000UL;
94 
100  BufferCore(ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME));
101  virtual ~BufferCore(void);
102 
104  void clear();
105 
112  bool setTransform(const geometry_msgs::TransformStamped& transform, const std::string & authority, bool is_static = false);
113 
114  /*********** Accessors *************/
115 
125  geometry_msgs::TransformStamped
126  lookupTransform(const std::string& target_frame, const std::string& source_frame,
127  const ros::Time& time) const;
128 
141  geometry_msgs::TransformStamped
142  lookupTransform(const std::string& target_frame, const ros::Time& target_time,
143  const std::string& source_frame, const ros::Time& source_time,
144  const std::string& fixed_frame) const;
145 
146  /* \brief Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame using the reference point
147  * \param tracking_frame The frame to track
148  * \param observation_frame The frame from which to measure the twist
149  * \param reference_frame The reference frame in which to express the twist
150  * \param reference_point The reference point with which to express the twist
151  * \param reference_point_frame The frame_id in which the reference point is expressed
152  * \param time The time at which to get the velocity
153  * \param duration The period over which to average
154  * \return twist The twist output
155  *
156  * This will compute the average velocity on the interval
157  * (time - duration/2, time+duration/2). If that is too close to the most
158  * recent reading, in which case it will shift the interval up to
159  * duration/2 to prevent extrapolation.
160  *
161  * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
162  * tf2::ExtrapolationException, tf2::InvalidArgumentException
163  *
164  * New in geometry 1.1
165  */
166  /*
167  geometry_msgs::Twist
168  lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
169  const tf::Point & reference_point, const std::string& reference_point_frame,
170  const ros::Time& time, const ros::Duration& averaging_interval) const;
171  */
172  /* \brief lookup the twist of the tracking frame with respect to the observational frame
173  *
174  * This is a simplified version of
175  * lookupTwist with it assumed that the reference point is the
176  * origin of the tracking frame, and the reference frame is the
177  * observation frame.
178  *
179  * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
180  * tf2::ExtrapolationException, tf2::InvalidArgumentException
181  *
182  * New in geometry 1.1
183  */
184  /*
185  geometry_msgs::Twist
186  lookupTwist(const std::string& tracking_frame, const std::string& observation_frame,
187  const ros::Time& time, const ros::Duration& averaging_interval) const;
188  */
196  bool canTransform(const std::string& target_frame, const std::string& source_frame,
197  const ros::Time& time, std::string* error_msg = NULL) const;
198 
208  bool canTransform(const std::string& target_frame, const ros::Time& target_time,
209  const std::string& source_frame, const ros::Time& source_time,
210  const std::string& fixed_frame, std::string* error_msg = NULL) const;
211 
215  std::string allFramesAsYAML(double current_time) const;
216 
219  std::string allFramesAsYAML() const;
220 
224  std::string allFramesAsString() const;
225 
226  typedef boost::function<void(TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame,
228 
230  TransformableCallbackHandle addTransformableCallback(const TransformableCallback& cb);
232  void removeTransformableCallback(TransformableCallbackHandle handle);
234  TransformableRequestHandle addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, ros::Time time);
236  void cancelTransformableRequest(TransformableRequestHandle handle);
237 
238 
239 
240 
241  // Tell the buffer that there are multiple threads serviciing it.
242  // This is useful for derived classes to know if they can block or not.
243  void setUsingDedicatedThread(bool value) { using_dedicated_thread_ = value;};
244  // Get the state of using_dedicated_thread_
246 
247 
248 
249 
250  /* Backwards compatability section for tf::Transformer you should not use these
251  */
252 
260  boost::signals2::connection _addTransformsChangedListener(boost::function<void(void)> callback);
261  void _removeTransformsChangedListener(boost::signals2::connection c);
262 
263 
266  bool _frameExists(const std::string& frame_id_str) const;
267 
272  bool _getParent(const std::string& frame_id, ros::Time time, std::string& parent) const;
273 
275  void _getFrameStrings(std::vector<std::string>& ids) const;
276 
277 
278  CompactFrameID _lookupFrameNumber(const std::string& frameid_str) const {
279  return lookupFrameNumber(frameid_str);
280  }
281  CompactFrameID _lookupOrInsertFrameNumber(const std::string& frameid_str) {
282  return lookupOrInsertFrameNumber(frameid_str);
283  }
284 
285  int _getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time& time, std::string* error_string) const {
286  boost::mutex::scoped_lock lock(frame_mutex_);
287  return getLatestCommonTime(target_frame, source_frame, time, error_string);
288  }
289 
290  CompactFrameID _validateFrameId(const char* function_name_arg, const std::string& frame_id) const {
291  return validateFrameId(function_name_arg, frame_id);
292  }
293 
296 
300  std::string _allFramesAsDot(double current_time) const;
301  std::string _allFramesAsDot() const;
302 
306  void _chainAsVector(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string & fixed_frame, std::vector<std::string>& output) const;
307 
308 private:
309 
313  std::string allFramesAsStringNoLock() const;
314 
315 
316  /******************** Internal Storage ****************/
317 
320  typedef std::vector<TimeCacheInterfacePtr> V_TimeCacheInterface;
321  V_TimeCacheInterface frames_;
322 
324  mutable boost::mutex frame_mutex_;
325 
327  typedef boost::unordered_map<std::string, CompactFrameID> M_StringToCompactFrameID;
328  M_StringToCompactFrameID frameIDs_;
330  std::vector<std::string> frameIDs_reverse;
332  std::map<CompactFrameID, std::string> frame_authority_;
333 
334 
337 
338  typedef boost::unordered_map<TransformableCallbackHandle, TransformableCallback> M_TransformableCallback;
339  M_TransformableCallback transformable_callbacks_;
342 
344  {
346  TransformableRequestHandle request_handle;
347  TransformableCallbackHandle cb_handle;
350  std::string target_string;
351  std::string source_string;
352  };
353  typedef std::vector<TransformableRequest> V_TransformableRequest;
354  V_TransformableRequest transformable_requests_;
357 
360 
361  // Backwards compatability for tf message_filter
362  typedef boost::signals2::signal<void(void)> TransformsChangedSignal;
364  TransformsChangedSignal _transforms_changed_;
365 
366 
367  /************************* Internal Functions ****************************/
368 
375  TimeCacheInterfacePtr getFrame(CompactFrameID c_frame_id) const;
376 
377  TimeCacheInterfacePtr allocateFrame(CompactFrameID cfid, bool is_static);
378 
379 
380  bool warnFrameId(const char* function_name_arg, const std::string& frame_id) const;
381  CompactFrameID validateFrameId(const char* function_name_arg, const std::string& frame_id) const;
382 
384  CompactFrameID lookupFrameNumber(const std::string& frameid_str) const;
385 
387  CompactFrameID lookupOrInsertFrameNumber(const std::string& frameid_str);
388 
390  const std::string& lookupFrameString(CompactFrameID frame_id_num) const;
391 
392  void createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const;
393 
396  int getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time& time, std::string* error_string) const;
397 
398  template<typename F>
399  int walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const;
400 
403  template<typename F>
404  int walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *frame_chain) const;
405 
408  const ros::Time& time, std::string* error_msg) const;
410  const ros::Time& time, std::string* error_msg) const;
411 
412 
413  //Whether it is safe to use canTransform with a timeout. (If another thread is not provided it will always timeout.)
415 
416 public:
417  friend class TestBufferCore; // For unit testing
418 
419 };
420 
423 {
424 public:
425  int _walkToTopParent(BufferCore& buffer, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *frame_chain) const;
426  const std::string& _lookupFrameString(BufferCore& buffer, CompactFrameID frame_id_num) const
427  {
428  return buffer.lookupFrameString(frame_id_num);
429  }
430 };
431 };
432 
433 #endif //TF2_CORE_H
bool _getParent(const std::string &frame_id, ros::Time time, std::string &parent) const
Fill the parent of a frame.
bool using_dedicated_thread_
Definition: buffer_core.h:414
bool isUsingDedicatedThread() const
Definition: buffer_core.h:245
A Class which provides coordinate transforms between any two frames in a system.
Definition: buffer_core.h:88
uint32_t CompactFrameID
TransformsChangedSignal _transforms_changed_
Signal which is fired whenever new transform data has arrived, from the thread the data arrived in...
Definition: buffer_core.h:364
static const uint32_t MAX_GRAPH_DEPTH
Maximum graph search depth (deeper graphs will be assumed to have loops)
Definition: buffer_core.h:93
uint64_t transformable_requests_counter_
Definition: buffer_core.h:356
std::string allFramesAsString() const
A way to see what frames have been cached Useful for debugging.
TransformableCallbackHandle addTransformableCallback(const TransformableCallback &cb)
Internal use only.
int walkToTopParent(F &f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string *error_string) const
void setUsingDedicatedThread(bool value)
Definition: buffer_core.h:243
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
boost::signals2::connection _addTransformsChangedListener(boost::function< void(void)> callback)
Add a callback that happens when a new transform has arrived.
const std::string & _lookupFrameString(BufferCore &buffer, CompactFrameID frame_id_num) const
Definition: buffer_core.h:426
boost::mutex transformable_requests_mutex_
Definition: buffer_core.h:355
uint32_t TransformableCallbackHandle
Definition: buffer_core.h:58
const std::string & lookupFrameString(CompactFrameID frame_id_num) const
Number to string frame lookup may throw LookupException if number invalid.
bool canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id, const ros::Time &time, std::string *error_msg) const
M_TransformableCallback transformable_callbacks_
Definition: buffer_core.h:339
uint32_t transformable_callbacks_counter_
Definition: buffer_core.h:340
TimeCacheInterfacePtr allocateFrame(CompactFrameID cfid, bool is_static)
std::vector< TimeCacheInterfacePtr > V_TimeCacheInterface
The pointers to potential frames that the tree can be made of. The frames will be dynamically allocat...
Definition: buffer_core.h:320
std::pair< ros::Time, CompactFrameID > P_TimeAndFrameID
Definition: buffer_core.h:57
TransformableCallbackHandle cb_handle
Definition: buffer_core.h:347
virtual ~BufferCore(void)
std::string allFramesAsYAML() const
std::vector< TransformableRequest > V_TransformableRequest
Definition: buffer_core.h:353
TransformableResult
Definition: buffer_core.h:64
TransformableRequestHandle request_handle
Definition: buffer_core.h:346
boost::mutex frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
Definition: buffer_core.h:324
std::vector< std::string > frameIDs_reverse
A map from CompactFrameID frame_id_numbers to string for debugging and output.
Definition: buffer_core.h:330
boost::unordered_map< TransformableCallbackHandle, TransformableCallback > M_TransformableCallback
Definition: buffer_core.h:338
void _removeTransformsChangedListener(boost::signals2::connection c)
bool _frameExists(const std::string &frame_id_str) const
Check if a frame exists in the tree.
void clear()
Clear all data.
bool warnFrameId(const char *function_name_arg, const std::string &frame_id) const
ros::Duration getCacheLength()
Get the duration over which this transformer will cache.
Definition: buffer_core.h:295
void _chainAsVector(const std::string &target_frame, ros::Time target_time, const std::string &source_frame, ros::Time source_time, const std::string &fixed_frame, std::vector< std::string > &output) const
Backwards compatabilityA way to see what frames are in a chain Useful for debugging.
CompactFrameID validateFrameId(const char *function_name_arg, const std::string &frame_id) const
void cancelTransformableRequest(TransformableRequestHandle handle)
Internal use only.
boost::signals2::signal< void(void)> TransformsChangedSignal
Definition: buffer_core.h:359
ros::Duration cache_time_
How long to cache transform history.
Definition: buffer_core.h:336
bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) 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:92
M_StringToCompactFrameID frameIDs_
Definition: buffer_core.h:328
TransformableRequestHandle addTransformableRequest(TransformableCallbackHandle handle, const std::string &target_frame, const std::string &source_frame, ros::Time time)
Internal use only.
CompactFrameID _lookupFrameNumber(const std::string &frameid_str) const
Definition: buffer_core.h:278
void createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string *out) const
int getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time &time, std::string *error_string) const
Return the latest rostime which is common across the spanning set zero if fails to cross...
V_TransformableRequest transformable_requests_
Definition: buffer_core.h:354
std::map< CompactFrameID, std::string > frame_authority_
A map to lookup the most recent authority for a given frame.
Definition: buffer_core.h:332
TimeCacheInterfacePtr getFrame(CompactFrameID c_frame_id) const
An accessor to get a frame, which will throw an exception if the frame is no there.
boost::shared_ptr< TimeCacheInterface > TimeCacheInterfacePtr
Definition: buffer_core.h:61
bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id, const ros::Time &time, std::string *error_msg) const
boost::unordered_map< std::string, CompactFrameID > M_StringToCompactFrameID
A map from string frame ids to CompactFrameID.
Definition: buffer_core.h:327
int _getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, ros::Time &time, std::string *error_string) const
Definition: buffer_core.h:285
void removeTransformableCallback(TransformableCallbackHandle handle)
Internal use only.
std::string _allFramesAsDot() const
boost::mutex transformable_callbacks_mutex_
Definition: buffer_core.h:341
void _getFrameStrings(std::vector< std::string > &ids) const
A way to get a std::vector of available frame ids.
std::string allFramesAsStringNoLock() const
A way to see what frames have been cached Useful for debugging. Use this call internally.
V_TimeCacheInterface frames_
Definition: buffer_core.h:321
CompactFrameID _lookupOrInsertFrameNumber(const std::string &frameid_str)
Definition: buffer_core.h:281
CompactFrameID lookupFrameNumber(const std::string &frameid_str) const
String to number for frame lookup with dynamic allocation of new frames.
CompactFrameID lookupOrInsertFrameNumber(const std::string &frameid_str)
String to number for frame lookup with dynamic allocation of new frames.
BufferCore(ros::Duration cache_time_=ros::Duration(DEFAULT_CACHE_TIME))
geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time) const
Get the transform between two frames by frame ID.
void testTransformableRequests()
boost::function< void(TransformableRequestHandle request_handle, const std::string &target_frame, const std::string &source_frame, ros::Time time, TransformableResult result)> TransformableCallback
Definition: buffer_core.h:227
uint64_t TransformableRequestHandle
Definition: buffer_core.h:59
CompactFrameID _validateFrameId(const char *function_name_arg, const std::string &frame_id) const
Definition: buffer_core.h:290


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Oct 16 2020 19:08:50