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  std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByCallback(handle));
1265  }
1266 }
1267 
1268 TransformableRequestHandle BufferCore::addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, ros::Time time)
1269 {
1270  // shortcut if target == source
1271  if (target_frame == source_frame)
1272  {
1273  return 0;
1274  }
1275 
1277  req.target_id = lookupFrameNumber(target_frame);
1278  req.source_id = lookupFrameNumber(source_frame);
1279 
1280  // First check if the request is already transformable. If it is, return immediately
1281  if (canTransformInternal(req.target_id, req.source_id, time, 0))
1282  {
1283  return 0;
1284  }
1285 
1286  // Might not be transformable at all, ever (if it's too far in the past)
1287  if (req.target_id && req.source_id)
1288  {
1289  ros::Time latest_time;
1290  // TODO: This is incorrect, but better than nothing. Really we want the latest time for
1291  // any of the frames
1292  getLatestCommonTime(req.target_id, req.source_id, latest_time, 0);
1293  if (!latest_time.isZero() && time + cache_time_ < latest_time)
1294  {
1295  return 0xffffffffffffffffULL;
1296  }
1297  }
1298 
1299  req.cb_handle = handle;
1300  req.time = time;
1302  if (req.request_handle == 0 || req.request_handle == 0xffffffffffffffffULL)
1303  {
1304  req.request_handle = 1;
1305  }
1306 
1307  if (req.target_id == 0)
1308  {
1309  req.target_string = target_frame;
1310  }
1311 
1312  if (req.source_id == 0)
1313  {
1314  req.source_string = source_frame;
1315  }
1316 
1317  boost::mutex::scoped_lock lock(transformable_requests_mutex_);
1318  transformable_requests_.push_back(req);
1319 
1320  return req.request_handle;
1321 }
1322 
1324 {
1326  : handle_(handle)
1327  {}
1328 
1330  {
1331  return req.request_handle == handle_;
1332  }
1333 
1335 };
1336 
1338 {
1339  boost::mutex::scoped_lock lock(transformable_requests_mutex_);
1340  std::remove_if(transformable_requests_.begin(), transformable_requests_.end(), RemoveRequestByID(handle));
1341 }
1342 
1343 
1344 
1345 // backwards compability for tf methods
1346 boost::signals2::connection BufferCore::_addTransformsChangedListener(boost::function<void(void)> callback)
1347 {
1348  boost::mutex::scoped_lock lock(transformable_requests_mutex_);
1349  return _transforms_changed_.connect(callback);
1350 }
1351 
1352 void BufferCore::_removeTransformsChangedListener(boost::signals2::connection c)
1353 {
1354  boost::mutex::scoped_lock lock(transformable_requests_mutex_);
1355  c.disconnect();
1356 }
1357 
1358 
1359 bool BufferCore::_frameExists(const std::string& frame_id_str) const
1360 {
1361  boost::mutex::scoped_lock lock(frame_mutex_);
1362  return frameIDs_.count(frame_id_str);
1363 }
1364 
1365 bool BufferCore::_getParent(const std::string& frame_id, ros::Time time, std::string& parent) const
1366 {
1367 
1368  boost::mutex::scoped_lock lock(frame_mutex_);
1369  CompactFrameID frame_number = lookupFrameNumber(frame_id);
1370  TimeCacheInterfacePtr frame = getFrame(frame_number);
1371 
1372  if (! frame)
1373  return false;
1374 
1375  CompactFrameID parent_id = frame->getParent(time, NULL);
1376  if (parent_id == 0)
1377  return false;
1378 
1379  parent = lookupFrameString(parent_id);
1380  return true;
1381 };
1382 
1383 void BufferCore::_getFrameStrings(std::vector<std::string> & vec) const
1384 {
1385  vec.clear();
1386 
1387  boost::mutex::scoped_lock lock(frame_mutex_);
1388 
1389  TransformStorage temp;
1390 
1391  // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it)
1392  for (unsigned int counter = 1; counter < frameIDs_reverse.size(); counter ++)
1393  {
1394  vec.push_back(frameIDs_reverse[counter]);
1395  }
1396  return;
1397 }
1398 
1399 
1400 
1401 
1403 {
1404  boost::mutex::scoped_lock lock(transformable_requests_mutex_);
1405  V_TransformableRequest::iterator it = transformable_requests_.begin();
1406 
1407  typedef boost::tuple<TransformableCallback&, TransformableRequestHandle, std::string,
1408  std::string, ros::Time&, TransformableResult&> TransformableTuple;
1409  std::vector<TransformableTuple> transformables;
1410 
1411  for (; it != transformable_requests_.end();)
1412  {
1413  TransformableRequest& req = *it;
1414 
1415  // One or both of the frames may not have existed when the request was originally made.
1416  if (req.target_id == 0)
1417  {
1419  }
1420 
1421  if (req.source_id == 0)
1422  {
1424  }
1425 
1426  ros::Time latest_time;
1427  bool do_cb = false;
1429  // TODO: This is incorrect, but better than nothing. Really we want the latest time for
1430  // any of the frames
1431  getLatestCommonTime(req.target_id, req.source_id, latest_time, 0);
1432  if (!latest_time.isZero() && req.time + cache_time_ < latest_time)
1433  {
1434  do_cb = true;
1435  result = TransformFailure;
1436  }
1437  else if (canTransformInternal(req.target_id, req.source_id, req.time, 0))
1438  {
1439  do_cb = true;
1440  result = TransformAvailable;
1441  }
1442 
1443  if (do_cb)
1444  {
1445  {
1446  boost::mutex::scoped_lock lock2(transformable_callbacks_mutex_);
1447  M_TransformableCallback::iterator it = transformable_callbacks_.find(req.cb_handle);
1448  if (it != transformable_callbacks_.end())
1449  {
1450  transformables.push_back(boost::make_tuple(boost::ref(it->second),
1451  req.request_handle,
1454  boost::ref(req.time),
1455  boost::ref(result)));
1456  }
1457  }
1458 
1459  if (transformable_requests_.size() > 1)
1460  {
1462  }
1463 
1465  }
1466  else
1467  {
1468  ++it;
1469  }
1470  }
1471 
1472  // unlock before allowing possible user callbacks to avoid potential deadlock (#91)
1473  lock.unlock();
1474 
1475  BOOST_FOREACH (TransformableTuple tt, transformables)
1476  {
1477  tt.get<0>()(tt.get<1>(), tt.get<2>(), tt.get<3>(), tt.get<4>(), tt.get<5>());
1478  }
1479 
1480  // Backwards compatability callback for tf
1482 }
1483 
1484 
1485 std::string BufferCore::_allFramesAsDot(double current_time) const
1486 {
1487  std::stringstream mstream;
1488  mstream << "digraph G {" << std::endl;
1489  boost::mutex::scoped_lock lock(frame_mutex_);
1490 
1491  TransformStorage temp;
1492 
1493  if (frames_.size() == 1) {
1494  mstream <<"\"no tf data recieved\"";
1495  }
1496  mstream.precision(3);
1497  mstream.setf(std::ios::fixed,std::ios::floatfield);
1498 
1499  for (unsigned int counter = 1; counter < frames_.size(); counter ++) // one referenced for 0 is no frame
1500  {
1501  unsigned int frame_id_num;
1502  TimeCacheInterfacePtr counter_frame = getFrame(counter);
1503  if (!counter_frame) {
1504  continue;
1505  }
1506  if(!counter_frame->getData(ros::Time(), temp)) {
1507  continue;
1508  } else {
1509  frame_id_num = temp.frame_id_;
1510  }
1511  std::string authority = "no recorded authority";
1512  std::map<unsigned int, std::string>::const_iterator it = frame_authority_.find(counter);
1513  if (it != frame_authority_.end())
1514  authority = it->second;
1515 
1516  double rate = counter_frame->getListLength() / std::max((counter_frame->getLatestTimestamp().toSec() -
1517  counter_frame->getOldestTimestamp().toSec()), 0.0001);
1518 
1519  mstream << std::fixed; //fixed point notation
1520  mstream.precision(3); //3 decimal places
1521  mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> "
1522  << "\"" << frameIDs_reverse[counter] << "\"" << "[label=\""
1523  //<< "Time: " << current_time.toSec() << "\\n"
1524  << "Broadcaster: " << authority << "\\n"
1525  << "Average rate: " << rate << " Hz\\n"
1526  << "Most recent transform: " << (counter_frame->getLatestTimestamp()).toSec() <<" ";
1527  if (current_time > 0)
1528  mstream << "( "<< current_time - counter_frame->getLatestTimestamp().toSec() << " sec old)";
1529  mstream << "\\n"
1530  // << "(time: " << getFrame(counter)->getLatestTimestamp().toSec() << ")\\n"
1531  // << "Oldest transform: " << (current_time - getFrame(counter)->getOldestTimestamp()).toSec() << " sec old \\n"
1532  // << "(time: " << (getFrame(counter)->getOldestTimestamp()).toSec() << ")\\n"
1533  << "Buffer length: " << (counter_frame->getLatestTimestamp()-counter_frame->getOldestTimestamp()).toSec() << " sec\\n"
1534  <<"\"];" <<std::endl;
1535  }
1536 
1537  for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame
1538  {
1539  unsigned int frame_id_num;
1540  TimeCacheInterfacePtr counter_frame = getFrame(counter);
1541  if (!counter_frame) {
1542  if (current_time > 0) {
1543  mstream << "edge [style=invis];" <<std::endl;
1544  mstream << " subgraph cluster_legend { style=bold; color=black; label =\"view_frames Result\";\n"
1545  << "\"Recorded at time: " << current_time << "\"[ shape=plaintext ] ;\n "
1546  << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl;
1547  }
1548  continue;
1549  }
1550  if (counter_frame->getData(ros::Time(), temp)) {
1551  frame_id_num = temp.frame_id_;
1552  } else {
1553  frame_id_num = 0;
1554  }
1555 
1556  if(frameIDs_reverse[frame_id_num]=="NO_PARENT")
1557  {
1558  mstream << "edge [style=invis];" <<std::endl;
1559  mstream << " subgraph cluster_legend { style=bold; color=black; label =\"view_frames Result\";\n";
1560  if (current_time > 0)
1561  mstream << "\"Recorded at time: " << current_time << "\"[ shape=plaintext ] ;\n ";
1562  mstream << "}" << "->" << "\"" << frameIDs_reverse[counter] << "\";" << std::endl;
1563  }
1564  }
1565  mstream << "}";
1566  return mstream.str();
1567 }
1568 
1569 std::string BufferCore::_allFramesAsDot() const
1570 {
1571  return _allFramesAsDot(0.0);
1572 }
1573 
1574 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
1575 {
1576  std::string error_string;
1577 
1578  output.clear(); //empty vector
1579 
1580  std::stringstream mstream;
1581  boost::mutex::scoped_lock lock(frame_mutex_);
1582 
1583  TransformAccum accum;
1584 
1585  // Get source frame/time using getFrame
1586  CompactFrameID source_id = lookupFrameNumber(source_frame);
1587  CompactFrameID fixed_id = lookupFrameNumber(fixed_frame);
1588  CompactFrameID target_id = lookupFrameNumber(target_frame);
1589 
1590  std::vector<CompactFrameID> source_frame_chain;
1591  int retval = walkToTopParent(accum, source_time, fixed_id, source_id, &error_string, &source_frame_chain);
1592 
1593  if (retval != tf2_msgs::TF2Error::NO_ERROR)
1594  {
1595  switch (retval)
1596  {
1597  case tf2_msgs::TF2Error::CONNECTIVITY_ERROR:
1598  throw ConnectivityException(error_string);
1599  case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR:
1600  throw ExtrapolationException(error_string);
1601  case tf2_msgs::TF2Error::LOOKUP_ERROR:
1602  throw LookupException(error_string);
1603  default:
1604  CONSOLE_BRIDGE_logError("Unknown error code: %d", retval);
1605  assert(0);
1606  }
1607  }
1608 
1609  std::vector<CompactFrameID> target_frame_chain;
1610  retval = walkToTopParent(accum, target_time, target_id, fixed_id, &error_string, &target_frame_chain);
1611 
1612  if (retval != tf2_msgs::TF2Error::NO_ERROR)
1613  {
1614  switch (retval)
1615  {
1616  case tf2_msgs::TF2Error::CONNECTIVITY_ERROR:
1617  throw ConnectivityException(error_string);
1618  case tf2_msgs::TF2Error::EXTRAPOLATION_ERROR:
1619  throw ExtrapolationException(error_string);
1620  case tf2_msgs::TF2Error::LOOKUP_ERROR:
1621  throw LookupException(error_string);
1622  default:
1623  CONSOLE_BRIDGE_logError("Unknown error code: %d", retval);
1624  assert(0);
1625  }
1626  }
1627  // If the two chains overlap clear the overlap
1628  if (source_frame_chain.size() > 0 && target_frame_chain.size() > 0 &&
1629  source_frame_chain.back() == target_frame_chain.front())
1630  {
1631  source_frame_chain.pop_back();
1632  }
1633  // Join the two walks
1634  for (unsigned int i = 0; i < target_frame_chain.size(); ++i)
1635  {
1636  source_frame_chain.push_back(target_frame_chain[i]);
1637  }
1638 
1639 
1640  // Write each element of source_frame_chain as string
1641  for (unsigned int i = 0; i < source_frame_chain.size(); ++i)
1642  {
1643  output.push_back(lookupFrameString(source_frame_chain[i]));
1644  }
1645 }
1646 
1647 int TestBufferCore::_walkToTopParent(BufferCore& buffer, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *frame_chain) const
1648 {
1649  TransformAccum accum;
1650  return buffer.walkToTopParent(accum, time, target_id, source_id, error_string, frame_chain);
1651 }
1652 
1653 } // 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:1402
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:1346
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:1323
tf2::BufferCore::_removeTransformsChangedListener
void _removeTransformsChangedListener(boost::signals2::connection c)
Definition: buffer_core.cpp:1352
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:1325
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:1574
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:1383
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:1569
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:1359
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:1337
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:1334
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:1647
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:1268
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:1329
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:1365
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 Fri Sep 4 2020 03:15:36