buffer_server.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_server.h>
38 
39 namespace tf2_ros
40 {
41  BufferServer::BufferServer(const Buffer& buffer, const std::string& ns, bool auto_start, ros::Duration check_period):
42  buffer_(buffer),
43  server_(ros::NodeHandle(),
44  ns,
45  boost::bind(&BufferServer::goalCB, this, _1),
46  boost::bind(&BufferServer::cancelCB, this, _1),
47  auto_start)
48  {
50  check_timer_ = n.createTimer(check_period, boost::bind(&BufferServer::checkTransforms, this, _1));
51  }
52 
54  {
55  (void) e; //Unused
56  boost::mutex::scoped_lock l(mutex_);
57  for(std::list<GoalInfo>::iterator it = active_goals_.begin(); it != active_goals_.end();)
58  {
59  GoalInfo& info = *it;
60 
61  //we want to lookup a transform if the time on the goal
62  //has expired, or a transform is available
63  if(canTransform(info.handle) || info.end_time < ros::Time::now())
64  {
65  tf2_msgs::LookupTransformResult result;
66 
67  //try to populate the result, catching exceptions if they occur
68  try
69  {
70  result.transform = lookupTransform(info.handle);
71  }
72  catch (tf2::ConnectivityException &ex)
73  {
74  result.error.error = result.error.CONNECTIVITY_ERROR;
75  result.error.error_string = ex.what();
76  }
77  catch (tf2::LookupException &ex)
78  {
79  result.error.error = result.error.LOOKUP_ERROR;
80  result.error.error_string = ex.what();
81  }
82  catch (tf2::ExtrapolationException &ex)
83  {
84  result.error.error = result.error.EXTRAPOLATION_ERROR;
85  result.error.error_string = ex.what();
86  }
88  {
89  result.error.error = result.error.INVALID_ARGUMENT_ERROR;
90  result.error.error_string = ex.what();
91  }
92  catch (tf2::TimeoutException &ex)
93  {
94  result.error.error = result.error.TIMEOUT_ERROR;
95  result.error.error_string = ex.what();
96  }
97  catch (tf2::TransformException &ex)
98  {
99  result.error.error = result.error.TRANSFORM_ERROR;
100  result.error.error_string = ex.what();
101  }
102 
103  //make sure to pass the result to the client
104  //even failed transforms are considered a success
105  //since the request was successfully processed
106  info.handle.setSucceeded(result);
107  it = active_goals_.erase(it);
108  }
109  else
110  ++it;
111  }
112  }
113 
115  {
116  boost::mutex::scoped_lock l(mutex_);
117  //we need to find the goal in the list and remove it... also setting it as canceled
118  //if its not in the list, we won't do anything since it will have already been set
119  //as completed
120  for(std::list<GoalInfo>::iterator it = active_goals_.begin(); it != active_goals_.end();)
121  {
122  GoalInfo& info = *it;
123  if(info.handle == gh)
124  {
125  info.handle.setCanceled();
126  it = active_goals_.erase(it);
127  return;
128  }
129  else
130  ++it;
131  }
132  }
133 
135  {
136  //we'll accept all goals we get
137  gh.setAccepted();
138 
139  //if the transform isn't immediately available, we'll push it onto our list to check
140  //along with the time that the goal will end
141  GoalInfo goal_info;
142  goal_info.handle = gh;
143  goal_info.end_time = ros::Time::now() + gh.getGoal()->timeout;
144 
145  //we can do a quick check here to see if the transform is valid
146  //we'll also do this if the end time has been reached
147  if(canTransform(gh) || goal_info.end_time <= ros::Time::now())
148  {
149  tf2_msgs::LookupTransformResult result;
150  try
151  {
152  result.transform = lookupTransform(gh);
153  }
154  catch (tf2::ConnectivityException &ex)
155  {
156  result.error.error = result.error.CONNECTIVITY_ERROR;
157  result.error.error_string = ex.what();
158  }
159  catch (tf2::LookupException &ex)
160  {
161  result.error.error = result.error.LOOKUP_ERROR;
162  result.error.error_string = ex.what();
163  }
164  catch (tf2::ExtrapolationException &ex)
165  {
166  result.error.error = result.error.EXTRAPOLATION_ERROR;
167  result.error.error_string = ex.what();
168  }
170  {
171  result.error.error = result.error.INVALID_ARGUMENT_ERROR;
172  result.error.error_string = ex.what();
173  }
174  catch (tf2::TimeoutException &ex)
175  {
176  result.error.error = result.error.TIMEOUT_ERROR;
177  result.error.error_string = ex.what();
178  }
179  catch (tf2::TransformException &ex)
180  {
181  result.error.error = result.error.TRANSFORM_ERROR;
182  result.error.error_string = ex.what();
183  }
184 
185  gh.setSucceeded(result);
186  return;
187  }
188 
189  boost::mutex::scoped_lock l(mutex_);
190  active_goals_.push_back(goal_info);
191  }
192 
194  {
195  const tf2_msgs::LookupTransformGoal::ConstPtr& goal = gh.getGoal();
196 
197  //check whether we need to used the advanced or simple api
198  if(!goal->advanced)
199  return buffer_.canTransform(goal->target_frame, goal->source_frame, goal->source_time);
200 
201  return buffer_.canTransform(goal->target_frame, goal->target_time,
202  goal->source_frame, goal->source_time, goal->fixed_frame);
203  }
204 
205  geometry_msgs::TransformStamped BufferServer::lookupTransform(GoalHandle gh)
206  {
207  const tf2_msgs::LookupTransformGoal::ConstPtr& goal = gh.getGoal();
208 
209  //check whether we need to used the advanced or simple api
210  if(!goal->advanced)
211  return buffer_.lookupTransform(goal->target_frame, goal->source_frame, goal->source_time);
212 
213  return buffer_.lookupTransform(goal->target_frame, goal->target_time,
214  goal->source_frame, goal->source_time, goal->fixed_frame);
215  }
216 
218  {
219  server_.start();
220  }
221 
222 };
void checkTransforms(const ros::TimerEvent &e)
boost::shared_ptr< const Goal > getGoal() const
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Standard implementation of the tf2_ros::BufferInterface abstract data type.
Definition: buffer.h:51
Action server for the actionlib-based implementation of tf2_ros::BufferInterface. ...
Definition: buffer_server.h:52
bool canTransform(GoalHandle gh)
void setCanceled(const Result &result=Result(), const std::string &text=std::string(""))
virtual bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &target_time, const ros::Duration timeout, std::string *errstr=NULL) const
Test if a transform is possible.
Definition: buffer.cpp:119
void setAccepted(const std::string &text=std::string(""))
LookupTransformServer server_
Definition: buffer_server.h:86
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
Get the transform between two frames by frame ID.
Definition: buffer.cpp:54
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
geometry_msgs::TransformStamped lookupTransform(GoalHandle gh)
const Buffer & buffer_
Definition: buffer_server.h:85
Definition: buffer.h:42
void goalCB(GoalHandle gh)
static Time now()
void start()
Start the action server.
BufferServer(const Buffer &buffer, const std::string &ns, bool auto_start=true, ros::Duration check_period=ros::Duration(0.01))
Constructor.
std::list< GoalInfo > active_goals_
Definition: buffer_server.h:87
void cancelCB(GoalHandle gh)


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