buffer.cpp
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 
33 #include "tf2_ros/buffer.h"
34 
35 #include <ros/assert.h>
36 #include <sstream>
37 
38 namespace tf2_ros
39 {
40 
41 Buffer::Buffer(ros::Duration cache_time, bool debug) :
42  BufferCore(cache_time)
43 {
44  if(debug && !ros::service::exists("~tf2_frames", false))
45  {
46  ros::NodeHandle n("~");
47  frames_server_ = n.advertiseService("tf2_frames", &Buffer::getFrames, this);
48  }
49 }
50 
51 geometry_msgs::TransformStamped
52 Buffer::lookupTransform(const std::string& target_frame, const std::string& source_frame,
53  const ros::Time& time, const ros::Duration timeout) const
54 {
55  canTransform(target_frame, source_frame, time, timeout);
56  return lookupTransform(target_frame, source_frame, time);
57 }
58 
59 
60 geometry_msgs::TransformStamped
61 Buffer::lookupTransform(const std::string& target_frame, const ros::Time& target_time,
62  const std::string& source_frame, const ros::Time& source_time,
63  const std::string& fixed_frame, const ros::Duration timeout) const
64 {
65  canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout);
66  return lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame);
67 }
68 
74 {
75  try
76  {
77  return ros::Time::now();
78  }
80  {
82  return ros::Time(wt.sec, wt.nsec);
83  }
84 }
85 
92 {
93  try
94  {
95  d.sleep();
96  }
98  {
100  wd.sleep();
101  }
102 }
103 
104 void conditionally_append_timeout_info(std::string * errstr, const ros::Time& start_time,
105  const ros::Duration& timeout)
106 {
107  if (errstr)
108  {
109  std::stringstream ss;
110  ss << ". canTransform returned after "<< (now_fallback_to_wall() - start_time).toSec() \
111  <<" timeout was " << timeout.toSec() << ".";
112  (*errstr) += ss.str();
113  }
114 }
115 
116 bool
117 Buffer::canTransform(const std::string& target_frame, const std::string& source_frame,
118  const ros::Time& time, const ros::Duration timeout, std::string* errstr) const
119 {
120  // Clear the errstr before populating it if it's valid.
121  if (errstr)
122  {
123  errstr->clear();
124  }
125 
127  return false;
128 
129  // poll for transform if timeout is set
130  ros::Time start_time = now_fallback_to_wall();
131  while (now_fallback_to_wall() < start_time + timeout &&
132  !canTransform(target_frame, source_frame, time) &&
133  (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) && //don't wait when we detect a bag loop
134  (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf)
135  {
137  }
138  bool retval = canTransform(target_frame, source_frame, time, errstr);
139  conditionally_append_timeout_info(errstr, start_time, timeout);
140  return retval;
141 }
142 
143 
144 bool
145 Buffer::canTransform(const std::string& target_frame, const ros::Time& target_time,
146  const std::string& source_frame, const ros::Time& source_time,
147  const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const
148 {
149  // Clear the errstr before populating it if it's valid.
150  if (errstr)
151  {
152  errstr->clear();
153  }
154 
156  return false;
157 
158  // poll for transform if timeout is set
159  ros::Time start_time = now_fallback_to_wall();
160  while (now_fallback_to_wall() < start_time + timeout &&
161  !canTransform(target_frame, target_time, source_frame, source_time, fixed_frame) &&
162  (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) && //don't wait when we detect a bag loop
163  (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf)
164  {
166  }
167  bool retval = canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, errstr);
168  conditionally_append_timeout_info(errstr, start_time, timeout);
169  return retval;
170 }
171 
172 
173 bool Buffer::getFrames(tf2_msgs::FrameGraph::Request& req, tf2_msgs::FrameGraph::Response& res)
174 {
175  res.frame_yaml = allFramesAsYAML();
176  return true;
177 }
178 
179 
180 
181 bool Buffer::checkAndErrorDedicatedThreadPresent(std::string* error_str) const
182 {
184  return true;
185 
186 
187 
188  if (error_str)
189  *error_str = tf2_ros::threading_error;
190 
191  ROS_ERROR("%s", tf2_ros::threading_error.c_str());
192  return false;
193 }
194 
195 
196 
197 }
bool isUsingDedicatedThread() const
ros::Time now_fallback_to_wall()
Definition: buffer.cpp:73
ROSCPP_DECL bool isInitialized()
bool sleep() const
void sleep_fallback_to_wall(const ros::Duration &d)
Definition: buffer.cpp:91
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:52
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std::string allFramesAsYAML() const
static const std::string threading_error
Definition: buffer.h:139
bool sleep() const
ROSCPP_DECL bool ok()
void conditionally_append_timeout_info(std::string *errstr, const ros::Time &start_time, const ros::Duration &timeout)
Definition: buffer.cpp:104
static WallTime now()
Definition: buffer.h:42
ros::ServiceServer frames_server_
Definition: buffer.h:134
static Time now()
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:117
bool getFrames(tf2_msgs::FrameGraph::Request &req, tf2_msgs::FrameGraph::Response &res)
Definition: buffer.cpp:173
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
#define ROS_ERROR(...)
bool checkAndErrorDedicatedThreadPresent(std::string *errstr) const
Definition: buffer.cpp:181
Buffer(ros::Duration cache_time=ros::Duration(BufferCore::DEFAULT_CACHE_TIME), bool debug=false)
Constructor for a Buffer object.
Definition: buffer.cpp:41


tf2_ros
Author(s): Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Jan 4 2018 03:35:09