tf2_ros  master
This package contains the ROS bindings for the tf2 library, for both Python and C++.
buffer_interface.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_ROS_BUFFER_INTERFACE_H
33 #define TF2_ROS_BUFFER_INTERFACE_H
34 
35 #include <rclcpp/rclcpp.hpp>
37 #include <tf2/transform_datatypes.h>
38 #include <tf2/exceptions.h>
39 #include <geometry_msgs/msg/transform_stamped.hpp>
40 #include <sstream>
41 #include <tf2/convert.h>
42 
43 namespace tf2_ros
44 {
47 
48  inline builtin_interfaces::msg::Time toMsg(const tf2::TimePoint & t)
49  {
51  std::chrono::duration_cast<std::chrono::nanoseconds>(t.time_since_epoch());
53  std::chrono::duration_cast<std::chrono::seconds>(t.time_since_epoch());
54  builtin_interfaces::msg::Time time_msg;
55  time_msg.sec = (int32_t)s.count();
56  time_msg.nanosec = (uint32_t)(ns.count() % 1000000000ull);
57  return time_msg;
58  }
59 
60  inline tf2::TimePoint fromMsg(const builtin_interfaces::msg::Time & time_msg)
61  {
62  int64_t d = time_msg.sec * 1000000000ull + time_msg.nanosec;
64  return tf2::TimePoint(std::chrono::duration_cast<tf2::Duration>(ns));
65  }
66 
67  inline builtin_interfaces::msg::Duration toMsg(const tf2::Duration & t)
68  {
70  std::chrono::duration_cast<std::chrono::nanoseconds>(t);
72  std::chrono::duration_cast<std::chrono::seconds>(t);
73  builtin_interfaces::msg::Duration duration_msg;
74  duration_msg.sec = (int32_t)s.count();
75  duration_msg.nanosec = (uint32_t)(ns.count() % 1000000000ull);
76  return duration_msg;
77  }
78 
79  inline tf2::Duration fromMsg(const builtin_interfaces::msg::Duration & duration_msg)
80  {
81  int64_t d = duration_msg.sec * 1000000000ull + duration_msg.nanosec;
83  return tf2::Duration(std::chrono::duration_cast<tf2::Duration>(ns));
84  }
85 
86  inline double timeToSec(const builtin_interfaces::msg::Time & time_msg)
87  {
88  auto ns = std::chrono::duration<double, std::nano>(time_msg.nanosec);
89  auto s = std::chrono::duration<double>(time_msg.sec);
91  }
92 
93  inline tf2::TimePoint fromRclcpp(const rclcpp::Time & time)
94  {
95  // tf2::TimePoint is a typedef to a system time point, but rclcpp::Time may be ROS time.
96  // Ignore that, and assume the clock used from rclcpp time points is consistent.
97  return tf2::TimePoint(std::chrono::nanoseconds(time.nanoseconds()));
98  }
99 
100  inline rclcpp::Time toRclcpp(const tf2::TimePoint & time)
101  {
102  // tf2::TimePoint is a typedef to a system time point, but rclcpp::Time may be ROS time.
103  // Use whatever the default clock is.
104  return rclcpp::Time(std::chrono::nanoseconds(time.time_since_epoch()).count());
105  }
106 
107  inline tf2::Duration fromRclcpp(const rclcpp::Duration & duration)
108  {
109  return tf2::Duration(std::chrono::nanoseconds(duration.nanoseconds()));
110  }
111 
112  inline rclcpp::Duration toRclcpp(const tf2::Duration & duration)
113  {
114  return rclcpp::Duration(std::chrono::duration_cast<std::chrono::nanoseconds>(duration));
115  }
116 
121 {
122 public:
134  virtual geometry_msgs::msg::TransformStamped
135  lookupTransform(const std::string& target_frame, const std::string& source_frame,
136  const tf2::TimePoint& time, const tf2::Duration timeout) const = 0;
137 
151  virtual geometry_msgs::msg::TransformStamped
152  lookupTransform(const std::string& target_frame, const tf2::TimePoint& target_time,
153  const std::string& source_frame, const tf2::TimePoint& source_time,
154  const std::string& fixed_frame, const tf2::Duration timeout) const = 0;
155 
156 
166  virtual bool
167  canTransform(const std::string& target_frame, const std::string& source_frame,
168  const tf2::TimePoint& time, const tf2::Duration timeout, std::string* errstr = NULL) const = 0;
169 
181  virtual bool
182  canTransform(const std::string& target_frame, const tf2::TimePoint& target_time,
183  const std::string& source_frame, const tf2::TimePoint& source_time,
184  const std::string& fixed_frame, const tf2::Duration timeout, std::string* errstr = NULL) const = 0;
185 
197  template <class T>
198  T& transform(const T& in, T& out,
199  const std::string& target_frame, tf2::Duration timeout=tf2::durationFromSec(0.0)) const
200  {
201  // do the transform
202  tf2::doTransform(in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout));
203  return out;
204  }
205 
217  template <class T>
218  T transform(const T& in,
219  const std::string& target_frame, tf2::Duration timeout=tf2::durationFromSec(0.0)) const
220  {
221  T out;
222  return transform(in, out, target_frame, timeout);
223  }
224 
242  template <class A, class B>
243  B& transform(const A& in, B& out,
244  const std::string& target_frame, tf2::Duration timeout=tf2::durationFromSec(0.0)) const
245  {
246  A copy = transform(in, target_frame, timeout);
247  tf2::convert(copy, out);
248  return out;
249  }
250 
266  template <class T>
267  T& transform(const T& in, T& out,
268  const std::string& target_frame, const tf2::TimePoint& target_time,
269  const std::string& fixed_frame, tf2::Duration timeout=tf2::durationFromSec(0.0)) const
270  {
271  // do the transform
272  tf2::doTransform(in, out, lookupTransform(target_frame, target_time,
273  tf2::getFrameId(in), tf2::getTimestamp(in),
274  fixed_frame, timeout));
275  return out;
276  }
277 
293  template <class T>
294  T transform(const T& in,
295  const std::string& target_frame, const tf2::TimePoint& target_time,
296  const std::string& fixed_frame, tf2::Duration timeout=tf2::durationFromSec(0.0)) const
297  {
298  T out;
299  return transform(in, out, target_frame, target_time, fixed_frame, timeout);
300  }
301 
323  template <class A, class B>
324  B& transform(const A& in, B& out,
325  const std::string& target_frame, const tf2::TimePoint& target_time,
326  const std::string& fixed_frame, tf2::Duration timeout=tf2::durationFromSec(0.0)) const
327  {
328  // do the transform
329  A copy = transform(in, target_frame, target_time, fixed_frame, timeout);
330  tf2::convert(copy, out);
331  return out;
332  }
333 
335  {
336  }
337  }; // class
338 
339 
340 } // namespace
341 
342 #endif // TF2_ROS_BUFFER_INTERFACE_H
std::shared_future
std::string
tf2_ros::BufferInterface
Abstract interface for wrapping tf2::BufferCoreInterface in a ROS-based API. Implementations include ...
Definition: buffer_interface.h:120
std::chrono::nanoseconds
tf2_ros::fromRclcpp
tf2::TimePoint fromRclcpp(const rclcpp::Time &time)
Definition: buffer_interface.h:93
std::function
tf2_ros::BufferInterface::transform
T & transform(const T &in, T &out, const std::string &target_frame, tf2::Duration timeout=tf2::durationFromSec(0.0)) const
Transform an input into the target frame. This function is templated and can take as input any valid ...
Definition: buffer_interface.h:198
tf2_ros::toRclcpp
rclcpp::Time toRclcpp(const tf2::TimePoint &time)
Definition: buffer_interface.h:100
tf2_ros
Definition: async_buffer_interface.h:41
tf2_ros::BufferInterface::lookupTransform
virtual TF2_ROS_PUBLIC geometry_msgs::msg::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const tf2::TimePoint &time, const tf2::Duration timeout) const =0
Get the transform between two frames by frame ID.
TF2_ROS_PUBLIC
#define TF2_ROS_PUBLIC
Definition: visibility_control.h:58
tf2_ros::BufferInterface::transform
B & transform(const A &in, B &out, const std::string &target_frame, const tf2::TimePoint &target_time, const std::string &fixed_frame, tf2::Duration timeout=tf2::durationFromSec(0.0)) const
Transform an input into the target frame and convert to a specified output type (advanced)....
Definition: buffer_interface.h:324
visibility_control.h
tf2_ros::TransformReadyCallback
std::function< void(const TransformStampedFuture &)> TransformReadyCallback
Definition: async_buffer_interface.h:45
tf2_ros::fromMsg
tf2::TimePoint fromMsg(const builtin_interfaces::msg::Time &time_msg)
Definition: buffer_interface.h:60
tf2_ros::BufferInterface::canTransform
virtual TF2_ROS_PUBLIC bool canTransform(const std::string &target_frame, const std::string &source_frame, const tf2::TimePoint &time, const tf2::Duration timeout, std::string *errstr=NULL) const =0
Test if a transform is possible.
std::chrono::duration_cast
T duration_cast(T... args)
tf2_ros::BufferInterface::transform
T transform(const T &in, const std::string &target_frame, const tf2::TimePoint &target_time, const std::string &fixed_frame, tf2::Duration timeout=tf2::durationFromSec(0.0)) const
Transform an input into the target frame (advanced). This function is templated and can take as input...
Definition: buffer_interface.h:294
std::chrono::seconds::count
T count(T... args)
tf2_ros::BufferInterface::transform
B & transform(const A &in, B &out, const std::string &target_frame, tf2::Duration timeout=tf2::durationFromSec(0.0)) const
Transform an input into the target frame and convert to a specified output type. It is templated on t...
Definition: buffer_interface.h:243
tf2_ros::BufferInterface::transform
T transform(const T &in, const std::string &target_frame, tf2::Duration timeout=tf2::durationFromSec(0.0)) const
Transform an input into the target frame. This function is templated and can take as input any valid ...
Definition: buffer_interface.h:218
tf2_ros::toMsg
builtin_interfaces::msg::Time toMsg(const tf2::TimePoint &t)
Definition: buffer_interface.h:48
tf2_ros::timeToSec
double timeToSec(const builtin_interfaces::msg::Time &time_msg)
Definition: buffer_interface.h:86
tf2_ros::BufferInterface::transform
T & transform(const T &in, T &out, const std::string &target_frame, const tf2::TimePoint &target_time, const std::string &fixed_frame, tf2::Duration timeout=tf2::durationFromSec(0.0)) const
Transform an input into the target frame (advanced). This function is templated and can take as input...
Definition: buffer_interface.h:267
tf2_ros::BufferInterface::~BufferInterface
virtual ~BufferInterface()
Definition: buffer_interface.h:334
tf2_ros::TransformStampedFuture
std::shared_future< geometry_msgs::msg::TransformStamped > TransformStampedFuture
Definition: async_buffer_interface.h:44