tf.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 
32 #include "tf/tf.h"
33 #include "ros/assert.h"
34 #include "ros/ros.h"
35 #include <angles/angles.h>
36 
37 using namespace tf;
38 
39 // Must provide storage for non-integral static const class members.
40 // Otherwise you get undefined symbol errors on OS X (why not on Linux?).
41 // Thanks to Rob for pointing out the right way to do this.
42 // In C++0x this must be initialized here #5401
43 const double tf::Transformer::DEFAULT_CACHE_TIME = 10.0;
44 
45 
47 {
52 };
53 
55 {
56  CompactFrameID gather(TimeCache* cache, ros::Time time, std::string* error_string)
57  {
58  return cache->getParent(time, error_string);
59  }
60 
61  void accum(bool source)
62  {
63  }
64 
65  void finalize(WalkEnding end, ros::Time _time)
66  {
67  }
68 
70 };
71 
73 {
75  : source_to_top_quat(0.0, 0.0, 0.0, 1.0)
76  , source_to_top_vec(0.0, 0.0, 0.0)
77  , target_to_top_quat(0.0, 0.0, 0.0, 1.0)
78  , target_to_top_vec(0.0, 0.0, 0.0)
79  , result_quat(0.0, 0.0, 0.0, 1.0)
80  , result_vec(0.0, 0.0, 0.0)
81  {
82  }
83 
84  CompactFrameID gather(TimeCache* cache, ros::Time time, std::string* error_string)
85  {
86  if (!cache->getData(time, st, error_string))
87  {
88  return 0;
89  }
90 
91  return st.frame_id_;
92  }
93 
94  void accum(bool source)
95  {
96  if (source)
97  {
98  source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_;
99  source_to_top_quat = st.rotation_ * source_to_top_quat;
100  }
101  else
102  {
103  target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_;
104  target_to_top_quat = st.rotation_ * target_to_top_quat;
105  }
106  }
107 
108  void finalize(WalkEnding end, ros::Time _time)
109  {
110  switch (end)
111  {
112  case Identity:
113  break;
115  result_vec = source_to_top_vec;
116  result_quat = source_to_top_quat;
117  break;
119  {
120  tf::Quaternion inv_target_quat = target_to_top_quat.inverse();
121  tf::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
122  result_vec = inv_target_vec;
123  result_quat = inv_target_quat;
124  break;
125  }
126  case FullPath:
127  {
128  tf::Quaternion inv_target_quat = target_to_top_quat.inverse();
129  tf::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
130 
131  result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
132  result_quat = inv_target_quat * source_to_top_quat;
133  }
134  break;
135  };
136 
137  time = _time;
138  }
139 
143  tf::Vector3 source_to_top_vec;
145  tf::Vector3 target_to_top_vec;
146 
148  tf::Vector3 result_vec;
149 };
150 
151 
152 std::string assert_resolved(const std::string& prefix, const std::string& frame_id)
153 {
154  ROS_DEBUG("tf::assert_resolved just calls tf::resolve");
155  return tf::resolve(prefix, frame_id);
156 }
157 
158 std::string tf::resolve(const std::string& prefix, const std::string& frame_name)
159 {
160  // printf ("resolveping prefix:%s with frame_name:%s\n", prefix.c_str(), frame_name.c_str());
161  if (frame_name.size() > 0)
162  if (frame_name[0] == '/')
163  {
164  return strip_leading_slash(frame_name);
165  }
166  if (prefix.size() > 0)
167  {
168  if (prefix[0] == '/')
169  {
170  std::string composite = strip_leading_slash(prefix);
171  composite.append("/");
172  composite.append(frame_name);
173  return composite;
174  }
175  else
176  {
177  std::string composite;
178  composite.append(prefix);
179  composite.append("/");
180  composite.append(frame_name);
181  return composite;
182  }
183 
184  }
185  else
186  {
187  std::string composite;
188  composite.append(frame_name);
189  return composite;
190  }
191 }
192 
193 
194 std::string tf::strip_leading_slash(const std::string& frame_name)
195 {
196  if (frame_name.size() > 0)
197  if (frame_name[0] == '/')
198  {
199  std::string shorter = frame_name;
200  shorter.erase(0,1);
201  return shorter;
202  }
203 
204  return frame_name;
205 }
206 
207 
208 Transformer::Transformer(bool interpolating,
209  ros::Duration cache_time):
210  fall_back_to_wall_time_(false),
211  tf2_buffer_ptr_(std::make_shared<tf2_ros::Buffer>(cache_time))
212 {
213 
214 }
215 
217 {
218 
219 }
220 
221 
223 {
224  tf2_buffer_ptr_->clear();
225 }
226 
227 
228 bool Transformer::setTransform(const StampedTransform& transform, const std::string& authority)
229 {
230  geometry_msgs::TransformStamped msgtf;
231  transformStampedTFToMsg(transform, msgtf);
232  return tf2_buffer_ptr_->setTransform(msgtf, authority);
233 
234 }
235 
236 
237 void Transformer::lookupTransform(const std::string& target_frame, const std::string& source_frame,
238  const ros::Time& time, StampedTransform& transform) const
239 {
240  geometry_msgs::TransformStamped output =
241  tf2_buffer_ptr_->lookupTransform(strip_leading_slash(target_frame),
242  strip_leading_slash(source_frame), time);
243  transformStampedMsgToTF(output, transform);
244  return;
245 }
246 
247 
248 void Transformer::lookupTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
249  const ros::Time& source_time, const std::string& fixed_frame, StampedTransform& transform) const
250 {
251  geometry_msgs::TransformStamped output =
252  tf2_buffer_ptr_->lookupTransform(strip_leading_slash(target_frame), target_time,
253  strip_leading_slash(source_frame), source_time,
254  strip_leading_slash(fixed_frame));
255  transformStampedMsgToTF(output, transform);
256 }
257 
258 
259 void Transformer::lookupTwist(const std::string& tracking_frame, const std::string& observation_frame,
260  const ros::Time& time, const ros::Duration& averaging_interval,
261  geometry_msgs::Twist& twist) const
262 {
263  // ref point is origin of tracking_frame, ref_frame = obs_frame
264  lookupTwist(tracking_frame, observation_frame, observation_frame, tf::Point(0,0,0), tracking_frame, time, averaging_interval, twist);
265 }
266 
267 
268 
269 void Transformer::lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
270  const tf::Point & reference_point, const std::string& reference_point_frame,
271  const ros::Time& time, const ros::Duration& averaging_interval,
272  geometry_msgs::Twist& twist) const
273 {
274 
275  ros::Time latest_time, target_time;
276  getLatestCommonTime(observation_frame, tracking_frame, latest_time, NULL);
277 
278  if (ros::Time() == time)
279  target_time = latest_time;
280  else
281  target_time = time;
282 
283  ros::Time end_time = std::min(target_time + averaging_interval *0.5 , latest_time);
284 
285  ros::Time start_time = std::max(ros::Time().fromSec(.00001) + averaging_interval, end_time) - averaging_interval; // don't collide with zero
286  ros::Duration corrected_averaging_interval = end_time - start_time; //correct for the possiblity that start time was truncated above.
287  StampedTransform start, end;
288  lookupTransform(observation_frame, tracking_frame, start_time, start);
289  lookupTransform(observation_frame, tracking_frame, end_time, end);
290 
291 
292  tf::Matrix3x3 temp = start.getBasis().inverse() * end.getBasis();
293  tf::Quaternion quat_temp;
294  temp.getRotation(quat_temp);
295  tf::Vector3 o = start.getBasis() * quat_temp.getAxis();
296  tfScalar ang = quat_temp.getAngle();
297 
298  double delta_x = end.getOrigin().getX() - start.getOrigin().getX();
299  double delta_y = end.getOrigin().getY() - start.getOrigin().getY();
300  double delta_z = end.getOrigin().getZ() - start.getOrigin().getZ();
301 
302 
303  tf::Vector3 twist_vel ((delta_x)/corrected_averaging_interval.toSec(),
304  (delta_y)/corrected_averaging_interval.toSec(),
305  (delta_z)/corrected_averaging_interval.toSec());
306  tf::Vector3 twist_rot = o * (ang / corrected_averaging_interval.toSec());
307 
308 
309  // This is a twist w/ reference frame in observation_frame and reference point is in the tracking_frame at the origin (at start_time)
310 
311 
312  //correct for the position of the reference frame
314  lookupTransform(reference_frame,tracking_frame, target_time, inverse);
315  tf::Vector3 out_rot = inverse.getBasis() * twist_rot;
316  tf::Vector3 out_vel = inverse.getBasis()* twist_vel + inverse.getOrigin().cross(out_rot);
317 
318 
319  //Rereference the twist about a new reference point
320  // Start by computing the original reference point in the reference frame:
321  tf::Stamped<tf::Point> rp_orig(tf::Point(0,0,0), target_time, tracking_frame);
322  transformPoint(reference_frame, rp_orig, rp_orig);
323  // convert the requrested reference point into the right frame
324  tf::Stamped<tf::Point> rp_desired(reference_point, target_time, reference_point_frame);
325  transformPoint(reference_frame, rp_desired, rp_desired);
326  // compute the delta
327  tf::Point delta = rp_desired - rp_orig;
328  // Correct for the change in reference point
329  out_vel = out_vel + out_rot * delta;
330  // out_rot unchanged
331 
332  /*
333  printf("KDL: Rotation %f %f %f, Translation:%f %f %f\n",
334  out_rot.x(),out_rot.y(),out_rot.z(),
335  out_vel.x(),out_vel.y(),out_vel.z());
336  */
337 
338  twist.linear.x = out_vel.x();
339  twist.linear.y = out_vel.y();
340  twist.linear.z = out_vel.z();
341  twist.angular.x = out_rot.x();
342  twist.angular.y = out_rot.y();
343  twist.angular.z = out_rot.z();
344 
345 }
346 
347 bool Transformer::waitForTransform(const std::string& target_frame, const std::string& source_frame,
348  const ros::Time& time,
349  const ros::Duration& timeout, const ros::Duration& polling_sleep_duration,
350  std::string* error_msg) const
351 {
352  return tf2_buffer_ptr_->canTransform(strip_leading_slash(target_frame),
353  strip_leading_slash(source_frame), time, timeout, error_msg);
354 }
355 
356 
357 bool Transformer::canTransform(const std::string& target_frame, const std::string& source_frame,
358  const ros::Time& time, std::string* error_msg) const
359 {
360  return tf2_buffer_ptr_->canTransform(strip_leading_slash(target_frame),
361  strip_leading_slash(source_frame), time, error_msg);
362 }
363 
364 
365 bool Transformer::canTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
366  const ros::Time& source_time, const std::string& fixed_frame,
367  std::string* error_msg) const
368 {
369  return tf2_buffer_ptr_->canTransform(strip_leading_slash(target_frame), target_time,
370  strip_leading_slash(source_frame), source_time,
371  strip_leading_slash(fixed_frame), error_msg);
372 }
373 
374 bool Transformer::waitForTransform(const std::string& target_frame,const ros::Time& target_time, const std::string& source_frame,
375  const ros::Time& source_time, const std::string& fixed_frame,
376  const ros::Duration& timeout, const ros::Duration& polling_sleep_duration,
377  std::string* error_msg) const
378 {
379  return tf2_buffer_ptr_->canTransform(strip_leading_slash(target_frame), target_time,
380  strip_leading_slash(source_frame), source_time,
381  strip_leading_slash(fixed_frame), timeout, error_msg);
382 }
383 
384 
385 bool Transformer::getParent(const std::string& frame_id, ros::Time time, std::string& parent) const
386 {
387  return tf2_buffer_ptr_->_getParent(strip_leading_slash(frame_id), time, parent);
388 }
389 
390 
391 bool Transformer::frameExists(const std::string& frame_id_str) const
392 {
393  return tf2_buffer_ptr_->_frameExists(strip_leading_slash(frame_id_str));
394 }
395 
397 {
398  ROS_WARN("Transformer::setExtrapolationLimit is deprecated and does not do anything");
399 }
400 
401 
403 {
405  : id(id)
406  {}
407 
408  bool operator()(const P_TimeAndFrameID& rhs) const
409  {
410  return rhs.second == id;
411  }
412 
414 };
415 
416 int Transformer::getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time& time, std::string* error_string) const
417 {
418  CompactFrameID target_id = tf2_buffer_ptr_->_lookupFrameNumber(strip_leading_slash(target_frame));
419  CompactFrameID source_id = tf2_buffer_ptr_->_lookupFrameNumber(strip_leading_slash(source_frame));
420 
421  return tf2_buffer_ptr_->_getLatestCommonTime(source_id, target_id, time, error_string);
422 }
423 
424 
425 //@todo - Fix this to work with new data structures
426 void Transformer::chainAsVector(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string& fixed_frame, std::vector<std::string>& output) const
427 {
428  tf2_buffer_ptr_->_chainAsVector(target_frame, target_time,
429  source_frame, source_time,
430  fixed_frame, output);
431 }
432 
434 {
435  return tf2_buffer_ptr_->allFramesAsString();
436 }
437 
438 std::string Transformer::allFramesAsDot(double current_time) const
439 {
440  return tf2_buffer_ptr_->_allFramesAsDot(current_time);
441 }
442 
443 
444 bool Transformer::ok() const { return true; }
445 
446 void Transformer::getFrameStrings(std::vector<std::string> & vec) const
447 {
448  tf2_buffer_ptr_->_getFrameStrings(vec);
449 }
450 
451 
452 void Transformer::transformQuaternion(const std::string& target_frame, const Stamped<Quaternion>& stamped_in, Stamped<Quaternion>& stamped_out) const
453 {
454  tf::assertQuaternionValid(stamped_in);
455 
456  StampedTransform transform;
457  lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
458 
459  stamped_out.setData( transform * stamped_in);
460  stamped_out.stamp_ = transform.stamp_;
461  stamped_out.frame_id_ = target_frame;
462 }
463 
464 
465 void Transformer::transformVector(const std::string& target_frame,
466  const Stamped<tf::Vector3>& stamped_in,
467  Stamped<tf::Vector3>& stamped_out) const
468 {
469  StampedTransform transform;
470  lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
471 
473  tf::Vector3 end = stamped_in;
474  tf::Vector3 origin = tf::Vector3(0,0,0);
475  tf::Vector3 output = (transform * end) - (transform * origin);
476  stamped_out.setData( output);
477 
478  stamped_out.stamp_ = transform.stamp_;
479  stamped_out.frame_id_ = target_frame;
480 }
481 
482 
483 void Transformer::transformPoint(const std::string& target_frame, const Stamped<Point>& stamped_in, Stamped<Point>& stamped_out) const
484 {
485  StampedTransform transform;
486  lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
487 
488  stamped_out.setData(transform * stamped_in);
489  stamped_out.stamp_ = transform.stamp_;
490  stamped_out.frame_id_ = target_frame;
491 }
492 
493 void Transformer::transformPose(const std::string& target_frame, const Stamped<Pose>& stamped_in, Stamped<Pose>& stamped_out) const
494 {
495  StampedTransform transform;
496  lookupTransform(target_frame, stamped_in.frame_id_, stamped_in.stamp_, transform);
497 
498  stamped_out.setData(transform * stamped_in);
499  stamped_out.stamp_ = transform.stamp_;
500  stamped_out.frame_id_ = target_frame;
501 }
502 
503 
504 void Transformer::transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
505  const Stamped<Quaternion>& stamped_in,
506  const std::string& fixed_frame,
507  Stamped<Quaternion>& stamped_out) const
508 {
509  tf::assertQuaternionValid(stamped_in);
510  StampedTransform transform;
511  lookupTransform(target_frame, target_time,
512  stamped_in.frame_id_,stamped_in.stamp_,
513  fixed_frame, transform);
514 
515  stamped_out.setData( transform * stamped_in);
516  stamped_out.stamp_ = transform.stamp_;
517  stamped_out.frame_id_ = target_frame;
518 }
519 
520 
521 void Transformer::transformVector(const std::string& target_frame, const ros::Time& target_time,
522  const Stamped<Vector3>& stamped_in,
523  const std::string& fixed_frame,
524  Stamped<Vector3>& stamped_out) const
525 {
526  StampedTransform transform;
527  lookupTransform(target_frame, target_time,
528  stamped_in.frame_id_,stamped_in.stamp_,
529  fixed_frame, transform);
530 
532  tf::Vector3 end = stamped_in;
533  tf::Vector3 origin = tf::Vector3(0,0,0);
534  tf::Vector3 output = (transform * end) - (transform * origin);
535  stamped_out.setData( output);
536 
537  stamped_out.stamp_ = transform.stamp_;
538  stamped_out.frame_id_ = target_frame;
539 }
540 
541 
542 void Transformer::transformPoint(const std::string& target_frame, const ros::Time& target_time,
543  const Stamped<Point>& stamped_in,
544  const std::string& fixed_frame,
545  Stamped<Point>& stamped_out) const
546 {
547  StampedTransform transform;
548  lookupTransform(target_frame, target_time,
549  stamped_in.frame_id_,stamped_in.stamp_,
550  fixed_frame, transform);
551 
552  stamped_out.setData(transform * stamped_in);
553  stamped_out.stamp_ = transform.stamp_;
554  stamped_out.frame_id_ = target_frame;
555 }
556 
557 void Transformer::transformPose(const std::string& target_frame, const ros::Time& target_time,
558  const Stamped<Pose>& stamped_in,
559  const std::string& fixed_frame,
560  Stamped<Pose>& stamped_out) const
561 {
562  StampedTransform transform;
563  lookupTransform(target_frame, target_time,
564  stamped_in.frame_id_,stamped_in.stamp_,
565  fixed_frame, transform);
566 
567  stamped_out.setData(transform * stamped_in);
568  stamped_out.stamp_ = transform.stamp_;
569  stamped_out.frame_id_ = target_frame;
570 }
571 
572 boost::signals2::connection Transformer::addTransformsChangedListener(boost::function<void(void)> callback)
573 {
574  return tf2_buffer_ptr_->_addTransformsChangedListener(callback);
575 }
576 
577 void Transformer::removeTransformsChangedListener(boost::signals2::connection c)
578 {
579  tf2_buffer_ptr_->_removeTransformsChangedListener(c);
580 }
bool getParent(const std::string &frame_id, ros::Time time, std::string &parent) const
Fill the parent of a frame.
Definition: tf.cpp:385
void assertQuaternionValid(const tf::Quaternion &q)
Throw InvalidArgument if quaternion is malformed.
Definition: tf.h:415
std::string assert_resolved(const std::string &prefix, const std::string &frame_id)
Definition: tf.cpp:152
TransformStorage st
Definition: tf.cpp:69
uint32_t CompactFrameID
void getRotation(Quaternion &q) const
Get the matrix represented as a quaternion.
Definition: Matrix3x3.h:245
Quaternion inverse() const
Return the inverse of this quaternion.
Definition: Quaternion.h:259
std::string allFramesAsString() const
A way to see what frames have been cached Useful for debugging.
Definition: tf.cpp:433
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30
TimeAndFrameIDFrameComparator(CompactFrameID id)
Definition: tf.cpp:404
bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const
Test if a transform is possible.
Definition: tf.cpp:357
ROSCPP_DECL void start()
tf::Vector3 source_to_top_vec
Definition: tf.cpp:143
tf::Quaternion source_to_top_quat
Definition: tf.cpp:142
void setData(const T &input)
std::string allFramesAsDot(double current_time=0) const
A way to see what frames have been cached Useful for debugging.
Definition: tf.cpp:438
double tfScalar
The tfScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: Scalar.h:160
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: Transform.h:110
tf::Vector3 result_vec
Definition: tf.cpp:148
Definition: tf.cpp:51
Matrix3x3 inverse() const
Return the inverse of the matrix.
Definition: Matrix3x3.h:567
void accum(bool source)
Definition: tf.cpp:94
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
Block until a transform is possible or it times out.
Definition: tf.cpp:347
bool setTransform(const StampedTransform &transform, const std::string &authority="default_authority")
Add transform information to the tf data structure.
Definition: tf.cpp:228
static const double DEFAULT_CACHE_TIME
10.0 is the default amount of time to cache data in seconds, set in cpp file.
Definition: tf.h:114
A class to keep a sorted linked list in time This builds and maintains a list of timestamped data...
Definition: time_cache.h:97
Definition: exceptions.h:38
ros::Time stamp_
The timestamp associated with this transform.
tf::Vector3 Point
CompactFrameID id
Definition: tf.cpp:413
std::pair< ros::Time, CompactFrameID > P_TimeAndFrameID
#define ROS_WARN(...)
void chainAsVector(const std::string &target_frame, ros::Time target_time, const std::string &source_frame, ros::Time source_time, const std::string &fixed_frame, std::vector< std::string > &output) const
Debugging function that will print the spanning chain of transforms. Possible exceptions tf::LookupEx...
Definition: tf.cpp:426
The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Q...
Definition: Matrix3x3.h:33
bool operator()(const P_TimeAndFrameID &rhs) const
Definition: tf.cpp:408
virtual bool ok() const
Definition: tf.cpp:444
std::string resolve(const std::string &prefix, const std::string &frame_name)
resolve tf names
Definition: tf.cpp:158
virtual ~Transformer(void)
Definition: tf.cpp:216
void removeTransformsChangedListener(boost::signals2::connection c)
Definition: tf.cpp:577
void transformPose(const std::string &target_frame, const Stamped< tf::Pose > &stamped_in, Stamped< tf::Pose > &stamped_out) const
Transform a Stamped Pose into the target frame This can throw anything a lookupTransform can throw as...
Definition: tf.cpp:493
Vector3 getAxis() const
Return the axis of the rotation represented by this quaternion.
Definition: Quaternion.h:249
ros::Time time
Definition: tf.cpp:141
tf::Vector3 target_to_top_vec
Definition: tf.cpp:145
Definition: tf.cpp:48
WalkEnding
void lookupTwist(const std::string &tracking_frame, const std::string &observation_frame, const std::string &reference_frame, const tf::Point &reference_point, const std::string &reference_point_frame, const ros::Time &time, const ros::Duration &averaging_interval, geometry_msgs::Twist &twist) const
Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame u...
Definition: tf.cpp:269
The data type which will be cross compatable with geometry_msgs This is the tf datatype equivilant of...
tf::Quaternion target_to_top_quat
Definition: tf.cpp:144
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
Return the inverse of a quaternion.
Definition: Quaternion.h:421
CompactFrameID gather(TimeCache *cache, ros::Time time, std::string *error_string)
Definition: tf.cpp:84
void accum(bool source)
Definition: tf.cpp:61
bool frameExists(const std::string &frame_id_str) const
Check if a frame exists in the tree.
Definition: tf.cpp:391
void finalize(WalkEnding end, ros::Time _time)
Definition: tf.cpp:65
TFSIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
Definition: Quaternion.h:438
CompactFrameID getParent(ros::Time time, std::string *error_str)
Definition: cache.cpp:225
std::shared_ptr< tf2_ros::Buffer > tf2_buffer_ptr_
Definition: tf.h:409
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
Return the origin vector translation.
Definition: Transform.h:115
void getFrameStrings(std::vector< std::string > &ids) const
A way to get a std::vector of available frame ids.
Definition: tf.cpp:446
void transformQuaternion(const std::string &target_frame, const Stamped< tf::Quaternion > &stamped_in, Stamped< tf::Quaternion > &stamped_out) const
Transform a Stamped Quaternion into the target frame This can throw anything a lookupTransform can th...
Definition: tf.cpp:452
bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
Definition: cache.cpp:192
std::string strip_leading_slash(const std::string &frame_name)
Definition: tf.cpp:194
ros::Time stamp_
The timestamp associated with this data.
CompactFrameID gather(TimeCache *cache, ros::Time time, std::string *error_string)
Definition: tf.cpp:56
int getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time &time, std::string *error_string) const
Return the latest rostime which is common across the spanning set zero if fails to cross...
Definition: tf.cpp:416
Transformer(bool interpolating=true, ros::Duration cache_time_=ros::Duration(DEFAULT_CACHE_TIME))
Definition: tf.cpp:208
std::string frame_id_
The frame_id associated this data.
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
Get the transform between two frames by frame ID.
Definition: tf.cpp:237
TransformAccum()
Definition: tf.cpp:74
void transformPoint(const std::string &target_frame, const Stamped< tf::Point > &stamped_in, Stamped< tf::Point > &stamped_out) const
Transform a Stamped Point into the target frame This can throw anything a lookupTransform can throw a...
Definition: tf.cpp:483
Storage for transforms and their parent.
Definition: time_cache.h:54
void clear()
Clear all data.
Definition: tf.cpp:222
tfScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: Quaternion.h:231
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
convert tf::StampedTransform to TransformStamped msg
void transformVector(const std::string &target_frame, const Stamped< tf::Vector3 > &stamped_in, Stamped< tf::Vector3 > &stamped_out) const
Transform a Stamped Vector3 into the target frame This can throw anything a lookupTransform can throw...
Definition: tf.cpp:465
void finalize(WalkEnding end, ros::Time _time)
Definition: tf.cpp:108
static void transformStampedMsgToTF(const geometry_msgs::TransformStamped &msg, StampedTransform &bt)
convert TransformStamped msg to tf::StampedTransform
void setExtrapolationLimit(const ros::Duration &distance)
Set the distance which tf is allow to extrapolate.
Definition: tf.cpp:396
boost::signals2::connection addTransformsChangedListener(boost::function< void(void)> callback)
Add a callback that happens when a new transform has arrived.
Definition: tf.cpp:572
uint32_t CompactFrameID
Definition: time_cache.h:50
The Stamped Transform datatype used by tf.
tf::Quaternion result_quat
Definition: tf.cpp:147
TransformStorage st
Definition: tf.cpp:140
#define ROS_DEBUG(...)


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Sep 25 2020 03:30:09