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


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Jun 7 2019 22:00:28