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


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Oct 16 2020 19:08:50