buffer_client.cpp
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 #include <tf2_ros/buffer_client.h>
38 
39 namespace tf2_ros
40 {
41  BufferClient::BufferClient(std::string ns, double check_frequency, ros::Duration timeout_padding):
42  client_(ns),
43  check_frequency_(check_frequency),
44  timeout_padding_(timeout_padding)
45  {
46  }
47 
48  geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const std::string& source_frame,
49  const ros::Time& time, const ros::Duration timeout) const
50  {
51  //populate the goal message
52  tf2_msgs::LookupTransformGoal goal;
53  goal.target_frame = target_frame;
54  goal.source_frame = source_frame;
55  goal.source_time = time;
56  goal.timeout = timeout;
57  goal.advanced = false;
58 
59  return processGoal(goal);
60  }
61 
62  geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const ros::Time& target_time,
63  const std::string& source_frame, const ros::Time& source_time,
64  const std::string& fixed_frame, const ros::Duration timeout) const
65  {
66  //populate the goal message
67  tf2_msgs::LookupTransformGoal goal;
68  goal.target_frame = target_frame;
69  goal.source_frame = source_frame;
70  goal.source_time = source_time;
71  goal.timeout = timeout;
72  goal.target_time = target_time;
73  goal.fixed_frame = fixed_frame;
74  goal.advanced = true;
75 
76  return processGoal(goal);
77  }
78 
79  geometry_msgs::TransformStamped BufferClient::processGoal(const tf2_msgs::LookupTransformGoal& goal) const
80  {
81  client_.sendGoal(goal);
82 
83  //this shouldn't happen, but could in rare cases where the server hangs
84  if(!client_.waitForResult(goal.timeout + timeout_padding_))
85  {
86  //make sure to cancel the goal the server is pursuing
88  throw tf2::TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server.");
89  }
90 
92  throw tf2::TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server.");
93 
94  //process the result for errors and return it
95  return processResult(*client_.getResult());
96  }
97 
98  geometry_msgs::TransformStamped BufferClient::processResult(const tf2_msgs::LookupTransformResult& result) const
99  {
100  //if there's no error, then we'll just return the transform
101  if(result.error.error != result.error.NO_ERROR){
102  //otherwise, we'll have to throw the appropriate exception
103  if(result.error.error == result.error.LOOKUP_ERROR)
104  throw tf2::LookupException(result.error.error_string);
105 
106  if(result.error.error == result.error.CONNECTIVITY_ERROR)
107  throw tf2::ConnectivityException(result.error.error_string);
108 
109  if(result.error.error == result.error.EXTRAPOLATION_ERROR)
110  throw tf2::ExtrapolationException(result.error.error_string);
111 
112  if(result.error.error == result.error.INVALID_ARGUMENT_ERROR)
113  throw tf2::InvalidArgumentException(result.error.error_string);
114 
115  if(result.error.error == result.error.TIMEOUT_ERROR)
116  throw tf2::TimeoutException(result.error.error_string);
117 
118  throw tf2::TransformException(result.error.error_string);
119  }
120 
121  return result.transform;
122  }
123 
124  bool BufferClient::canTransform(const std::string& target_frame, const std::string& source_frame,
125  const ros::Time& time, const ros::Duration timeout, std::string* errstr) const
126  {
127  try
128  {
129  lookupTransform(target_frame, source_frame, time, timeout);
130  return true;
131  }
132  catch(tf2::TransformException& ex)
133  {
134  if(errstr)
135  {
136  errstr->clear();
137  *errstr = ex.what();
138  }
139  return false;
140  }
141  }
142 
143  bool BufferClient::canTransform(const std::string& target_frame, const ros::Time& target_time,
144  const std::string& source_frame, const ros::Time& source_time,
145  const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const
146  {
147  try
148  {
149  lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout);
150  return true;
151  }
152  catch(tf2::TransformException& ex)
153  {
154  if(errstr)
155  {
156  errstr->clear();
157  *errstr = ex.what();
158  }
159  return false;
160  }
161  }
162 };
ResultConstPtr getResult() const
geometry_msgs::TransformStamped processResult(const tf2_msgs::LookupTransformResult &result) const
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
virtual bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout=ros::Duration(0.0), std::string *errstr=NULL) const
Test if a transform is possible.
SimpleClientGoalState getState() const
ros::Duration timeout_padding_
LookupActionClient client_
geometry_msgs::TransformStamped processGoal(const tf2_msgs::LookupTransformGoal &goal) const
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout=ros::Duration(0.0)) const
Get the transform between two frames by frame ID.
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
Definition: buffer.h:42
BufferClient(std::string ns, double check_frequency=10.0, ros::Duration timeout_padding=ros::Duration(2.0))
BufferClient constructor.


tf2_ros
Author(s): Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Jun 27 2022 02:43:12