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


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