tf2  master
tf2 maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.
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 "LinearMath/Transform.h"
36 #include "transform_storage.h"
37 
38 #include <algorithm>
39 #include <cmath>
40 #include <string>
41 
42 //#include "geometry_msgs/TwistStamped.h"
43 #include "geometry_msgs/msg/transform_stamped.hpp"
44 
45 #include <map>
46 #include <unordered_map>
47 #include <mutex>
48 #include <functional>
49 #include <memory>
50 
51 #include <tf2/exceptions.h>
53 #include <tf2/visibility_control.h>
54 
55 namespace tf2
56 {
57 
59 typedef uint32_t TransformableCallbackHandle;
60 typedef uint64_t TransformableRequestHandle;
61 
62 class TimeCacheInterface;
64 
66 {
69 };
70 
71 
72 static constexpr Duration BUFFER_CORE_DEFAULT_CACHE_TIME = std::chrono::seconds(10);
73 
93 {
94 public:
95  /************* Constants ***********************/
97  static const uint32_t MAX_GRAPH_DEPTH = 1000UL;
98 
104  TF2_PUBLIC
105  BufferCore(tf2::Duration cache_time_ = BUFFER_CORE_DEFAULT_CACHE_TIME);
106 
107  TF2_PUBLIC
108  virtual ~BufferCore(void);
109 
111  TF2_PUBLIC
112  void clear() override;
113 
120  TF2_PUBLIC
121  bool setTransform(const geometry_msgs::msg::TransformStamped& transform, const std::string & authority, bool is_static = false);
122 
123  /*********** Accessors *************/
124 
134  TF2_PUBLIC
135  geometry_msgs::msg::TransformStamped
136  lookupTransform(const std::string& target_frame, const std::string& source_frame,
137  const TimePoint& time) const override;
138 
151  TF2_PUBLIC
152  geometry_msgs::msg::TransformStamped
153  lookupTransform(const std::string& target_frame, const TimePoint& target_time,
154  const std::string& source_frame, const TimePoint& source_time,
155  const std::string& fixed_frame) const override;
156 
177  /*
178  geometry_msgs::Twist
179  lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
180  const tf::Point & reference_point, const std::string& reference_point_frame,
181  const tf2::TimePoint& time, const tf2::Duration& averaging_interval) const;
182  */
195  /*
196  geometry_msgs::Twist
197  lookupTwist(const std::string& tracking_frame, const std::string& observation_frame,
198  const tf2::TimePoint& time, const tf2::Duration& averaging_interval) const;
199  */
207  TF2_PUBLIC
208  bool canTransform(const std::string& target_frame, const std::string& source_frame,
209  const TimePoint& time, std::string* error_msg = NULL) const override;
210 
220  TF2_PUBLIC
221  bool canTransform(const std::string& target_frame, const TimePoint& target_time,
222  const std::string& source_frame, const TimePoint& source_time,
223  const std::string& fixed_frame, std::string* error_msg = NULL) const override;
224 
227  TF2_PUBLIC
229 
233  TF2_PUBLIC
234  std::string allFramesAsYAML(TimePoint current_time) const;
235 
238  TF2_PUBLIC
240 
244  TF2_PUBLIC
246 
247  using TransformableCallback = std::function<void(TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame,
249 
251  TF2_PUBLIC
254  TF2_PUBLIC
257  TF2_PUBLIC
260  TF2_PUBLIC
262 
263 
264 
265 
266  // Tell the buffer that there are multiple threads serviciing it.
267  // This is useful for derived classes to know if they can block or not.
268  TF2_PUBLIC
269  void setUsingDedicatedThread(bool value) { using_dedicated_thread_ = value;};
270  // Get the state of using_dedicated_thread_
271  TF2_PUBLIC
272  bool isUsingDedicatedThread() const { return using_dedicated_thread_;};
273 
274 
275 
276 
277  /* Backwards compatability section for tf::Transformer you should not use these
278  */
279 
282  TF2_PUBLIC
283  bool _frameExists(const std::string& frame_id_str) const;
284 
289  TF2_PUBLIC
290  bool _getParent(const std::string& frame_id, TimePoint time, std::string& parent) const;
291 
293  TF2_PUBLIC
295 
296 
297  TF2_PUBLIC
298  CompactFrameID _lookupFrameNumber(const std::string& frameid_str) const {
299  return lookupFrameNumber(frameid_str);
300  }
301  TF2_PUBLIC
303  return lookupOrInsertFrameNumber(frameid_str);
304  }
305 
306  TF2_PUBLIC
307  tf2::TF2Error _getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, TimePoint& time, std::string* error_string) const {
308  std::unique_lock<std::mutex> lock(frame_mutex_);
309  return getLatestCommonTime(target_frame, source_frame, time, error_string);
310  }
311 
312  TF2_PUBLIC
313  CompactFrameID _validateFrameId(const char* function_name_arg, const std::string& frame_id) const {
314  return validateFrameId(function_name_arg, frame_id);
315  }
316 
318  TF2_PUBLIC
319  tf2::Duration getCacheLength() { return cache_time_;}
320 
324  TF2_PUBLIC
325  std::string _allFramesAsDot(TimePoint current_time) const;
326  TF2_PUBLIC
328 
332  TF2_PUBLIC
333  void _chainAsVector(const std::string & target_frame, TimePoint target_time, const std::string & source_frame, TimePoint source_time, const std::string & fixed_frame, std::vector<std::string>& output) const;
334 
335 private:
336 
340  std::string allFramesAsStringNoLock() const;
341 
342 
343  /******************** Internal Storage ****************/
344 
347  typedef std::vector<TimeCacheInterfacePtr> V_TimeCacheInterface;
348  V_TimeCacheInterface frames_;
349 
351  mutable std::mutex frame_mutex_;
352 
354  typedef std::unordered_map<std::string, CompactFrameID> M_StringToCompactFrameID;
355  M_StringToCompactFrameID frameIDs_;
357  std::vector<std::string> frameIDs_reverse;
359  std::map<CompactFrameID, std::string> frame_authority_;
360 
361 
363  tf2::Duration cache_time_;
364 
366  M_TransformableCallback transformable_callbacks_;
367  uint32_t transformable_callbacks_counter_;
368  std::mutex transformable_callbacks_mutex_;
369 
370  struct TransformableRequest
371  {
372  TimePoint time;
373  TransformableRequestHandle request_handle;
374  TransformableCallbackHandle cb_handle;
375  CompactFrameID target_id;
376  CompactFrameID source_id;
377  std::string target_string;
378  std::string source_string;
379  };
380  typedef std::vector<TransformableRequest> V_TransformableRequest;
381  V_TransformableRequest transformable_requests_;
382  std::mutex transformable_requests_mutex_;
383  uint64_t transformable_requests_counter_;
384 
385  struct RemoveRequestByCallback;
386  struct RemoveRequestByID;
387 
388 
389  /************************* Internal Functions ****************************/
390 
391  bool setTransformImpl(const tf2::Transform& transform_in, const std::string frame_id,
392  const std::string child_frame_id, const TimePoint stamp,
393  const std::string& authority, bool is_static);
394  void lookupTransformImpl(const std::string& target_frame, const std::string& source_frame,
395  const TimePoint& time_in, tf2::Transform& transform, TimePoint& time_out) const;
396 
397  void lookupTransformImpl(const std::string& target_frame, const TimePoint& target_time,
398  const std::string& source_frame, const TimePoint& source_time,
399  const std::string& fixed_frame, tf2::Transform& transform, TimePoint& time_out) const;
400 
407  TimeCacheInterfacePtr getFrame(CompactFrameID c_frame_id) const;
408 
409  TimeCacheInterfacePtr allocateFrame(CompactFrameID cfid, bool is_static);
410 
420  CompactFrameID validateFrameId(
421  const char* function_name_arg,
422  const std::string& frame_id,
423  std::string* error_msg) const;
424 
434  CompactFrameID validateFrameId(const char* function_name_arg, const std::string& frame_id) const;
435 
437  CompactFrameID lookupFrameNumber(const std::string& frameid_str) const;
438 
440  CompactFrameID lookupOrInsertFrameNumber(const std::string& frameid_str);
441 
443  const std::string& lookupFrameString(CompactFrameID frame_id_num) const;
444 
445  void createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const;
446 
449  tf2::TF2Error getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, TimePoint& time, std::string* error_string) const;
450 
451  template<typename F>
452  tf2::TF2Error walkToTopParent(F& f, TimePoint time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const;
453 
456  template<typename F>
457  tf2::TF2Error walkToTopParent(F& f, TimePoint time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *frame_chain) const;
458 
459  void testTransformableRequests();
460  // Thread safe transform check, acquire lock and call canTransformNoLock.
461  bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id,
462  const TimePoint& time, std::string* error_msg) const;
463  // Actual implementation to walk the transform tree and find out if a transform exists.
464  bool canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id,
465  const TimePoint& time, std::string* error_msg) const;
466 
467 
468  //Whether it is safe to use canTransform with a timeout. (If another thread is not provided it will always timeout.)
469  bool using_dedicated_thread_;
470 
471 };
472 
473 
474 
475 
476 }
477 
478 #endif //TF2_CORE_H
tf2::BufferCore::getCacheLength
tf2::Duration getCacheLength()
Get the duration over which this transformer will cache.
Definition: buffer_core.h:319
exceptions.h
std::string
std::shared_ptr
tf2::BufferCore::setUsingDedicatedThread
void setUsingDedicatedThread(bool value)
Definition: buffer_core.h:269
buffer_core_interface.h
tf2::TimeCacheInterfacePtr
std::shared_ptr< TimeCacheInterface > TimeCacheInterfacePtr
Definition: buffer_core.h:63
std::pair
tf2::BufferCore::_chainAsVector
void _chainAsVector(const std::string &target_frame, TimePoint target_time, const std::string &source_frame, TimePoint 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.
tf2::BufferCore::setTransform
bool setTransform(const geometry_msgs::msg::TransformStamped &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
tf2::Duration
std::chrono::nanoseconds Duration
Definition: time.h:44
std::vector< std::string >
tf2::BufferCore::_validateFrameId
CompactFrameID _validateFrameId(const char *function_name_arg, const std::string &frame_id) const
Definition: buffer_core.h:313
tf2::BufferCore::addTransformableCallback
TransformableCallbackHandle addTransformableCallback(const TransformableCallback &cb)
Internal use only.
std::chrono::seconds
tf2::BufferCoreInterface
Interface for providing coordinate transforms between any two frames in a system.
Definition: buffer_core_interface.h:50
tf2::BufferCore::MAX_GRAPH_DEPTH
static const uint32_t MAX_GRAPH_DEPTH
Maximum graph search depth (deeper graphs will be assumed to have loops)
Definition: buffer_core.h:97
Transform.h
tf2::BufferCore::allFramesAsYAML
std::string allFramesAsYAML() const
std::function
tf2::BufferCore::canTransform
bool canTransform(const std::string &target_frame, const std::string &source_frame, const TimePoint &time, std::string *error_msg=NULL) const override
Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame u...
tf2::BufferCore::getAllFrameNames
std::vector< std::string > getAllFrameNames() const override
Get all frames that exist in the system.
tf2::BufferCore::BufferCore
BufferCore(tf2::Duration cache_time_=BUFFER_CORE_DEFAULT_CACHE_TIME)
tf2::BufferCore::_lookupFrameNumber
CompactFrameID _lookupFrameNumber(const std::string &frameid_str) const
Definition: buffer_core.h:298
tf2::CompactFrameID
uint32_t CompactFrameID
Definition: transform_storage.h:46
tf2::TransformFailure
@ TransformFailure
Definition: buffer_core.h:68
tf2::BufferCore::allFramesAsString
std::string allFramesAsString() const
A way to see what frames have been cached Useful for debugging.
tf2::TimePoint
std::chrono::time_point< std::chrono::system_clock, Duration > TimePoint
Definition: time.h:45
tf2::BufferCore::_getFrameStrings
void _getFrameStrings(std::vector< std::string > &ids) const
A way to get a std::vector of available frame ids.
tf2::P_TimeAndFrameID
std::pair< TimePoint, CompactFrameID > P_TimeAndFrameID
Definition: buffer_core.h:58
tf2::TransformableRequestHandle
uint64_t TransformableRequestHandle
Definition: buffer_core.h:60
std::unique_lock
tf2::BufferCore::isUsingDedicatedThread
bool isUsingDedicatedThread() const
Definition: buffer_core.h:272
tf2::Transform
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:33
std::chrono::time_point< std::chrono::system_clock, Duration >
transform_storage.h
tf2::BufferCore::addTransformableRequest
TransformableRequestHandle addTransformableRequest(TransformableCallbackHandle handle, const std::string &target_frame, const std::string &source_frame, TimePoint time)
Internal use only.
tf2::BufferCore::_allFramesAsDot
std::string _allFramesAsDot() const
tf2::TimeCacheInterface
Definition: time_cache.h:48
tf2::BufferCore::_frameExists
bool _frameExists(const std::string &frame_id_str) const
Check if a frame exists in the tree.
std::map< CompactFrameID, std::string >
tf2::BufferCore::cancelTransformableRequest
void cancelTransformableRequest(TransformableRequestHandle handle)
Internal use only.
tf2::BufferCore::lookupTransform
geometry_msgs::msg::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const TimePoint &time) const override
Get the transform between two frames by frame ID.
tf2::BufferCore::_getParent
bool _getParent(const std::string &frame_id, TimePoint time, std::string &parent) const
Fill the parent of a frame.
tf2::BufferCore::_lookupOrInsertFrameNumber
CompactFrameID _lookupOrInsertFrameNumber(const std::string &frameid_str)
Definition: buffer_core.h:302
visibility_control.h
tf2::TF2Error
TF2Error
Definition: exceptions.h:42
tf2::BufferCore
A Class which provides coordinate transforms between any two frames in a system.
Definition: buffer_core.h:92
tf2::BufferCore::clear
void clear() override
Clear all data.
tf2
Definition: buffer_core.h:55
TF2_PUBLIC
#define TF2_PUBLIC
Definition: visibility_control.h:58
std::mutex
tf2::BufferCore::~BufferCore
virtual ~BufferCore(void)
tf2::BufferCore::removeTransformableCallback
void removeTransformableCallback(TransformableCallbackHandle handle)
Internal use only.
tf2::BufferCore::_getLatestCommonTime
tf2::TF2Error _getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, TimePoint &time, std::string *error_string) const
Definition: buffer_core.h:307
std::unordered_map< std::string, CompactFrameID >
tf2::TransformableResult
TransformableResult
Definition: buffer_core.h:65
tf2::TransformableCallbackHandle
uint32_t TransformableCallbackHandle
Definition: buffer_core.h:59
tf2::TransformAvailable
@ TransformAvailable
Definition: buffer_core.h:67