tf2_ros  master
This package contains the ROS bindings for the tf2 library, for both Python and C++.
buffer.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_H
33 #define TF2_ROS_BUFFER_H
34 
35 #include <memory>
36 #include <mutex>
37 #include <unordered_map>
38 
43 #include <tf2/buffer_core.h>
44 #include <tf2_msgs/srv/frame_graph.hpp>
45 #include <rclcpp/rclcpp.hpp>
46 //TODO(tfoote) review removal #include <tf2/convert.h>
47 
48 
49 namespace tf2_ros
50 {
51 
58  class Buffer: public BufferInterface, public AsyncBufferInterface, public tf2::BufferCore
59  {
60  public:
61  using tf2::BufferCore::lookupTransform;
62  using tf2::BufferCore::canTransform;
63 
71  TF2_ROS_PUBLIC Buffer(rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME));
72 
84  geometry_msgs::msg::TransformStamped
85  lookupTransform(const std::string& target_frame, const std::string& source_frame,
86  const tf2::TimePoint& time, const tf2::Duration timeout) const override;
87 
93  geometry_msgs::msg::TransformStamped
94  lookupTransform(const std::string& target_frame, const std::string& source_frame,
95  const rclcpp::Time & time, const rclcpp::Duration timeout=rclcpp::Duration(0)) const
96  {
97  return lookupTransform(target_frame, source_frame, fromRclcpp(time), fromRclcpp(timeout));
98  }
99 
113  geometry_msgs::msg::TransformStamped
114  lookupTransform(const std::string& target_frame, const tf2::TimePoint& target_time,
115  const std::string& source_frame, const tf2::TimePoint& source_time,
116  const std::string& fixed_frame, const tf2::Duration timeout) const override;
117 
124  geometry_msgs::msg::TransformStamped
125  lookupTransform(const std::string & target_frame, const rclcpp::Time & target_time,
126  const std::string & source_frame, const rclcpp::Time & source_time,
127  const std::string & fixed_frame, const rclcpp::Duration timeout=rclcpp::Duration(0)) const
128  {
129  return lookupTransform(
130  target_frame, fromRclcpp(target_time),
131  source_frame, fromRclcpp(source_time),
132  fixed_frame, fromRclcpp(timeout));
133  }
134 
144  bool
145  canTransform(const std::string& target_frame, const std::string& source_frame,
146  const tf2::TimePoint& target_time, const tf2::Duration timeout, std::string* errstr = NULL) const override;
147 
153  bool
154  canTransform(const std::string & target_frame, const std::string & source_frame,
155  const rclcpp::Time & time, const rclcpp::Duration timeout=rclcpp::Duration(0),
156  std::string * errstr=NULL) const
157  {
158  return canTransform(target_frame, source_frame, fromRclcpp(time), fromRclcpp(timeout), errstr);
159  }
160 
172  bool
173  canTransform(const std::string& target_frame, const tf2::TimePoint& target_time,
174  const std::string& source_frame, const tf2::TimePoint& source_time,
175  const std::string& fixed_frame, const tf2::Duration timeout, std::string* errstr = NULL) const override;
176 
184  bool
185  canTransform(const std::string & target_frame, const rclcpp::Time & target_time,
186  const std::string & source_frame, const rclcpp::Time & source_time,
187  const std::string & fixed_frame, const rclcpp::Duration timeout=rclcpp::Duration(0),
188  std::string * errstr=NULL) const
189  {
190  return canTransform(
191  target_frame, fromRclcpp(target_time),
192  source_frame, fromRclcpp(source_time),
193  fixed_frame, fromRclcpp(timeout),
194  errstr);
195  }
196 
215  waitForTransform(const std::string& target_frame, const std::string& source_frame, const tf2::TimePoint& time,
216  const tf2::Duration& timeout, TransformReadyCallback callback) override;
217 
224  waitForTransform(const std::string & target_frame, const std::string & source_frame,
225  const rclcpp::Time & time,
226  const rclcpp::Duration & timeout, TransformReadyCallback callback)
227  {
228  return waitForTransform(
229  target_frame, source_frame,
230  fromRclcpp(time), fromRclcpp(timeout),
231  callback);
232  }
233 
235  inline void
237  {
238  timer_interface_ = create_timer_interface;
239  }
240 
241  private:
242  void timerCallback(const TimerHandle & timer_handle,
244  TransformStampedFuture future,
245  TransformReadyCallback callback);
246 
247  bool getFrames(tf2_msgs::srv::FrameGraph::Request& req, tf2_msgs::srv::FrameGraph::Response& res) ;
248 
249  void onTimeJump(const rcl_time_jump_t & jump);
250 
251  // conditionally error if dedicated_thread unset.
252  bool checkAndErrorDedicatedThreadPresent(std::string* errstr) const;
253 
254 //TODO(tfoote)renable framegraph service
255 // ros::ServiceServer frames_server_;
256 
258  rclcpp::Clock::SharedPtr clock_;
259 
261  CreateTimerInterface::SharedPtr timer_interface_;
262 
265 
267  std::mutex timer_to_request_map_mutex_;
268 
270  rclcpp::JumpHandler::SharedPtr jump_handler_;
271  }; // class
272 
273 static const std::string threading_error = "Do not call canTransform or lookupTransform with a timeout unless you are using another thread for populating data. Without a dedicated thread it will always timeout. If you have a seperate thread servicing tf messages, call setUsingDedicatedThread(true) on your Buffer instance.";
274 
275 
276 } // namespace
277 
278 #endif // TF2_ROS_BUFFER_H
tf2_ros::Buffer::waitForTransform
TF2_ROS_PUBLIC TransformStampedFuture waitForTransform(const std::string &target_frame, const std::string &source_frame, const tf2::TimePoint &time, const tf2::Duration &timeout, TransformReadyCallback callback) override
Wait for a transform between two frames to become available.
std::shared_future
std::string
std::shared_ptr< CreateTimerInterface >
tf2_ros::TimerHandle
uint64_t TimerHandle
Definition: create_timer_interface.h:44
tf2_ros::BufferInterface
Abstract interface for wrapping tf2::BufferCoreInterface in a ROS-based API. Implementations include ...
Definition: buffer_interface.h:120
tf2_ros::Buffer::lookupTransform
TF2_ROS_PUBLIC geometry_msgs::msg::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const rclcpp::Time &time, const rclcpp::Duration timeout=rclcpp::Duration(0)) const
Get the transform between two frames by frame ID.
Definition: buffer.h:94
std::promise
tf2_ros::AsyncBufferInterface
Abstract interface for asynchronous operations on a tf2::BufferCoreInterface. Implementations include...
Definition: async_buffer_interface.h:51
tf2_ros::Buffer::lookupTransform
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 override
Get the transform between two frames by frame ID.
tf2_ros::fromRclcpp
tf2::TimePoint fromRclcpp(const rclcpp::Time &time)
Definition: buffer_interface.h:93
std::function
buffer_interface.h
tf2_ros::Buffer::canTransform
TF2_ROS_PUBLIC bool canTransform(const std::string &target_frame, const std::string &source_frame, const tf2::TimePoint &target_time, const tf2::Duration timeout, std::string *errstr=NULL) const override
Test if a transform is possible.
tf2_ros
Definition: async_buffer_interface.h:41
tf2_ros::Buffer::waitForTransform
TF2_ROS_PUBLIC TransformStampedFuture waitForTransform(const std::string &target_frame, const std::string &source_frame, const rclcpp::Time &time, const rclcpp::Duration &timeout, TransformReadyCallback callback)
Wait for a transform between two frames to become available.
Definition: buffer.h:224
create_timer_interface.h
tf2_ros::Buffer::setCreateTimerInterface
TF2_ROS_PUBLIC void setCreateTimerInterface(CreateTimerInterface::SharedPtr create_timer_interface)
Definition: buffer.h:236
tf2_ros::Buffer
Standard implementation of the tf2_ros::BufferInterface abstract data type.
Definition: buffer.h:58
TF2_ROS_PUBLIC
#define TF2_ROS_PUBLIC
Definition: visibility_control.h:58
visibility_control.h
tf2_ros::Buffer::lookupTransform
TF2_ROS_PUBLIC geometry_msgs::msg::TransformStamped lookupTransform(const std::string &target_frame, const rclcpp::Time &target_time, const std::string &source_frame, const rclcpp::Time &source_time, const std::string &fixed_frame, const rclcpp::Duration timeout=rclcpp::Duration(0)) const
Get the transform between two frames by frame ID assuming fixed frame.
Definition: buffer.h:125
async_buffer_interface.h
tf2_ros::Buffer::canTransform
TF2_ROS_PUBLIC bool canTransform(const std::string &target_frame, const rclcpp::Time &target_time, const std::string &source_frame, const rclcpp::Time &source_time, const std::string &fixed_frame, const rclcpp::Duration timeout=rclcpp::Duration(0), std::string *errstr=NULL) const
Test if a transform is possible.
Definition: buffer.h:185
std::mutex
tf2_ros::Buffer::Buffer
TF2_ROS_PUBLIC Buffer(rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time=tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME))
Constructor for a Buffer object.
tf2_ros::Buffer::canTransform
TF2_ROS_PUBLIC bool canTransform(const std::string &target_frame, const std::string &source_frame, const rclcpp::Time &time, const rclcpp::Duration timeout=rclcpp::Duration(0), std::string *errstr=NULL) const
Test if a transform is possible.
Definition: buffer.h:154
std::unordered_map< TimerHandle, tf2::TransformableRequestHandle >