buffer_core.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2010, 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 "tf2/buffer_core.h"
33 #include "tf2/time_cache.h"
34 #include "tf2/exceptions.h"
35 #include "tf2_msgs/TF2Error.h"
36 
37 #include <assert.h>
38 #include <console_bridge/console.h>
40 #include <boost/foreach.hpp>
41 
42 namespace tf2
43 {
44 
45 // Tolerance for acceptable quaternion normalization
46 static double QUATERNION_NORMALIZATION_TOLERANCE = 10e-3;
47 
49 void transformMsgToTF2(const geometry_msgs::Transform& msg, tf2::Transform& tf2)
50 {tf2 = tf2::Transform(tf2::Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w), tf2::Vector3(msg.translation.x, msg.translation.y, msg.translation.z));}
51 
53 void transformTF2ToMsg(const tf2::Transform& tf2, geometry_msgs::Transform& msg)
54 {
55  msg.translation.x = tf2.getOrigin().x();
56  msg.translation.y = tf2.getOrigin().y();
57  msg.translation.z = tf2.getOrigin().z();
58  msg.rotation.x = tf2.getRotation().x();
59  msg.rotation.y = tf2.getRotation().y();
60  msg.rotation.z = tf2.getRotation().z();
61  msg.rotation.w = tf2.getRotation().w();
62 }
63 
65 void transformTF2ToMsg(const tf2::Transform& tf2, geometry_msgs::TransformStamped& msg, ros::Time stamp, const std::string& frame_id, const std::string& child_frame_id)
66 {
67  transformTF2ToMsg(tf2, msg.transform);
68  msg.header.stamp = stamp;
69  msg.header.frame_id = frame_id;
70  msg.child_frame_id = child_frame_id;
71 }
72 
73 void transformTF2ToMsg(const tf2::Quaternion& orient, const tf2::Vector3& pos, geometry_msgs::Transform& msg)
74 {
75  msg.translation.x = pos.x();
76  msg.translation.y = pos.y();
77  msg.translation.z = pos.z();
78  msg.rotation.x = orient.x();
79  msg.rotation.y = orient.y();
80  msg.rotation.z = orient.z();
81  msg.rotation.w = orient.w();
82 }
83 
84 void transformTF2ToMsg(const tf2::Quaternion& orient, const tf2::Vector3& pos, geometry_msgs::TransformStamped& msg, ros::Time stamp, const std::string& frame_id, const std::string& child_frame_id)
85 {
86  transformTF2ToMsg(orient, pos, msg.transform);
87  msg.header.stamp = stamp;
88  msg.header.frame_id = frame_id;
89  msg.child_frame_id = child_frame_id;
90 }
91 
92 void setIdentity(geometry_msgs::Transform& tx)
93 {
94  tx.translation.x = 0;
95  tx.translation.y = 0;
96  tx.translation.z = 0;
97  tx.rotation.x = 0;
98  tx.rotation.y = 0;
99  tx.rotation.z = 0;
100  tx.rotation.w = 1;
101 }
102 
103 bool startsWithSlash(const std::string& frame_id)
104 {
105  if (frame_id.size() > 0)
106  if (frame_id[0] == '/')
107  return true;
108  return false;
109 }
110 
111 std::string stripSlash(const std::string& in)
112 {
113  std::string out = in;
114  if (startsWithSlash(in))
115  out.erase(0,1);
116  return out;
117 }
118 
119 
120 bool BufferCore::warnFrameId(const char* function_name_arg, const std::string& frame_id) const
121 {
122  if (frame_id.size() == 0)
123  {
124  std::stringstream ss;
125  ss << "Invalid argument passed to "<< function_name_arg <<" in tf2 frame_ids cannot be empty";
126  CONSOLE_BRIDGE_logWarn("%s",ss.str().c_str());
127  return true;
128  }
129 
130  if (startsWithSlash(frame_id))
131  {
132  std::stringstream ss;
133  ss << "Invalid argument \"" << frame_id << "\" passed to "<< function_name_arg <<" in tf2 frame_ids cannot start with a '/' like: ";
134  CONSOLE_BRIDGE_logWarn("%s",ss.str().c_str());
135  return true;
136  }
137 
138  return false;
139 }
140 
141 CompactFrameID BufferCore::validateFrameId(const char* function_name_arg, const std::string& frame_id) const
142 {
143  if (frame_id.empty())
144  {
145  std::stringstream ss;
146  ss << "Invalid argument passed to "<< function_name_arg <<" in tf2 frame_ids cannot be empty";
147  throw tf2::InvalidArgumentException(ss.str().c_str());
148  }
149 
150  if (startsWithSlash(frame_id))
151  {
152  std::stringstream ss;
153  ss << "Invalid argument \"" << frame_id << "\" passed to "<< function_name_arg <<" in tf2 frame_ids cannot start with a '/' like: ";
154  throw tf2::InvalidArgumentException(ss.str().c_str());
155  }
156 
157  CompactFrameID id = lookupFrameNumber(frame_id);
158  if (id == 0)
159  {
160  std::stringstream ss;
161  ss << "\"" << frame_id << "\" passed to "<< function_name_arg <<" does not exist. ";
162  throw tf2::LookupException(ss.str().c_str());
163  }
164 
165  return id;
166 }
167 
169 : cache_time_(cache_time)
170 , transformable_callbacks_counter_(0)
171 , transformable_requests_counter_(0)
172 , using_dedicated_thread_(false)
173 {
174  frameIDs_["NO_PARENT"] = 0;
175  frames_.push_back(TimeCacheInterfacePtr());
176  frameIDs_reverse.push_back("NO_PARENT");
177 }
178 
180 {
181 
182 }
183 
185 {
186  //old_tf_.clear();
187 
188 
189  boost::mutex::scoped_lock lock(frame_mutex_);
190  if ( frames_.size() > 1 )
191  {
192  for (std::vector<TimeCacheInterfacePtr>::iterator cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it)
193  {
194  if (*cache_it)
195  (*cache_it)->clearList();
196  }
197  }
198 
199 }
200 
201 bool BufferCore::setTransform(const geometry_msgs::TransformStamped& transform_in, const std::string& authority, bool is_static)
202 {
203 
205  /* tf::StampedTransform tf_transform;
206  tf::transformStampedMsgToTF(transform_in, tf_transform);
207  if (!old_tf_.setTransform(tf_transform, authority))
208  {
209  printf("Warning old setTransform Failed but was not caught\n");
210  }*/
211 
213  geometry_msgs::TransformStamped stripped = transform_in;
214  stripped.header.frame_id = stripSlash(stripped.header.frame_id);
215  stripped.child_frame_id = stripSlash(stripped.child_frame_id);
216 
217 
218  bool error_exists = false;
219  if (stripped.child_frame_id == stripped.header.frame_id)
220  {
221  CONSOLE_BRIDGE_logError("TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with frame_id and child_frame_id \"%s\" because they are the same", authority.c_str(), stripped.child_frame_id.c_str());
222  error_exists = true;
223  }
224 
225  if (stripped.child_frame_id == "")
226  {
227  CONSOLE_BRIDGE_logError("TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str());
228  error_exists = true;
229  }
230 
231  if (stripped.header.frame_id == "")
232  {
233  CONSOLE_BRIDGE_logError("TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"%s\" from authority \"%s\" because frame_id not set", stripped.child_frame_id.c_str(), authority.c_str());
234  error_exists = true;
235  }
236 
237  if (std::isnan(stripped.transform.translation.x) || std::isnan(stripped.transform.translation.y) || std::isnan(stripped.transform.translation.z)||
238  std::isnan(stripped.transform.rotation.x) || std::isnan(stripped.transform.rotation.y) || std::isnan(stripped.transform.rotation.z) || std::isnan(stripped.transform.rotation.w))
239  {
240  CONSOLE_BRIDGE_logError("TF_NAN_INPUT: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)",
241  stripped.child_frame_id.c_str(), authority.c_str(),
242  stripped.transform.translation.x, stripped.transform.translation.y, stripped.transform.translation.z,
243  stripped.transform.rotation.x, stripped.transform.rotation.y, stripped.transform.rotation.z, stripped.transform.rotation.w
244  );
245  error_exists = true;
246  }
247 
248  bool valid = std::abs((stripped.transform.rotation.w * stripped.transform.rotation.w
249  + stripped.transform.rotation.x * stripped.transform.rotation.x
250  + stripped.transform.rotation.y * stripped.transform.rotation.y
251  + stripped.transform.rotation.z * stripped.transform.rotation.z) - 1.0f) < QUATERNION_NORMALIZATION_TOLERANCE;
252 
253  if (!valid)
254  {
255  CONSOLE_BRIDGE_logError("TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of an invalid quaternion in the transform (%f %f %f %f)",
256  stripped.child_frame_id.c_str(), authority.c_str(),
257  stripped.transform.rotation.x, stripped.transform.rotation.y, stripped.transform.rotation.z, stripped.transform.rotation.w);
258  error_exists = true;
259  }
260 
261  if (error_exists)
262  return false;
263 
264  {
265  boost::mutex::scoped_lock lock(frame_mutex_);
266  CompactFrameID frame_number = lookupOrInsertFrameNumber(stripped.child_frame_id);
267  TimeCacheInterfacePtr frame = getFrame(frame_number);
268  if (frame == NULL)
269  frame = allocateFrame(frame_number, is_static);
270 
271  std::string error_string;
272  if (frame->insertData(TransformStorage(stripped, lookupOrInsertFrameNumber(stripped.header.frame_id), frame_number), &error_string))
273  {
274  frame_authority_[frame_number] = authority;
275  }
276  else
277  {
278  CONSOLE_BRIDGE_logWarn((error_string+" for frame %s at time %lf according to authority %s").c_str(), stripped.child_frame_id.c_str(), stripped.header.stamp.toSec(), authority.c_str());
279  return false;
280  }
281  }
282 
284 
285  return true;
286 }
287 
289 {
290  TimeCacheInterfacePtr frame_ptr = frames_[cfid];
291  if (is_static) {
293  } else {
295  }
296 
297  return frames_[cfid];
298 }
299 
301 {
306 };
307 
308 // TODO for Jade: Merge walkToTopParent functions; this is now a stub to preserve ABI
309 template<typename F>
310 int BufferCore::walkToTopParent(F& f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const
311 {
312  return walkToTopParent(f, time, target_id, source_id, error_string, NULL);
313 }
314 
315 template<typename F>
317  CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID>
318  *frame_chain) const
319 {
320  if (frame_chain)
321  frame_chain->clear();
322 
323  // Short circuit if zero length transform to allow lookups on non existant links
324  if (source_id == target_id)
325  {
326  f.finalize(Identity, time);
327  return tf2_msgs::TF2Error::NO_ERROR;
328  }
329 
330  //If getting the latest get the latest common time
331  if (time == ros::Time())
332  {
333  int retval = getLatestCommonTime(target_id, source_id, time, error_string);
334  if (retval != tf2_msgs::TF2Error::NO_ERROR)
335  {
336  return retval;
337  }
338  }
339 
340  // Walk the tree to its root from the source frame, accumulating the transform
341  CompactFrameID frame = source_id;
342  CompactFrameID top_parent = frame;
343  uint32_t depth = 0;
344 
345  std::string extrapolation_error_string;
346  bool extrapolation_might_have_occurred = false;
347 
348  while (frame != 0)
349  {
350  TimeCacheInterfacePtr cache = getFrame(frame);
351  if (frame_chain)
352  frame_chain->push_back(frame);
353 
354  if (!cache)
355  {
356  // There will be no cache for the very root of the tree
357  top_parent = frame;
358  break;
359  }
360 
361  CompactFrameID parent = f.gather(cache, time, error_string ? &extrapolation_error_string : NULL);
362  if (parent == 0)
363  {
364  // Just break out here... there may still be a path from source -> target
365  top_parent = frame;
366  extrapolation_might_have_occurred = true;
367  break;
368  }
369 
370  // Early out... target frame is a direct parent of the source frame
371  if (frame == target_id)
372  {
373  f.finalize(TargetParentOfSource, time);
374  return tf2_msgs::TF2Error::NO_ERROR;
375  }
376 
377  f.accum(true);
378 
379  top_parent = frame;
380  frame = parent;
381 
382  ++depth;
383  if (depth > MAX_GRAPH_DEPTH)
384  {
385  if (error_string)
386  {
387  std::stringstream ss;
388  ss << "The tf tree is invalid because it contains a loop." << std::endl
389  << allFramesAsStringNoLock() << std::endl;
390  *error_string = ss.str();
391  }
392  return tf2_msgs::TF2Error::LOOKUP_ERROR;
393  }
394  }
395 
396  // Now walk to the top parent from the target frame, accumulating its transform
397  frame = target_id;
398  depth = 0;
399  std::vector<CompactFrameID> reverse_frame_chain;
400 
401  while (frame != top_parent)
402  {
403  TimeCacheInterfacePtr cache = getFrame(frame);
404  if (frame_chain)
405  reverse_frame_chain.push_back(frame);
406 
407  if (!cache)
408  {
409  break;
410  }
411 
412  CompactFrameID parent = f.gather(cache, time, error_string);
413  if (parent == 0)
414  {
415  if (error_string)
416  {
417  // optimize performance by not using stringstream
418  char str[1000];
419  snprintf(str, sizeof(str), "%s, when looking up transform from frame [%s] to frame [%s]", error_string->c_str(), lookupFrameString(source_id).c_str(), lookupFrameString(target_id).c_str());
420  *error_string = str;
421  }
422 
423  return tf2_msgs::TF2Error::EXTRAPOLATION_ERROR;
424  }
425 
426  // Early out... source frame is a direct parent of the target frame
427  if (frame == source_id)
428  {
429  f.finalize(SourceParentOfTarget, time);
430  if (frame_chain)
431  {
432  // Use the walk we just did
433  frame_chain->swap(reverse_frame_chain);
434  // Reverse it before returning because this is the reverse walk.
435  std::reverse(frame_chain->begin(), frame_chain->end());
436  }
437  return tf2_msgs::TF2Error::NO_ERROR;
438  }
439 
440  f.accum(false);
441 
442  frame = parent;
443 
444  ++depth;
445  if (depth > MAX_GRAPH_DEPTH)
446  {
447  if (error_string)
448  {
449  std::stringstream ss;
450  ss << "The tf tree is invalid because it contains a loop." << std::endl
451  << allFramesAsStringNoLock() << std::endl;
452  *error_string = ss.str();
453  }
454  return tf2_msgs::TF2Error::LOOKUP_ERROR;
455  }
456  }
457 
458  if (frame != top_parent)
459  {
460  if (extrapolation_might_have_occurred)
461  {
462  if (error_string)
463  {
464  // optimize performance by not using stringstream
465  char str[1000];
466  snprintf(str, sizeof(str), "%s, when looking up transform from frame [%s] to frame [%s]", extrapolation_error_string.c_str(), lookupFrameString(source_id).c_str(), lookupFrameString(target_id).c_str());
467  *error_string = str;
468  }
469 
470  return tf2_msgs::TF2Error::EXTRAPOLATION_ERROR;
471 
472  }
473 
474  createConnectivityErrorString(source_id, target_id, error_string);
475  return tf2_msgs::TF2Error::CONNECTIVITY_ERROR;
476  }
477  else if (frame_chain){
478  // append top_parent to reverse_frame_chain for easier matching/trimming
479  reverse_frame_chain.push_back(frame);
480  }
481 
482  f.finalize(FullPath, time);
483  if (frame_chain)
484  {
485  // Pruning: Compare the chains starting at the parent (end) until they differ
486  int m = reverse_frame_chain.size()-1;
487  int n = frame_chain->size()-1;
488  for (; m >= 0 && n >= 0; --m, --n)
489  {
490  if ((*frame_chain)[n] != reverse_frame_chain[m])
491  {
492  break;
493  }
494  }
495  // Erase all duplicate items from frame_chain
496  if (n > 0)
497  {
498  // N is offset by 1 and leave the common parent for this result
499  frame_chain->erase(frame_chain->begin() + (n + 2), frame_chain->end());
500  }
501  if (m < reverse_frame_chain.size())
502  {
503  for (int i = m; i >= 0; --i)
504  {
505  frame_chain->push_back(reverse_frame_chain[i]);
506  }
507  }
508  }
509 
510  return tf2_msgs::TF2Error::NO_ERROR;
511 }
512 
513 
514 
516 {
518  : source_to_top_quat(0.0, 0.0, 0.0, 1.0)
519  , source_to_top_vec(0.0, 0.0, 0.0)
520  , target_to_top_quat(0.0, 0.0, 0.0, 1.0)
521  , target_to_top_vec(0.0, 0.0, 0.0)
522  , result_quat(0.0, 0.0, 0.0, 1.0)
523  , result_vec(0.0, 0.0, 0.0)
524  {
525  }
526 
527  CompactFrameID gather(TimeCacheInterfacePtr cache, ros::Time time, std::string* error_string)
528  {
529  if (!cache->getData(time, st, error_string))
530  {
531  return 0;
532  }
533 
534  return st.frame_id_;
535  }
536 
537  void accum(bool source)
538  {
539  if (source)
540  {
543  }
544  else
545  {
548  }
549  }
550 
551  void finalize(WalkEnding end, ros::Time _time)
552  {
553  switch (end)
554  {
555  case Identity:
556  break;
560  break;
562  {
563  tf2::Quaternion inv_target_quat = target_to_top_quat.inverse();
564  tf2::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
565  result_vec = inv_target_vec;
566  result_quat = inv_target_quat;
567  break;
568  }
569  case FullPath:
570  {
571  tf2::Quaternion inv_target_quat = target_to_top_quat.inverse();
572  tf2::Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
573 
574  result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
575  result_quat = inv_target_quat * source_to_top_quat;
576  }
577  break;
578  };
579 
580  time = _time;
581  }
582 
586  tf2::Vector3 source_to_top_vec;
588  tf2::Vector3 target_to_top_vec;
589 
591  tf2::Vector3 result_vec;
592 };
593 
594 geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame,
595  const std::string& source_frame,
596  const ros::Time& time) const
597 {
598  boost::mutex::scoped_lock lock(frame_mutex_);
599 
600  if (target_frame == source_frame) {
601  geometry_msgs::TransformStamped identity;
602  identity.header.frame_id = target_frame;
603  identity.child_frame_id = source_frame;
604  identity.transform.rotation.w = 1;
605 
606  if (time == ros::Time())
607  {
608  CompactFrameID target_id = lookupFrameNumber(target_frame);
609  TimeCacheInterfacePtr cache = getFrame(target_id);
610  if (cache)
611  identity.header.stamp = cache->getLatestTimestamp();
612  else
613  identity.header.stamp = time;
614  }
615  else
616  identity.header.stamp = time;
617 
618  return identity;
619  }
620 
621  //Identify case does not need to be validated above
622  CompactFrameID target_id = validateFrameId("lookupTransform argument target_frame", target_frame);
623  CompactFrameID source_id = validateFrameId("lookupTransform argument source_frame", source_frame);
624 
625  std::string error_string;
626  TransformAccum accum;
627  int retval = walkToTopParent(accum, time, target_id, source_id, &error_string);
628  if (retval != tf2_msgs::TF2Error::NO_ERROR)
629  {
630  switch (retval)
631  {
632  case tf2_msgs::TF2Error::CONNECTIVITY_ERROR:
633  throw ConnectivityException(error_string);
634  case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR:
635  throw ExtrapolationException(error_string);
636  case tf2_msgs::TF2Error::LOOKUP_ERROR:
637  throw LookupException(error_string);
638  default:
639  CONSOLE_BRIDGE_logError("Unknown error code: %d", retval);
640  assert(0);
641  }
642  }
643 
644  geometry_msgs::TransformStamped output_transform;
645  transformTF2ToMsg(accum.result_quat, accum.result_vec, output_transform, accum.time, target_frame, source_frame);
646  return output_transform;
647 }
648 
649 
650 geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame,
651  const ros::Time& target_time,
652  const std::string& source_frame,
653  const ros::Time& source_time,
654  const std::string& fixed_frame) const
655 {
656  validateFrameId("lookupTransform argument target_frame", target_frame);
657  validateFrameId("lookupTransform argument source_frame", source_frame);
658  validateFrameId("lookupTransform argument fixed_frame", fixed_frame);
659 
660  geometry_msgs::TransformStamped output;
661  geometry_msgs::TransformStamped temp1 = lookupTransform(fixed_frame, source_frame, source_time);
662  geometry_msgs::TransformStamped temp2 = lookupTransform(target_frame, fixed_frame, target_time);
663 
664  tf2::Transform tf1, tf2;
665  transformMsgToTF2(temp1.transform, tf1);
666  transformMsgToTF2(temp2.transform, tf2);
667  transformTF2ToMsg(tf2*tf1, output.transform);
668  output.header.stamp = temp2.header.stamp;
669  output.header.frame_id = target_frame;
670  output.child_frame_id = source_frame;
671  return output;
672 }
673 
674 
675 
676 /*
677 geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
678  const std::string& observation_frame,
679  const ros::Time& time,
680  const ros::Duration& averaging_interval) const
681 {
682  try
683  {
684  geometry_msgs::Twist t;
685  old_tf_.lookupTwist(tracking_frame, observation_frame,
686  time, averaging_interval, t);
687  return t;
688  }
689  catch (tf::LookupException& ex)
690  {
691  throw tf2::LookupException(ex.what());
692  }
693  catch (tf::ConnectivityException& ex)
694  {
695  throw tf2::ConnectivityException(ex.what());
696  }
697  catch (tf::ExtrapolationException& ex)
698  {
699  throw tf2::ExtrapolationException(ex.what());
700  }
701  catch (tf::InvalidArgument& ex)
702  {
703  throw tf2::InvalidArgumentException(ex.what());
704  }
705 }
706 
707 geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
708  const std::string& observation_frame,
709  const std::string& reference_frame,
710  const tf2::Point & reference_point,
711  const std::string& reference_point_frame,
712  const ros::Time& time,
713  const ros::Duration& averaging_interval) const
714 {
715  try{
716  geometry_msgs::Twist t;
717  old_tf_.lookupTwist(tracking_frame, observation_frame, reference_frame, reference_point, reference_point_frame,
718  time, averaging_interval, t);
719  return t;
720  }
721  catch (tf::LookupException& ex)
722  {
723  throw tf2::LookupException(ex.what());
724  }
725  catch (tf::ConnectivityException& ex)
726  {
727  throw tf2::ConnectivityException(ex.what());
728  }
729  catch (tf::ExtrapolationException& ex)
730  {
731  throw tf2::ExtrapolationException(ex.what());
732  }
733  catch (tf::InvalidArgument& ex)
734  {
735  throw tf2::InvalidArgumentException(ex.what());
736  }
737 }
738 */
739 
741 {
742  CompactFrameID gather(TimeCacheInterfacePtr cache, ros::Time time, std::string* error_string)
743  {
744  return cache->getParent(time, error_string);
745  }
746 
747  void accum(bool source)
748  {
749  }
750 
751  void finalize(WalkEnding end, ros::Time _time)
752  {
753  }
754 
756 };
757 
759  const ros::Time& time, std::string* error_msg) const
760 {
761  if (target_id == 0 || source_id == 0)
762  {
763  if (error_msg)
764  {
765  if (target_id == 0)
766  {
767  *error_msg += std::string("target_frame: " + lookupFrameString(target_id ) + " does not exist.");
768  }
769  if (source_id == 0)
770  {
771  if (target_id == 0)
772  {
773  *error_msg += std::string(" ");
774  }
775  *error_msg += std::string("source_frame: " + lookupFrameString(source_id) + " " + lookupFrameString(source_id ) + " does not exist.");
776  }
777  }
778  return false;
779  }
780 
781  if (target_id == source_id)
782  {
783  return true;
784  }
785 
786  CanTransformAccum accum;
787  if (walkToTopParent(accum, time, target_id, source_id, error_msg) == tf2_msgs::TF2Error::NO_ERROR)
788  {
789  return true;
790  }
791 
792  return false;
793 }
794 
796  const ros::Time& time, std::string* error_msg) const
797 {
798  boost::mutex::scoped_lock lock(frame_mutex_);
799  return canTransformNoLock(target_id, source_id, time, error_msg);
800 }
801 
802 bool BufferCore::canTransform(const std::string& target_frame, const std::string& source_frame,
803  const ros::Time& time, std::string* error_msg) const
804 {
805  // Short circuit if target_frame == source_frame
806  if (target_frame == source_frame)
807  return true;
808 
809  if (warnFrameId("canTransform argument target_frame", target_frame))
810  return false;
811  if (warnFrameId("canTransform argument source_frame", source_frame))
812  return false;
813 
814  boost::mutex::scoped_lock lock(frame_mutex_);
815 
816  CompactFrameID target_id = lookupFrameNumber(target_frame);
817  CompactFrameID source_id = lookupFrameNumber(source_frame);
818 
819  if (target_id == 0 || source_id == 0)
820  {
821  if (error_msg)
822  {
823  if (target_id == 0)
824  {
825  *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist.");
826  }
827  if (source_id == 0)
828  {
829  if (target_id == 0)
830  {
831  *error_msg += std::string(" ");
832  }
833  *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist.");
834  }
835  }
836  return false;
837  }
838  return canTransformNoLock(target_id, source_id, time, error_msg);
839 }
840 
841 bool BufferCore::canTransform(const std::string& target_frame, const ros::Time& target_time,
842  const std::string& source_frame, const ros::Time& source_time,
843  const std::string& fixed_frame, std::string* error_msg) const
844 {
845  if (warnFrameId("canTransform argument target_frame", target_frame))
846  return false;
847  if (warnFrameId("canTransform argument source_frame", source_frame))
848  return false;
849  if (warnFrameId("canTransform argument fixed_frame", fixed_frame))
850  return false;
851 
852  boost::mutex::scoped_lock lock(frame_mutex_);
853  CompactFrameID target_id = lookupFrameNumber(target_frame);
854  CompactFrameID source_id = lookupFrameNumber(source_frame);
855  CompactFrameID fixed_id = lookupFrameNumber(fixed_frame);
856 
857  if (target_id == 0 || source_id == 0 || fixed_id == 0)
858  {
859  if (error_msg)
860  {
861  if (target_id == 0)
862  {
863  *error_msg += std::string("canTransform: target_frame " + target_frame + " does not exist.");
864  }
865  if (source_id == 0)
866  {
867  if (target_id == 0)
868  {
869  *error_msg += std::string(" ");
870  }
871  *error_msg += std::string("canTransform: source_frame " + source_frame + " does not exist.");
872  }
873  if (source_id == 0)
874  {
875  if (target_id == 0 || source_id == 0)
876  {
877  *error_msg += std::string(" ");
878  }
879  *error_msg += std::string("fixed_frame: " + fixed_frame + "does not exist.");
880  }
881  }
882  return false;
883  }
884  return canTransformNoLock(target_id, fixed_id, target_time, error_msg) && canTransformNoLock(fixed_id, source_id, source_time, error_msg);
885 }
886 
887 
889 {
890  if (frame_id >= frames_.size())
891  return TimeCacheInterfacePtr();
892  else
893  {
894  return frames_[frame_id];
895  }
896 }
897 
898 CompactFrameID BufferCore::lookupFrameNumber(const std::string& frameid_str) const
899 {
900  CompactFrameID retval;
901  M_StringToCompactFrameID::const_iterator map_it = frameIDs_.find(frameid_str);
902  if (map_it == frameIDs_.end())
903  {
904  retval = CompactFrameID(0);
905  }
906  else
907  retval = map_it->second;
908  return retval;
909 }
910 
912 {
913  CompactFrameID retval = 0;
914  M_StringToCompactFrameID::iterator map_it = frameIDs_.find(frameid_str);
915  if (map_it == frameIDs_.end())
916  {
917  retval = CompactFrameID(frames_.size());
918  frames_.push_back(TimeCacheInterfacePtr());//Just a place holder for iteration
919  frameIDs_[frameid_str] = retval;
920  frameIDs_reverse.push_back(frameid_str);
921  }
922  else
923  retval = frameIDs_[frameid_str];
924 
925  return retval;
926 }
927 
928 const std::string& BufferCore::lookupFrameString(CompactFrameID frame_id_num) const
929 {
930  if (frame_id_num >= frameIDs_reverse.size())
931  {
932  std::stringstream ss;
933  ss << "Reverse lookup of frame id " << frame_id_num << " failed!";
934  throw tf2::LookupException(ss.str());
935  }
936  else
937  return frameIDs_reverse[frame_id_num];
938 }
939 
940 void BufferCore::createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const
941 {
942  if (!out)
943  {
944  return;
945  }
946  *out = std::string("Could not find a connection between '"+lookupFrameString(target_frame)+"' and '"+
947  lookupFrameString(source_frame)+"' because they are not part of the same tree."+
948  "Tf has two or more unconnected trees.");
949 }
950 
951 std::string BufferCore::allFramesAsString() const
952 {
953  boost::mutex::scoped_lock lock(frame_mutex_);
954  return this->allFramesAsStringNoLock();
955 }
956 
958 {
959  std::stringstream mstream;
960 
961  TransformStorage temp;
962 
963  // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it)
964 
966  for (unsigned int counter = 1; counter < frames_.size(); counter ++)
967  {
968  TimeCacheInterfacePtr frame_ptr = getFrame(CompactFrameID(counter));
969  if (frame_ptr == NULL)
970  continue;
971  CompactFrameID frame_id_num;
972  if( frame_ptr->getData(ros::Time(), temp))
973  frame_id_num = temp.frame_id_;
974  else
975  {
976  frame_id_num = 0;
977  }
978  mstream << "Frame "<< frameIDs_reverse[counter] << " exists with parent " << frameIDs_reverse[frame_id_num] << "." <<std::endl;
979  }
980 
981  return mstream.str();
982 }
983 
985 {
987  : id(id)
988  {}
989 
990  bool operator()(const P_TimeAndFrameID& rhs) const
991  {
992  return rhs.second == id;
993  }
994 
996 };
997 
998 int BufferCore::getLatestCommonTime(CompactFrameID target_id, CompactFrameID source_id, ros::Time & time, std::string * error_string) const
999 {
1000  // Error if one of the frames don't exist.
1001  if (source_id == 0 || target_id == 0) return tf2_msgs::TF2Error::LOOKUP_ERROR;
1002 
1003  if (source_id == target_id)
1004  {
1005  TimeCacheInterfacePtr cache = getFrame(source_id);
1006  //Set time to latest timestamp of frameid in case of target and source frame id are the same
1007  if (cache)
1008  time = cache->getLatestTimestamp();
1009  else
1010  time = ros::Time();
1011  return tf2_msgs::TF2Error::NO_ERROR;
1012  }
1013 
1014  std::vector<P_TimeAndFrameID> lct_cache;
1015 
1016  // Walk the tree to its root from the source frame, accumulating the list of parent/time as well as the latest time
1017  // in the target is a direct parent
1018  CompactFrameID frame = source_id;
1019  P_TimeAndFrameID temp;
1020  uint32_t depth = 0;
1021  ros::Time common_time = ros::TIME_MAX;
1022  while (frame != 0)
1023  {
1024  TimeCacheInterfacePtr cache = getFrame(frame);
1025 
1026  if (!cache)
1027  {
1028  // There will be no cache for the very root of the tree
1029  break;
1030  }
1031 
1032  P_TimeAndFrameID latest = cache->getLatestTimeAndParent();
1033 
1034  if (latest.second == 0)
1035  {
1036  // Just break out here... there may still be a path from source -> target
1037  break;
1038  }
1039 
1040  if (!latest.first.isZero())
1041  {
1042  common_time = std::min(latest.first, common_time);
1043  }
1044 
1045  lct_cache.push_back(latest);
1046 
1047  frame = latest.second;
1048 
1049  // Early out... target frame is a direct parent of the source frame
1050  if (frame == target_id)
1051  {
1052  time = common_time;
1053  if (time == ros::TIME_MAX)
1054  {
1055  time = ros::Time();
1056  }
1057  return tf2_msgs::TF2Error::NO_ERROR;
1058  }
1059 
1060  ++depth;
1061  if (depth > MAX_GRAPH_DEPTH)
1062  {
1063  if (error_string)
1064  {
1065  std::stringstream ss;
1066  ss<<"The tf tree is invalid because it contains a loop." << std::endl
1067  << allFramesAsStringNoLock() << std::endl;
1068  *error_string = ss.str();
1069  }
1070  return tf2_msgs::TF2Error::LOOKUP_ERROR;
1071  }
1072  }
1073 
1074  // Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent
1075  frame = target_id;
1076  depth = 0;
1077  common_time = ros::TIME_MAX;
1078  CompactFrameID common_parent = 0;
1079  while (true)
1080  {
1081  TimeCacheInterfacePtr cache = getFrame(frame);
1082 
1083  if (!cache)
1084  {
1085  break;
1086  }
1087 
1088  P_TimeAndFrameID latest = cache->getLatestTimeAndParent();
1089 
1090  if (latest.second == 0)
1091  {
1092  break;
1093  }
1094 
1095  if (!latest.first.isZero())
1096  {
1097  common_time = std::min(latest.first, common_time);
1098  }
1099 
1100  std::vector<P_TimeAndFrameID>::iterator it = std::find_if(lct_cache.begin(), lct_cache.end(), TimeAndFrameIDFrameComparator(latest.second));
1101  if (it != lct_cache.end()) // found a common parent
1102  {
1103  common_parent = it->second;
1104  break;
1105  }
1106 
1107  frame = latest.second;
1108 
1109  // Early out... source frame is a direct parent of the target frame
1110  if (frame == source_id)
1111  {
1112  time = common_time;
1113  if (time == ros::TIME_MAX)
1114  {
1115  time = ros::Time();
1116  }
1117  return tf2_msgs::TF2Error::NO_ERROR;
1118  }
1119 
1120  ++depth;
1121  if (depth > MAX_GRAPH_DEPTH)
1122  {
1123  if (error_string)
1124  {
1125  std::stringstream ss;
1126  ss<<"The tf tree is invalid because it contains a loop." << std::endl
1127  << allFramesAsStringNoLock() << std::endl;
1128  *error_string = ss.str();
1129  }
1130  return tf2_msgs::TF2Error::LOOKUP_ERROR;
1131  }
1132  }
1133 
1134  if (common_parent == 0)
1135  {
1136  createConnectivityErrorString(source_id, target_id, error_string);
1137  return tf2_msgs::TF2Error::CONNECTIVITY_ERROR;
1138  }
1139 
1140  // Loop through the source -> root list until we hit the common parent
1141  {
1142  std::vector<P_TimeAndFrameID>::iterator it = lct_cache.begin();
1143  std::vector<P_TimeAndFrameID>::iterator end = lct_cache.end();
1144  for (; it != end; ++it)
1145  {
1146  if (!it->first.isZero())
1147  {
1148  common_time = std::min(common_time, it->first);
1149  }
1150 
1151  if (it->second == common_parent)
1152  {
1153  break;
1154  }
1155  }
1156  }
1157 
1158  if (common_time == ros::TIME_MAX)
1159  {
1160  common_time = ros::Time();
1161  }
1162 
1163  time = common_time;
1164  return tf2_msgs::TF2Error::NO_ERROR;
1165 }
1166 
1167 std::string BufferCore::allFramesAsYAML(double current_time) const
1168 {
1169  std::stringstream mstream;
1170  boost::mutex::scoped_lock lock(frame_mutex_);
1171 
1172  TransformStorage temp;
1173 
1174  if (frames_.size() ==1)
1175  mstream <<"{}";
1176 
1177  mstream.precision(3);
1178  mstream.setf(std::ios::fixed,std::ios::floatfield);
1179 
1180  // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it)
1181  for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame
1182  {
1183  CompactFrameID cfid = CompactFrameID(counter);
1184  CompactFrameID frame_id_num;
1185  TimeCacheInterfacePtr cache = getFrame(cfid);
1186  if (!cache)
1187  {
1188  continue;
1189  }
1190 
1191  if(!cache->getData(ros::Time(), temp))
1192  {
1193  continue;
1194  }
1195 
1196  frame_id_num = temp.frame_id_;
1197 
1198  std::string authority = "no recorded authority";
1199  std::map<CompactFrameID, std::string>::const_iterator it = frame_authority_.find(cfid);
1200  if (it != frame_authority_.end()) {
1201  authority = it->second;
1202  }
1203 
1204  double rate = cache->getListLength() / std::max((cache->getLatestTimestamp().toSec() -
1205  cache->getOldestTimestamp().toSec() ), 0.0001);
1206 
1207  mstream << std::fixed; //fixed point notation
1208  mstream.precision(3); //3 decimal places
1209  mstream << frameIDs_reverse[cfid] << ": " << std::endl;
1210  mstream << " parent: '" << frameIDs_reverse[frame_id_num] << "'" << std::endl;
1211  mstream << " broadcaster: '" << authority << "'" << std::endl;
1212  mstream << " rate: " << rate << std::endl;
1213  mstream << " most_recent_transform: " << (cache->getLatestTimestamp()).toSec() << std::endl;
1214  mstream << " oldest_transform: " << (cache->getOldestTimestamp()).toSec() << std::endl;
1215  if ( current_time > 0 ) {
1216  mstream << " transform_delay: " << current_time - cache->getLatestTimestamp().toSec() << std::endl;
1217  }
1218  mstream << " buffer_length: " << (cache->getLatestTimestamp() - cache->getOldestTimestamp()).toSec() << std::endl;
1219  }
1220 
1221  return mstream.str();
1222 }
1223 
1224 std::string BufferCore::allFramesAsYAML() const
1225 {
1226  return this->allFramesAsYAML(0.0);
1227 }
1228 
1230 {
1231  boost::mutex::scoped_lock lock(transformable_callbacks_mutex_);
1233  while (!transformable_callbacks_.insert(std::make_pair(handle, cb)).second)
1234  {
1236  }
1237 
1238  return handle;
1239 }
1240 
1242 {
1244  : handle_(handle)
1245  {}
1246 
1248  {
1249  return req.cb_handle == handle_;
1250  }
1251 
1253 };
1254 
1256 {
1257  {
1258  boost::mutex::scoped_lock lock(transformable_callbacks_mutex_);
1259  transformable_callbacks_.erase(handle);
1260  }
1261 
1262  {
1263  boost::mutex::scoped_lock lock(transformable_requests_mutex_);
1264  auto it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByCallback(handle));
1265  if (it != transformable_requests_.end())
1266  {
1268  }
1269  }
1270 }
1271 
1272 TransformableRequestHandle BufferCore::addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, ros::Time time)
1273 {
1274  // shortcut if target == source
1275  if (target_frame == source_frame)
1276  {
1277  return 0;
1278  }
1279 
1281  req.target_id = lookupFrameNumber(target_frame);
1282  req.source_id = lookupFrameNumber(source_frame);
1283 
1284  // First check if the request is already transformable. If it is, return immediately
1285  if (canTransformInternal(req.target_id, req.source_id, time, 0))
1286  {
1287  return 0;
1288  }
1289 
1290  // Might not be transformable at all, ever (if it's too far in the past)
1291  if (req.target_id && req.source_id)
1292  {
1293  ros::Time latest_time;
1294  // TODO: This is incorrect, but better than nothing. Really we want the latest time for
1295  // any of the frames
1296  getLatestCommonTime(req.target_id, req.source_id, latest_time, 0);
1297  if (!latest_time.isZero() && time + cache_time_ < latest_time)
1298  {
1299  return 0xffffffffffffffffULL;
1300  }
1301  }
1302 
1303  req.cb_handle = handle;
1304  req.time = time;
1306  if (req.request_handle == 0 || req.request_handle == 0xffffffffffffffffULL)
1307  {
1308  req.request_handle = 1;
1309  }
1310 
1311  if (req.target_id == 0)
1312  {
1313  req.target_string = target_frame;
1314  }
1315 
1316  if (req.source_id == 0)
1317  {
1318  req.source_string = source_frame;
1319  }
1320 
1321  boost::mutex::scoped_lock lock(transformable_requests_mutex_);
1322  transformable_requests_.push_back(req);
1323 
1324  return req.request_handle;
1325 }
1326 
1328 {
1330  : handle_(handle)
1331  {}
1332 
1334  {
1335  return req.request_handle == handle_;
1336  }
1337 
1339 };
1340 
1342 {
1343  boost::mutex::scoped_lock lock(transformable_requests_mutex_);
1344  auto it = std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByID(handle));
1345  if (it != transformable_requests_.end())
1346  {
1348  }
1349 }
1350 
1351 
1352 
1353 // backwards compability for tf methods
1354 boost::signals2::connection BufferCore::_addTransformsChangedListener(boost::function<void(void)> callback)
1355 {
1356  boost::mutex::scoped_lock lock(transformable_requests_mutex_);
1357  return _transforms_changed_.connect(callback);
1358 }
1359 
1360 void BufferCore::_removeTransformsChangedListener(boost::signals2::connection c)
1361 {
1362  boost::mutex::scoped_lock lock(transformable_requests_mutex_);
1363  c.disconnect();
1364 }
1365 
1366 
1367 bool BufferCore::_frameExists(const std::string& frame_id_str) const
1368 {
1369  boost::mutex::scoped_lock lock(frame_mutex_);
1370  return frameIDs_.count(frame_id_str);
1371 }
1372 
1373 bool BufferCore::_getParent(const std::string& frame_id, ros::Time time, std::string& parent) const
1374 {
1375 
1376  boost::mutex::scoped_lock lock(frame_mutex_);
1377  CompactFrameID frame_number = lookupFrameNumber(frame_id);
1378  TimeCacheInterfacePtr frame = getFrame(frame_number);
1379 
1380  if (! frame)
1381  return false;
1382 
1383  CompactFrameID parent_id = frame->getParent(time, NULL);
1384  if (parent_id == 0)
1385  return false;
1386 
1387  parent = lookupFrameString(parent_id);
1388  return true;
1389 };
1390 
1391 void BufferCore::_getFrameStrings(std::vector<std::string> & vec) const
1392 {
1393  vec.clear();
1394 
1395  boost::mutex::scoped_lock lock(frame_mutex_);
1396 
1397  TransformStorage temp;
1398 
1399  // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it)
1400  for (unsigned int counter = 1; counter < frameIDs_reverse.size(); counter ++)
1401  {
1402  vec.push_back(frameIDs_reverse[counter]);
1403  }
1404  return;
1405 }
1406 
1407 
1408 
1409 
1411 {
1412  boost::mutex::scoped_lock lock(transformable_requests_mutex_);
1413  V_TransformableRequest::iterator it = transformable_requests_.begin();
1414 
1415  typedef boost::tuple<TransformableCallback&, TransformableRequestHandle, std::string,
1416  std::string, ros::Time&, TransformableResult&> TransformableTuple;
1417  std::vector<TransformableTuple> transformables;
1418 
1419  for (; it != transformable_requests_.end();)
1420  {
1421  TransformableRequest& req = *it;
1422 
1423  // One or both of the frames may not have existed when the request was originally made.
1424  if (req.target_id == 0)
1425  {
1427  }
1428 
1429  if (req.source_id == 0)
1430  {
1432  }
1433 
1434  ros::Time latest_time;
1435  bool do_cb = false;
1437  // TODO: This is incorrect, but better than nothing. Really we want the latest time for
1438  // any of the frames
1439  getLatestCommonTime(req.target_id, req.source_id, latest_time, 0);
1440  if (!latest_time.isZero() && req.time + cache_time_ < latest_time)
1441  {
1442  do_cb = true;
1443  result = TransformFailure;
1444  }
1445  else if (canTransformInternal(req.target_id, req.source_id, req.time, 0))
1446  {
1447  do_cb = true;
1448  result = TransformAvailable;
1449  }
1450 
1451  if (do_cb)
1452  {
1453  {
1454  boost::mutex::scoped_lock lock2(transformable_callbacks_mutex_);
1455  M_TransformableCallback::iterator it = transformable_callbacks_.find(req.cb_handle);
1456  if (it != transformable_callbacks_.end())
1457  {
1458  transformables.push_back(boost::make_tuple(boost::ref(it->second),
1459  req.request_handle,
1462  boost::ref(req.time),
1463  boost::ref(result)));
1464  }
1465  }
1466 
1467  if (transformable_requests_.size() > 1)
1468  {
1470  }
1471 
1473  }
1474  else
1475  {
1476  ++it;
1477  }
1478  }
1479 
1480  // unlock before allowing possible user callbacks to avoid potential deadlock (#91)
1481  lock.unlock();
1482 
1483  BOOST_FOREACH (TransformableTuple tt, transformables)
1484  {
1485  tt.get<0>()(tt.get<1>(), tt.get<2>(), tt.get<3>(), tt.get<4>(), tt.get<5>());
1486  }
1487 
1488  // Backwards compatability callback for tf
1490 }
1491 
1492 
1493 std::string BufferCore::_allFramesAsDot(double current_time) const
1494 {
1495  std::stringstream mstream;
1496  mstream << "digraph G {" << std::endl;
1497  boost::mutex::scoped_lock lock(frame_mutex_);
1498 
1499  TransformStorage temp;
1500 
1501  if (frames_.size() == 1) {
1502  mstream <<"\"no tf data recieved\"";
1503  }
1504  mstream.precision(3);
1505  mstream.setf(std::ios::fixed,std::ios::floatfield);
1506 
1507  for (unsigned int counter = 1; counter < frames_.size(); counter ++) // one referenced for 0 is no frame
1508  {
1509  unsigned int frame_id_num;
1510  TimeCacheInterfacePtr counter_frame = getFrame(counter);
1511  if (!counter_frame) {
1512  continue;
1513  }
1514  if(!counter_frame->getData(ros::Time(), temp)) {
1515  continue;
1516  } else {
1517  frame_id_num = temp.frame_id_;
1518  }
1519  std::string authority = "no recorded authority";
1520  std::map<unsigned int, std::string>::const_iterator it = frame_authority_.find(counter);
1521  if (it != frame_authority_.end())
1522  authority = it->second;
1523 
1524  double rate = counter_frame->getListLength() / std::max((counter_frame->getLatestTimestamp().toSec() -
1525  counter_frame->getOldestTimestamp().toSec()), 0.0001);
1526 
1527  mstream << std::fixed; //fixed point notation
1528  mstream.precision(3); //3 decimal places
1529  mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> "
1530  << "\"" << frameIDs_reverse[counter] << "\"" << "[label=\""
1531  //<< "Time: " << current_time.toSec() << "\\n"
1532  << "Broadcaster: " << authority << "\\n"
1533  << "Average rate: " << rate << " Hz\\n"
1534  << "Most recent transform: " << (counter_frame->getLatestTimestamp()).toSec() <<" ";
1535  if (current_time > 0)
1536  mstream << "( "<< current_time - counter_frame->getLatestTimestamp().toSec() << " sec old)";
1537  mstream << "\\n"
1538  // << "(time: " << getFrame(counter)->getLatestTimestamp().toSec() << ")\\n"
1539  // << "Oldest transform: " << (current_time - getFrame(counter)->getOldestTimestamp()).toSec() << " sec old \\n"
1540  // << "(time: " << (getFrame(counter)->getOldestTimestamp()).toSec() << ")\\n"
1541  << "Buffer length: " << (counter_frame->getLatestTimestamp()-counter_frame->getOldestTimestamp()).toSec() << " sec\\n"
1542  <<"\"];" <<std::endl;
1543  }
1544 
1545  for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame
1546  {
1547  unsigned int frame_id_num;
1548  TimeCacheInterfacePtr counter_frame = getFrame(counter);
1549  if (!counter_frame) {
1550  if (current_time > 0) {
1551  mstream << "edge [style=invis];" <<std::endl;
1552  mstream << " subgraph cluster_legend { style=bold; color=black; label =\"view_frames Result\";\n"
1553  << "\"Recorded at time: " << current_time << "\"[ shape=plaintext ] ;\n "
1554  << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl;
1555  }
1556  continue;
1557  }
1558  if (counter_frame->getData(ros::Time(), temp)) {
1559  frame_id_num = temp.frame_id_;
1560  } else {
1561  frame_id_num = 0;
1562  }
1563 
1564  if(frameIDs_reverse[frame_id_num]=="NO_PARENT")
1565  {
1566  mstream << "edge [style=invis];" <<std::endl;
1567  mstream << " subgraph cluster_legend { style=bold; color=black; label =\"view_frames Result\";\n";
1568  if (current_time > 0)
1569  mstream << "\"Recorded at time: " << current_time << "\"[ shape=plaintext ] ;\n ";
1570  mstream << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl;
1571  }
1572  }
1573  mstream << "}";
1574  return mstream.str();
1575 }
1576 
1577 std::string BufferCore::_allFramesAsDot() const
1578 {
1579  return _allFramesAsDot(0.0);
1580 }
1581 
1582 void BufferCore::_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
1583 {
1584  std::string error_string;
1585 
1586  output.clear(); //empty vector
1587 
1588  std::stringstream mstream;
1589  boost::mutex::scoped_lock lock(frame_mutex_);
1590 
1591  TransformAccum accum;
1592 
1593  // Get source frame/time using getFrame
1594  CompactFrameID source_id = lookupFrameNumber(source_frame);
1595  CompactFrameID fixed_id = lookupFrameNumber(fixed_frame);
1596  CompactFrameID target_id = lookupFrameNumber(target_frame);
1597 
1598  std::vector<CompactFrameID> source_frame_chain;
1599  int retval = walkToTopParent(accum, source_time, fixed_id, source_id, &error_string, &source_frame_chain);
1600 
1601  if (retval != tf2_msgs::TF2Error::NO_ERROR)
1602  {
1603  switch (retval)
1604  {
1605  case tf2_msgs::TF2Error::CONNECTIVITY_ERROR:
1606  throw ConnectivityException(error_string);
1607  case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR:
1608  throw ExtrapolationException(error_string);
1609  case tf2_msgs::TF2Error::LOOKUP_ERROR:
1610  throw LookupException(error_string);
1611  default:
1612  CONSOLE_BRIDGE_logError("Unknown error code: %d", retval);
1613  assert(0);
1614  }
1615  }
1616 
1617  std::vector<CompactFrameID> target_frame_chain;
1618  retval = walkToTopParent(accum, target_time, target_id, fixed_id, &error_string, &target_frame_chain);
1619 
1620  if (retval != tf2_msgs::TF2Error::NO_ERROR)
1621  {
1622  switch (retval)
1623  {
1624  case tf2_msgs::TF2Error::CONNECTIVITY_ERROR:
1625  throw ConnectivityException(error_string);
1626  case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR:
1627  throw ExtrapolationException(error_string);
1628  case tf2_msgs::TF2Error::LOOKUP_ERROR:
1629  throw LookupException(error_string);
1630  default:
1631  CONSOLE_BRIDGE_logError("Unknown error code: %d", retval);
1632  assert(0);
1633  }
1634  }
1635  // If the two chains overlap clear the overlap
1636  if (source_frame_chain.size() > 0 && target_frame_chain.size() > 0 &&
1637  source_frame_chain.back() == target_frame_chain.front())
1638  {
1639  source_frame_chain.pop_back();
1640  }
1641  // Join the two walks
1642  for (unsigned int i = 0; i < target_frame_chain.size(); ++i)
1643  {
1644  source_frame_chain.push_back(target_frame_chain[i]);
1645  }
1646 
1647 
1648  // Write each element of source_frame_chain as string
1649  for (unsigned int i = 0; i < source_frame_chain.size(); ++i)
1650  {
1651  output.push_back(lookupFrameString(source_frame_chain[i]));
1652  }
1653 }
1654 
1655 int TestBufferCore::_walkToTopParent(BufferCore& buffer, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *frame_chain) const
1656 {
1657  TransformAccum accum;
1658  return buffer.walkToTopParent(accum, time, target_id, source_id, error_string, frame_chain);
1659 }
1660 
1661 } // namespace tf2
tf2::BufferCore::transformable_requests_counter_
uint64_t transformable_requests_counter_
Definition: buffer_core.h:356
tf2::TimeAndFrameIDFrameComparator::TimeAndFrameIDFrameComparator
TimeAndFrameIDFrameComparator(CompactFrameID id)
Definition: buffer_core.cpp:986
exceptions.h
tf2::TransformAccum::target_to_top_quat
tf2::Quaternion target_to_top_quat
Definition: buffer_core.cpp:587
tf2::BufferCore::getLatestCommonTime
int getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_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: buffer_core.cpp:998
tf2::transformTF2ToMsg
void transformTF2ToMsg(const tf2::Transform &tf2, geometry_msgs::Transform &msg)
convert Transform to Transform msg
Definition: buffer_core.cpp:53
tf2::BufferCore::lookupTransform
geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time) const
Get the transform between two frames by frame ID.
Definition: buffer_core.cpp:594
tf2::TimeAndFrameIDFrameComparator::id
CompactFrameID id
Definition: buffer_core.cpp:995
tf2::BufferCore::testTransformableRequests
void testTransformableRequests()
Definition: buffer_core.cpp:1410
tf2::BufferCore::_transforms_changed_
TransformsChangedSignal _transforms_changed_
Signal which is fired whenever new transform data has arrived, from the thread the data arrived in.
Definition: buffer_core.h:364
tf2::BufferCore::warnFrameId
bool warnFrameId(const char *function_name_arg, const std::string &frame_id) const
Definition: buffer_core.cpp:120
boost::shared_ptr
tf2::BufferCore::TransformableRequest::target_id
CompactFrameID target_id
Definition: buffer_core.h:348
tf2::TransformAccum::gather
CompactFrameID gather(TimeCacheInterfacePtr cache, ros::Time time, std::string *error_string)
Definition: buffer_core.cpp:527
tf2::BufferCore::TransformableCallback
boost::function< void(TransformableRequestHandle request_handle, const std::string &target_frame, const std::string &source_frame, ros::Time time, TransformableResult result)> TransformableCallback
Definition: buffer_core.h:227
tf2::TargetParentOfSource
@ TargetParentOfSource
Definition: buffer_core.cpp:303
tf2::TimeCacheInterfacePtr
boost::shared_ptr< TimeCacheInterface > TimeCacheInterfacePtr
Definition: buffer_core.h:61
tf2::TransformAccum::finalize
void finalize(WalkEnding end, ros::Time _time)
Definition: buffer_core.cpp:551
tf2::BufferCore::allocateFrame
TimeCacheInterfacePtr allocateFrame(CompactFrameID cfid, bool is_static)
Definition: buffer_core.cpp:288
tf2::BufferCore::TransformableRequest::request_handle
TransformableRequestHandle request_handle
Definition: buffer_core.h:346
tf2::BufferCore::addTransformableCallback
TransformableCallbackHandle addTransformableCallback(const TransformableCallback &cb)
Internal use only.
Definition: buffer_core.cpp:1229
buffer_core.h
tf2::BufferCore::TransformableRequest::cb_handle
TransformableCallbackHandle cb_handle
Definition: buffer_core.h:347
tf2::BufferCore::allFramesAsStringNoLock
std::string allFramesAsStringNoLock() const
A way to see what frames have been cached Useful for debugging. Use this call internally.
Definition: buffer_core.cpp:957
tf2::BufferCore::~BufferCore
virtual ~BufferCore(void)
Definition: buffer_core.cpp:179
tf2::BufferCore::setTransform
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
Definition: buffer_core.cpp:201
CONSOLE_BRIDGE_logError
#define CONSOLE_BRIDGE_logError(fmt,...)
tf2::BufferCore::getFrame
TimeCacheInterfacePtr getFrame(CompactFrameID c_frame_id) const
An accessor to get a frame, which will throw an exception if the frame is no there.
Definition: buffer_core.cpp:888
tf2::BufferCore::_addTransformsChangedListener
boost::signals2::connection _addTransformsChangedListener(boost::function< void(void)> callback)
Add a callback that happens when a new transform has arrived.
Definition: buffer_core.cpp:1354
tf2::BufferCore::MAX_GRAPH_DEPTH
static const uint32_t MAX_GRAPH_DEPTH
Maximum graph search depth (deeper graphs will be assumed to have loops)
Definition: buffer_core.h:93
Transform.h
tf2::BufferCore::transformable_requests_mutex_
boost::mutex transformable_requests_mutex_
Definition: buffer_core.h:355
tf2::BufferCore::allFramesAsYAML
std::string allFramesAsYAML() const
Definition: buffer_core.cpp:1224
tf2::BufferCore::transformable_callbacks_counter_
uint32_t transformable_callbacks_counter_
Definition: buffer_core.h:340
tf2::BufferCore::RemoveRequestByID
Definition: buffer_core.cpp:1327
tf2::BufferCore::_removeTransformsChangedListener
void _removeTransformsChangedListener(boost::signals2::connection c)
Definition: buffer_core.cpp:1360
tf2::CanTransformAccum::st
TransformStorage st
Definition: buffer_core.cpp:755
tf2::CanTransformAccum::finalize
void finalize(WalkEnding end, ros::Time _time)
Definition: buffer_core.cpp:751
tf2::BufferCore::RemoveRequestByCallback
Definition: buffer_core.cpp:1241
tf2::BufferCore::TransformableRequest::time
ros::Time time
Definition: buffer_core.h:345
tf2::BufferCore::walkToTopParent
int walkToTopParent(F &f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string *error_string) const
Definition: buffer_core.cpp:310
tf2::TimeAndFrameIDFrameComparator::operator()
bool operator()(const P_TimeAndFrameID &rhs) const
Definition: buffer_core.cpp:990
tf2::TransformAccum::time
ros::Time time
Definition: buffer_core.cpp:584
tf2::TimeCache
A class to keep a sorted linked list in time This builds and maintains a list of timestamped data....
Definition: time_cache.h:92
tf2::Identity
@ Identity
Definition: buffer_core.cpp:302
tf2::CompactFrameID
uint32_t CompactFrameID
Definition: transform_storage.h:50
tf2::TransformStorage::rotation_
tf2::Quaternion rotation_
Definition: transform_storage.h:76
tf2::WalkEnding
WalkEnding
Definition: buffer_core.cpp:300
tf2::BufferCore::RemoveRequestByCallback::handle_
TransformableCallbackHandle handle_
Definition: buffer_core.cpp:1252
tf2::BufferCore::RemoveRequestByID::RemoveRequestByID
RemoveRequestByID(TransformableRequestHandle handle)
Definition: buffer_core.cpp:1329
tf2::TransformAccum::accum
void accum(bool source)
Definition: buffer_core.cpp:537
tf2::BufferCore::RemoveRequestByCallback::operator()
bool operator()(const TransformableRequest &req)
Definition: buffer_core.cpp:1247
tf2::TransformStorage
Storage for transforms and their parent.
Definition: transform_storage.h:53
tf2::TransformFailure
@ TransformFailure
Definition: buffer_core.h:67
tf2::BufferCore::allFramesAsString
std::string allFramesAsString() const
A way to see what frames have been cached Useful for debugging.
Definition: buffer_core.cpp:951
tf2::BufferCore::canTransformNoLock
bool canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id, const ros::Time &time, std::string *error_msg) const
Definition: buffer_core.cpp:758
tf2::TransformAccum::st
TransformStorage st
Definition: buffer_core.cpp:583
tf2::BufferCore::frameIDs_reverse
std::vector< std::string > frameIDs_reverse
A map from CompactFrameID frame_id_numbers to string for debugging and output.
Definition: buffer_core.h:330
tf2::BufferCore::_chainAsVector
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
Backwards compatabilityA way to see what frames are in a chain Useful for debugging.
Definition: buffer_core.cpp:1582
tf2::startsWithSlash
bool startsWithSlash(const std::string &frame_id)
Definition: buffer_core.cpp:103
tf2::BufferCore::transformable_callbacks_
M_TransformableCallback transformable_callbacks_
Definition: buffer_core.h:339
tf2::BufferCore::_getFrameStrings
void _getFrameStrings(std::vector< std::string > &ids) const
A way to get a std::vector of available frame ids.
Definition: buffer_core.cpp:1391
TimeBase< Time, Duration >::isZero
bool isZero() const
tf2::BufferCore::canTransform
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: buffer_core.cpp:802
tf2::StaticCache
Definition: time_cache.h:136
tf2::ExtrapolationException
An exception class to notify that the requested value would have required extrapolation beyond curren...
Definition: exceptions.h:79
tf2::TransformableRequestHandle
uint64_t TransformableRequestHandle
Definition: buffer_core.h:59
tf2::BufferCore::clear
void clear()
Clear all data.
Definition: buffer_core.cpp:184
tf2::BufferCore::cache_time_
ros::Duration cache_time_
How long to cache transform history.
Definition: buffer_core.h:336
tf2::BufferCore::frame_mutex_
boost::mutex frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
Definition: buffer_core.h:324
tf2::Transform
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:32
tf2::SourceParentOfTarget
@ SourceParentOfTarget
Definition: buffer_core.cpp:304
tf2::BufferCore::frameIDs_
M_StringToCompactFrameID frameIDs_
Definition: buffer_core.h:328
tf2::BufferCore::_allFramesAsDot
std::string _allFramesAsDot() const
Definition: buffer_core.cpp:1577
time_cache.h
tf2::TransformStorage::translation_
tf2::Vector3 translation_
Definition: transform_storage.h:77
tf2::TransformAccum::TransformAccum
TransformAccum()
Definition: buffer_core.cpp:517
tf2::BufferCore::_frameExists
bool _frameExists(const std::string &frame_id_str) const
Check if a frame exists in the tree.
Definition: buffer_core.cpp:1367
tf2::TransformStorage::frame_id_
CompactFrameID frame_id_
Definition: transform_storage.h:79
tf2::TimeAndFrameIDFrameComparator
Definition: buffer_core.cpp:984
tf2::TransformAccum::target_to_top_vec
tf2::Vector3 target_to_top_vec
Definition: buffer_core.cpp:588
tf2::LookupException
An exception class to notify of bad frame number.
Definition: exceptions.h:70
tf2::TransformAccum::source_to_top_quat
tf2::Quaternion source_to_top_quat
Definition: buffer_core.cpp:585
tf2::FullPath
@ FullPath
Definition: buffer_core.cpp:305
tf2::BufferCore::cancelTransformableRequest
void cancelTransformableRequest(TransformableRequestHandle handle)
Internal use only.
Definition: buffer_core.cpp:1341
tf2::CanTransformAccum::accum
void accum(bool source)
Definition: buffer_core.cpp:747
tf2::CanTransformAccum::gather
CompactFrameID gather(TimeCacheInterfacePtr cache, ros::Time time, std::string *error_string)
Definition: buffer_core.cpp:742
tf2::ConnectivityException
An exception class to notify of no connection.
Definition: exceptions.h:55
tf2::BufferCore::lookupFrameString
const std::string & lookupFrameString(CompactFrameID frame_id_num) const
Number to string frame lookup may throw LookupException if number invalid.
Definition: buffer_core.cpp:928
tf2::BufferCore::validateFrameId
CompactFrameID validateFrameId(const char *function_name_arg, const std::string &frame_id) const
Definition: buffer_core.cpp:141
tf2::BufferCore::RemoveRequestByID::handle_
TransformableCallbackHandle handle_
Definition: buffer_core.cpp:1338
tf2::BufferCore::TransformableRequest
Definition: buffer_core.h:343
tf2::BufferCore::transformable_requests_
V_TransformableRequest transformable_requests_
Definition: buffer_core.h:354
tf2::BufferCore::frame_authority_
std::map< CompactFrameID, std::string > frame_authority_
A map to lookup the most recent authority for a given frame.
Definition: buffer_core.h:332
ros::TIME_MAX
const ROSTIME_DECL Time TIME_MAX
tf2::P_TimeAndFrameID
std::pair< ros::Time, CompactFrameID > P_TimeAndFrameID
Definition: buffer_core.h:57
tf2::BufferCore::frames_
V_TimeCacheInterface frames_
Definition: buffer_core.h:321
tf2::Quaternion::inverse
Quaternion inverse() const
Return the inverse of this quaternion.
Definition: Quaternion.h:259
ros::Time
tf2::TransformAccum::source_to_top_vec
tf2::Vector3 source_to_top_vec
Definition: buffer_core.cpp:586
tf2::BufferCore
A Class which provides coordinate transforms between any two frames in a system.
Definition: buffer_core.h:88
tf2::TestBufferCore::_walkToTopParent
int _walkToTopParent(BufferCore &buffer, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string *error_string, std::vector< CompactFrameID > *frame_chain) const
Definition: buffer_core.cpp:1655
tf2::TransformAccum
Definition: buffer_core.cpp:515
tf2::quatRotate
TF2SIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
Definition: Quaternion.h:438
tf2::BufferCore::createConnectivityErrorString
void createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string *out) const
Definition: buffer_core.cpp:940
tf2::BufferCore::addTransformableRequest
TransformableRequestHandle addTransformableRequest(TransformableCallbackHandle handle, const std::string &target_frame, const std::string &source_frame, ros::Time time)
Internal use only.
Definition: buffer_core.cpp:1272
tf2
Definition: buffer_core.h:54
tf2::BufferCore::lookupOrInsertFrameNumber
CompactFrameID lookupOrInsertFrameNumber(const std::string &frameid_str)
String to number for frame lookup with dynamic allocation of new frames.
Definition: buffer_core.cpp:911
tf2::TransformAccum::result_vec
tf2::Vector3 result_vec
Definition: buffer_core.cpp:591
tf2::QUATERNION_NORMALIZATION_TOLERANCE
static double QUATERNION_NORMALIZATION_TOLERANCE
Definition: buffer_core.cpp:46
tf2::BufferCore::TransformableRequest::source_string
std::string source_string
Definition: buffer_core.h:351
tf2::BufferCore::BufferCore
BufferCore(ros::Duration cache_time_=ros::Duration(DEFAULT_CACHE_TIME))
Definition: buffer_core.cpp:168
tf2::Quaternion
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30
tf2::BufferCore::TransformableRequest::source_id
CompactFrameID source_id
Definition: buffer_core.h:349
tf2::setIdentity
void setIdentity(geometry_msgs::Transform &tx)
Definition: buffer_core.cpp:92
tf2::transformMsgToTF2
void transformMsgToTF2(const geometry_msgs::Transform &msg, tf2::Transform &tf2)
convert Transform msg to Transform
Definition: buffer_core.cpp:49
tf2::TransformAccum::result_quat
tf2::Quaternion result_quat
Definition: buffer_core.cpp:590
tf2::BufferCore::canTransformInternal
bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id, const ros::Time &time, std::string *error_msg) const
Definition: buffer_core.cpp:795
tf2::BufferCore::RemoveRequestByID::operator()
bool operator()(const TransformableRequest &req)
Definition: buffer_core.cpp:1333
tf2::BufferCore::removeTransformableCallback
void removeTransformableCallback(TransformableCallbackHandle handle)
Internal use only.
Definition: buffer_core.cpp:1255
tf2::BufferCore::RemoveRequestByCallback::RemoveRequestByCallback
RemoveRequestByCallback(TransformableCallbackHandle handle)
Definition: buffer_core.cpp:1243
tf2::CanTransformAccum
Definition: buffer_core.cpp:740
tf2::BufferCore::transformable_callbacks_mutex_
boost::mutex transformable_callbacks_mutex_
Definition: buffer_core.h:341
tf2::InvalidArgumentException
An exception class to notify that one of the arguments is invalid.
Definition: exceptions.h:90
tf2::BufferCore::lookupFrameNumber
CompactFrameID lookupFrameNumber(const std::string &frameid_str) const
String to number for frame lookup with dynamic allocation of new frames.
Definition: buffer_core.cpp:898
ros::Duration
tf2::TransformableResult
TransformableResult
Definition: buffer_core.h:64
tf2::BufferCore::_getParent
bool _getParent(const std::string &frame_id, ros::Time time, std::string &parent) const
Fill the parent of a frame.
Definition: buffer_core.cpp:1373
tf2::TransformableCallbackHandle
uint32_t TransformableCallbackHandle
Definition: buffer_core.h:58
tf2::BufferCore::TransformableRequest::target_string
std::string target_string
Definition: buffer_core.h:350
tf2::TransformAvailable
@ TransformAvailable
Definition: buffer_core.h:66
tf2::stripSlash
std::string stripSlash(const std::string &in)
Definition: buffer_core.cpp:111


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Apr 28 2022 02:15:23