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);
83  bool timed_out = false;
84  ros::Time start_time = ros::Time::now();
85  while(ros::ok() && !client_.getState().isDone() && !timed_out)
86  {
87  timed_out = ros::Time::now() > start_time + goal.timeout + timeout_padding_;
88  r.sleep();
89  }
90 
91  //this shouldn't happen, but could in rare cases where the server hangs
92  if(timed_out)
93  {
94  //make sure to cancel the goal the server is pursuing
96  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.");
97  }
98 
100  throw tf2::TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server.");
101 
102  //process the result for errors and return it
103  return processResult(*client_.getResult());
104  }
105 
106  geometry_msgs::TransformStamped BufferClient::processResult(const tf2_msgs::LookupTransformResult& result) const
107  {
108  //if there's no error, then we'll just return the transform
109  if(result.error.error != result.error.NO_ERROR){
110  //otherwise, we'll have to throw the appropriate exception
111  if(result.error.error == result.error.LOOKUP_ERROR)
112  throw tf2::LookupException(result.error.error_string);
113 
114  if(result.error.error == result.error.CONNECTIVITY_ERROR)
115  throw tf2::ConnectivityException(result.error.error_string);
116 
117  if(result.error.error == result.error.EXTRAPOLATION_ERROR)
118  throw tf2::ExtrapolationException(result.error.error_string);
119 
120  if(result.error.error == result.error.INVALID_ARGUMENT_ERROR)
121  throw tf2::InvalidArgumentException(result.error.error_string);
122 
123  if(result.error.error == result.error.TIMEOUT_ERROR)
124  throw tf2::TimeoutException(result.error.error_string);
125 
126  throw tf2::TransformException(result.error.error_string);
127  }
128 
129  return result.transform;
130  }
131 
132  bool BufferClient::canTransform(const std::string& target_frame, const std::string& source_frame,
133  const ros::Time& time, const ros::Duration timeout, std::string* errstr) const
134  {
135  try
136  {
137  lookupTransform(target_frame, source_frame, time, timeout);
138  return true;
139  }
140  catch(tf2::TransformException& ex)
141  {
142  if(errstr)
143  {
144  errstr->clear();
145  *errstr = ex.what();
146  }
147  return false;
148  }
149  }
150 
151  bool BufferClient::canTransform(const std::string& target_frame, const ros::Time& target_time,
152  const std::string& source_frame, const ros::Time& source_time,
153  const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const
154  {
155  try
156  {
157  lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout);
158  return true;
159  }
160  catch(tf2::TransformException& ex)
161  {
162  if(errstr)
163  {
164  errstr->clear();
165  *errstr = ex.what();
166  }
167  return false;
168  }
169  }
170 };
ROSCPP_DECL bool ok()
ros::Duration timeout_padding_
LookupActionClient client_
bool sleep()
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
geometry_msgs::TransformStamped processGoal(const tf2_msgs::LookupTransformGoal &goal) const
Definition: buffer.h:42
static Time now()
BufferClient(std::string ns, double check_frequency=10.0, ros::Duration timeout_padding=ros::Duration(2.0))
BufferClient constructor.
SimpleClientGoalState getState() const
ResultConstPtr getResult() 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.
geometry_msgs::TransformStamped processResult(const tf2_msgs::LookupTransformResult &result) const
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.


tf2_ros
Author(s): Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Oct 16 2020 19:08:54