tf2_ros  master
This package contains the ROS bindings for the tf2 library, for both Python and C++.
buffer_client.h
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2009, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
37 #ifndef TF2_ROS_BUFFER_CLIENT_H_
38 #define TF2_ROS_BUFFER_CLIENT_H_
39 
40 #include <rclcpp_action/rclcpp_action.hpp>
41 #include <tf2_msgs/action/lookup_transform.hpp>
42 
45 
46 namespace tf2_ros
47 {
51  class LookupTransformGoalException : public std::runtime_error
52  {
53  public:
55  explicit LookupTransformGoalException(const std::string & message)
56  : std::runtime_error(message)
57  {
58  }
59  };
60 
61  class GoalRejectedException : public LookupTransformGoalException
62  {
63  public:
65  explicit GoalRejectedException(const std::string & message)
67  {
68  }
69  };
70 
71  class GoalAbortedException : public LookupTransformGoalException
72  {
73  public:
75  explicit GoalAbortedException(const std::string & message)
77  {
78  }
79  };
80 
81  class GoalCanceledException : public LookupTransformGoalException
82  {
83  public:
85  explicit GoalCanceledException(const std::string & message)
87  {
88  }
89  };
90 
91  class UnexpectedResultCodeException : public LookupTransformGoalException
92  {
93  public:
95  explicit UnexpectedResultCodeException(const std::string & message)
97  {
98  }
99  };
100 
107  class BufferClient : public BufferInterface
108  {
109  public:
110  using LookupTransformAction = tf2_msgs::action::LookupTransform;
111 
118  template<typename NodePtr>
119  BufferClient(
120  NodePtr node,
121  const std::string ns,
122  const double& check_frequency = 10.0,
123  const tf2::Duration& timeout_padding = tf2::durationFromSec(2.0))
124  : check_frequency_(check_frequency),
125  timeout_padding_(timeout_padding)
126  {
127  client_ = rclcpp_action::create_client<LookupTransformAction>(node, ns);
128  }
129  virtual ~BufferClient() = default;
130 
154  geometry_msgs::msg::TransformStamped
156  const std::string& target_frame,
157  const std::string& source_frame,
158  const tf2::TimePoint& time,
159  const tf2::Duration timeout = tf2::durationFromSec(0.0)) const override;
160 
186  geometry_msgs::msg::TransformStamped
188  const std::string& target_frame,
189  const tf2::TimePoint& target_time,
190  const std::string& source_frame,
191  const tf2::TimePoint& source_time,
192  const std::string& fixed_frame,
193  const tf2::Duration timeout = tf2::durationFromSec(0.0)) const override;
194 
204  bool
205  canTransform(
206  const std::string& target_frame,
207  const std::string& source_frame,
208  const tf2::TimePoint& time,
209  const tf2::Duration timeout = tf2::durationFromSec(0.0),
210  std::string* errstr = nullptr) const override;
211 
223  bool
224  canTransform(
225  const std::string& target_frame,
226  const tf2::TimePoint& target_time,
227  const std::string& source_frame,
228  const tf2::TimePoint& source_time,
229  const std::string& fixed_frame,
230  const tf2::Duration timeout = tf2::durationFromSec(0.0),
231  std::string* errstr = nullptr) const override;
232 
238  bool waitForServer(const tf2::Duration& timeout = tf2::durationFromSec(0))
239  {
240  return client_->wait_for_action_server(timeout);
241  }
242 
243  private:
244  geometry_msgs::msg::TransformStamped
245  processGoal(const LookupTransformAction::Goal& goal) const;
246 
247  geometry_msgs::msg::TransformStamped
248  processResult(const LookupTransformAction::Result::SharedPtr& result) const;
249 
250  rclcpp_action::Client<LookupTransformAction>::SharedPtr client_;
251  double check_frequency_;
252  tf2::Duration timeout_padding_;
253  };
254 }
255 #endif
tf2_ros::LookupTransformGoalException
Definition: buffer_client.h:86
std::string
tf2_ros::BufferInterface
Abstract interface for wrapping tf2::BufferCoreInterface in a ROS-based API. Implementations include ...
Definition: buffer_interface.h:120
tf2_ros::BufferClient::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=tf2::durationFromSec(0.0)) const override
Get the transform between two frames by frame ID.
tf2_ros::BufferClient::LookupTransformAction
tf2_msgs::action::LookupTransform LookupTransformAction
Definition: buffer_client.h:145
tf2_ros::BufferClient::canTransform
TF2_ROS_PUBLIC bool canTransform(const std::string &target_frame, const std::string &source_frame, const tf2::TimePoint &time, const tf2::Duration timeout=tf2::durationFromSec(0.0), std::string *errstr=nullptr) const override
Test if a transform is possible.
tf2_ros::UnexpectedResultCodeException::UnexpectedResultCodeException
TF2_ROS_PUBLIC UnexpectedResultCodeException(const std::string &message)
Definition: buffer_client.h:130
tf2_ros::LookupTransformGoalException::LookupTransformGoalException
TF2_ROS_PUBLIC LookupTransformGoalException(const std::string &message)
Definition: buffer_client.h:125
buffer_interface.h
tf2_ros
Definition: async_buffer_interface.h:41
tf2_ros::GoalRejectedException::GoalRejectedException
TF2_ROS_PUBLIC GoalRejectedException(const std::string &message)
Definition: buffer_client.h:100
std::runtime_error
tf2_ros::BufferClient::waitForServer
TF2_ROS_PUBLIC bool waitForServer(const tf2::Duration &timeout=tf2::durationFromSec(0))
Block until the action server is ready to respond to requests.
Definition: buffer_client.h:273
TF2_ROS_PUBLIC
#define TF2_ROS_PUBLIC
Definition: visibility_control.h:58
visibility_control.h
tf2_ros::BufferClient::BufferClient
BufferClient(NodePtr node, const std::string ns, const double &check_frequency=10.0, const tf2::Duration &timeout_padding=tf2::durationFromSec(2.0))
BufferClient constructor.
Definition: buffer_client.h:154
tf2_ros::GoalCanceledException::GoalCanceledException
TF2_ROS_PUBLIC GoalCanceledException(const std::string &message)
Definition: buffer_client.h:120
std
tf2_ros::BufferClient
Action client-based implementation of the tf2_ros::BufferInterface abstract data type.
Definition: buffer_client.h:142
tf2_ros::GoalAbortedException::GoalAbortedException
TF2_ROS_PUBLIC GoalAbortedException(const std::string &message)
Definition: buffer_client.h:110
tf2_ros::BufferClient::~BufferClient
virtual ~BufferClient()=default