Fawkes API  Fawkes Development Version
buffer_core.cpp
1 /***************************************************************************
2  * buffer_core.h - Fawkes tf buffer core (based on ROS tf2)
3  *
4  * Created: Mon Oct 26 11:02:22 2015
5  * Copyright 2015 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version. A runtime exception applies to
12  * this software (see LICENSE.GPL_WRE file mentioned below for details).
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
20  */
21 
22 /* This code is based on ROS tf2 with the following copyright and license:
23  *
24  * Copyright (c) 2010, Willow Garage, Inc.
25  * All rights reserved.
26  *
27  * Redistribution and use in source and binary forms, with or without
28  * modification, are permitted provided that the following conditions are met:
29  *
30  * * Redistributions of source code must retain the above copyright
31  * notice, this list of conditions and the following disclaimer.
32  * * Redistributions in binary form must reproduce the above copyright
33  * notice, this list of conditions and the following disclaimer in the
34  * documentation and/or other materials provided with the distribution.
35  * * Neither the name of the Willow Garage, Inc. nor the names of its
36  * contributors may be used to endorse or promote products derived from
37  * this software without specific prior written permission.
38  *
39  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
40  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
41  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
42  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
43  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
44  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
45  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
46  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
47  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49  * POSSIBILITY OF SUCH DAMAGE.
50  */
51 
52 #include <tf/buffer_core.h>
53 #include <tf/time_cache.h>
54 #include <tf/exceptions.h>
55 #include <tf/types.h>
56 #include <tf/utils.h>
57 
58 #include <sstream>
59 #include <algorithm>
60 
61 namespace fawkes {
62  namespace tf {
63 #if 0 /* just to make Emacs auto-indent happy */
64  }
65 }
66 #endif
67 
68 /// @cond INTERNAL
69 typedef std::pair<fawkes::Time, CompactFrameID> P_TimeAndFrameID;
70 /// @endcond
71 
72 
73 /** Warn if an illegal frame_id was passed.
74  * @param function_name_arg informative message content
75  * @param frame_id frame ID to check
76  * @return true if we have something to warn about, false otherwise
77  */
78 bool
79 BufferCore::warn_frame_id(const char* function_name_arg, const std::string& frame_id) const
80 {
81  if (frame_id.size() == 0) {
82  printf("Invalid argument passed to %s in tf2 frame_ids cannot be empty", function_name_arg);
83  return true;
84  }
85 
86  if (starts_with_slash(frame_id))
87  {
88  printf("Invalid argument '%s' passed to %s in tf2 frame_ids cannot start with a '/'",
89  frame_id.c_str(), function_name_arg);
90  return true;
91  }
92 
93  return false;
94 }
95 
96 /** Check if frame ID is valid and return compact ID.
97  * @param function_name_arg informational for error message
98  * @param frame_id frame ID to validate
99  * @return compact frame ID, if frame is valid
100  * @throw InvalidArgumentException thrown if argument s invalid,
101  * i.e. if it is empty or starts with a slash.
102  * @throw LookupException thrown if frame does not exist
103  */
104 CompactFrameID
105 BufferCore::validate_frame_id(const char* function_name_arg, const std::string& frame_id) const
106 {
107  if (frame_id.empty())
108  {
109  throw InvalidArgumentException("Invalid argument passed to %s in tf2 frame_ids cannot be empty",
110  function_name_arg);
111  }
112 
113  if (starts_with_slash(frame_id))
114  {
115  throw InvalidArgumentException("Invalid argument \"%s\" passed to %s. "
116  "In tf2 frame_ids cannot start with a '/'",
117  frame_id.c_str(), function_name_arg);
118  }
119 
120  CompactFrameID id = lookup_frame_number(frame_id);
121  if (id == 0)
122  {
123  throw LookupException("Frame ID \"%s\" passed to %s does not exist.",
124  frame_id.c_str(), function_name_arg);
125  }
126 
127  return id;
128 }
129 
130 /** Constructor
131  * @param cache_time How long to keep a history of transforms in nanoseconds
132  */
133 BufferCore::BufferCore(float cache_time)
134  : cache_time_(cache_time)
135 {
136  frameIDs_["NO_PARENT"] = 0;
137  frames_.push_back(TimeCacheInterfacePtr());
138  frameIDs_reverse.push_back("NO_PARENT");
139 }
140 
141 BufferCore::~BufferCore()
142 {
143 }
144 
145 /** Clear all data. */
146 void
148 {
149  //old_tf_.clear();
150  std::unique_lock<std::mutex> lock(frame_mutex_);
151  if ( frames_.size() > 1 )
152  {
153  for (std::vector<TimeCacheInterfacePtr>::iterator cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it)
154  {
155  if (*cache_it)
156  (*cache_it)->clear_list();
157  }
158  }
159 
160 }
161 
162 /** Add transform information to the tf data structure
163  * @param transform_in The transform to store
164  * @param authority The source of the information for this transform
165  * @param is_static Record this transform as a static transform. It
166  * will be good across all time. (This cannot be changed after the
167  * first call.)
168  * @return true unless an error occured
169  */
170 bool
172  const std::string& authority, bool is_static)
173 {
174  StampedTransform stripped = transform_in;
175  stripped.frame_id = strip_slash(stripped.frame_id);
176  stripped.child_frame_id = strip_slash(stripped.child_frame_id);
177 
178  bool error_exists = false;
179  if (stripped.child_frame_id == stripped.frame_id)
180  {
181  printf("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());
182  error_exists = true;
183  }
184 
185  if (stripped.child_frame_id == "")
186  {
187  printf("TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str());
188  error_exists = true;
189  }
190 
191  if (stripped.frame_id == "")
192  {
193  printf("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());
194  error_exists = true;
195  }
196 
197  if (std::isnan(stripped.getOrigin().x()) ||
198  std::isnan(stripped.getOrigin().y()) ||
199  std::isnan(stripped.getOrigin().z()) ||
200  std::isnan(stripped.getRotation().x()) ||
201  std::isnan(stripped.getRotation().y()) ||
202  std::isnan(stripped.getRotation().z()) ||
203  std::isnan(stripped.getRotation().w()))
204  {
205  printf("TF_NAN_INPUT: Ignoring transform for child_frame_id \"%s\" "
206  "from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)",
207  stripped.child_frame_id.c_str(), authority.c_str(),
208  stripped.getOrigin().x(), stripped.getOrigin().y(), stripped.getOrigin().z(),
209  stripped.getRotation().x(), stripped.getRotation().y(),
210  stripped.getRotation().z(), stripped.getRotation().w());
211  error_exists = true;
212  }
213 
214  if (error_exists)
215  return false;
216 
217  {
218  std::unique_lock<std::mutex> lock(frame_mutex_);
219  CompactFrameID frame_number = lookup_or_insert_frame_number(stripped.child_frame_id);
220  TimeCacheInterfacePtr frame = get_frame(frame_number);
221  if (! frame)
222  frame = allocate_frame(frame_number, is_static);
223 
224  if (frame->insert_data(TransformStorage(stripped, lookup_or_insert_frame_number(stripped.frame_id), frame_number)))
225  {
226  frame_authority_[frame_number] = authority;
227  }
228  else
229  {
230  printf("TF_OLD_DATA ignoring data from the past for frame %s "
231  "at time %g according to authority %s\n"
232  "Possible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained",
233  stripped.child_frame_id.c_str(), stripped.stamp.in_sec(), authority.c_str());
234  return false;
235  }
236  }
237 
238  //test_transformable_requests();
239 
240  return true;
241 }
242 
243 /** Allocate a new frame cache.
244  * @param cfid frame ID for which to create the frame cache
245  * @param is_static true if the transforms for this frame are static, false otherwise
246  * @return pointer to new cache
247  */
248 TimeCacheInterfacePtr
249 BufferCore::allocate_frame(CompactFrameID cfid, bool is_static)
250 {
251  TimeCacheInterfacePtr frame_ptr = frames_[cfid];
252  if (is_static) {
253  frames_[cfid] = TimeCacheInterfacePtr(new StaticCache());
254  } else {
255  frames_[cfid] = TimeCacheInterfacePtr(new TimeCache(cache_time_));
256  }
257 
258  return frames_[cfid];
259 }
260 
261 enum WalkEnding
262 {
263  Identity,
264  TargetParentOfSource,
265  SourceParentOfTarget,
266  FullPath,
267 };
268 
269 /** Traverse transform tree: walk from frame to top-parent of both.
270  * @param f accumulator
271  * @param time timestamp
272  * @param target_id frame number of target
273  * @param source_id frame number of source
274  * @param error_string accumulated error string
275  * @return error flag from ErrorValues
276  */
277 template<typename F>
279  CompactFrameID target_id, CompactFrameID source_id,
280  std::string* error_string) const
281 {
282  return walk_to_top_parent(f, time, target_id, source_id, error_string, NULL);
283 }
284 
285 /** Traverse transform tree: walk from frame to top-parent of both.
286  * @param f accumulator
287  * @param time timestamp
288  * @param target_id frame number of target
289  * @param source_id frame number of source
290  * @param error_string accumulated error string
291  * @param frame_chain If frame_chain is not NULL, store the traversed
292  * frame tree in vector frame_chain.
293  * @return error flag from ErrorValues
294  */
295 template<typename F>
296 int BufferCore::walk_to_top_parent(F& f, fawkes::Time time, CompactFrameID target_id,
297  CompactFrameID source_id, std::string* error_string,
298  std::vector<CompactFrameID> *frame_chain) const
299 {
300  if (frame_chain)
301  frame_chain->clear();
302 
303  // Short circuit if zero length transform to allow lookups on non existant links
304  if (source_id == target_id)
305  {
306  f.finalize(Identity, time);
307  return NO_ERROR;
308  }
309 
310  //If getting the latest get the latest common time
311  if (time == fawkes::Time(0,0))
312  {
313  int retval = get_latest_common_time(target_id, source_id, time, error_string);
314  if (retval != NO_ERROR)
315  {
316  return retval;
317  }
318  }
319 
320  // Walk the tree to its root from the source frame, accumulating the transform
321  CompactFrameID frame = source_id;
322  CompactFrameID top_parent = frame;
323  uint32_t depth = 0;
324 
325  std::string extrapolation_error_string;
326  bool extrapolation_might_have_occurred = false;
327 
328  while (frame != 0)
329  {
330  TimeCacheInterfacePtr cache = get_frame(frame);
331  if (frame_chain)
332  frame_chain->push_back(frame);
333 
334  if (!cache)
335  {
336  // There will be no cache for the very root of the tree
337  top_parent = frame;
338  break;
339  }
340 
341  CompactFrameID parent = f.gather(cache, time, &extrapolation_error_string);
342  if (parent == 0)
343  {
344  // Just break out here... there may still be a path from source -> target
345  top_parent = frame;
346  extrapolation_might_have_occurred = true;
347  break;
348  }
349 
350  // Early out... target frame is a direct parent of the source frame
351  if (frame == target_id)
352  {
353  f.finalize(TargetParentOfSource, time);
354  return NO_ERROR;
355  }
356 
357  f.accum(true);
358 
359  top_parent = frame;
360  frame = parent;
361 
362  ++depth;
363  if (depth > MAX_GRAPH_DEPTH)
364  {
365  if (error_string)
366  {
367  std::stringstream ss;
368  ss << "The tf tree is invalid because it contains a loop." << std::endl
369  << all_frames_as_string_no_lock() << std::endl;
370  *error_string = ss.str();
371  }
372  return LOOKUP_ERROR;
373  }
374  }
375 
376  // Now walk to the top parent from the target frame, accumulating its transform
377  frame = target_id;
378  depth = 0;
379  std::vector<CompactFrameID> reverse_frame_chain;
380 
381  while (frame != top_parent)
382  {
383  TimeCacheInterfacePtr cache = get_frame(frame);
384  if (frame_chain)
385  reverse_frame_chain.push_back(frame);
386 
387  if (!cache)
388  {
389  break;
390  }
391 
392  CompactFrameID parent = f.gather(cache, time, error_string);
393  if (parent == 0)
394  {
395  if (error_string)
396  {
397  std::stringstream ss;
398  ss << *error_string << ", when looking up transform from frame [" << lookup_frame_string(source_id) << "] to frame [" << lookup_frame_string(target_id) << "]";
399  *error_string = ss.str();
400  }
401 
402  return EXTRAPOLATION_ERROR;
403  }
404 
405  // Early out... source frame is a direct parent of the target frame
406  if (frame == source_id)
407  {
408  f.finalize(SourceParentOfTarget, time);
409  if (frame_chain)
410  {
411  frame_chain->swap(reverse_frame_chain);
412  }
413  return NO_ERROR;
414  }
415 
416  f.accum(false);
417 
418  frame = parent;
419 
420  ++depth;
421  if (depth > MAX_GRAPH_DEPTH)
422  {
423  if (error_string)
424  {
425  std::stringstream ss;
426  ss << "The tf tree is invalid because it contains a loop." << std::endl
427  << all_frames_as_string_no_lock() << std::endl;
428  *error_string = ss.str();
429  }
430  return LOOKUP_ERROR;
431  }
432  }
433 
434  if (frame != top_parent)
435  {
436  if (extrapolation_might_have_occurred)
437  {
438  if (error_string)
439  {
440  std::stringstream ss;
441  ss << extrapolation_error_string << ", when looking up transform from frame [" << lookup_frame_string(source_id) << "] to frame [" << lookup_frame_string(target_id) << "]";
442  *error_string = ss.str();
443  }
444 
445  return EXTRAPOLATION_ERROR;
446 
447  }
448 
449  create_connectivity_error_string(source_id, target_id, error_string);
450  return CONNECTIVITY_ERROR;
451  }
452 
453  f.finalize(FullPath, time);
454  if (frame_chain)
455  {
456  // Pruning: Compare the chains starting at the parent (end) until they differ
457  ssize_t m = reverse_frame_chain.size()-1;
458  ssize_t n = frame_chain->size()-1;
459  for (; m >= 0 && n >= 0; --m, --n)
460  {
461  if ((*frame_chain)[n] != reverse_frame_chain[m])
462  break;
463  }
464  // Erase all duplicate items from frame_chain
465  if (n > 0)
466  frame_chain->erase(frame_chain->begin() + (n-1), frame_chain->end());
467 
468  if (m < (ssize_t)reverse_frame_chain.size())
469  {
470  for (int i = m; i >= 0; --i)
471  {
472  frame_chain->push_back(reverse_frame_chain[i]);
473  }
474  }
475  }
476 
477  return NO_ERROR;
478 }
479 
480 
481 /// @cond INTERNAL
482 struct TransformAccum
483 {
484  TransformAccum()
485  : source_to_top_quat(0.0, 0.0, 0.0, 1.0)
486  , source_to_top_vec(0.0, 0.0, 0.0)
487  , target_to_top_quat(0.0, 0.0, 0.0, 1.0)
488  , target_to_top_vec(0.0, 0.0, 0.0)
489  , result_quat(0.0, 0.0, 0.0, 1.0)
490  , result_vec(0.0, 0.0, 0.0)
491  {
492  }
493 
494  CompactFrameID gather(TimeCacheInterfacePtr cache, fawkes::Time time, std::string* error_string)
495  {
496  if (!cache->get_data(time, st, error_string))
497  {
498  return 0;
499  }
500 
501  return st.frame_id;
502  }
503 
504  void accum(bool source)
505  {
506  if (source)
507  {
508  source_to_top_vec = quatRotate(st.rotation, source_to_top_vec) + st.translation;
509  source_to_top_quat = st.rotation * source_to_top_quat;
510  }
511  else
512  {
513  target_to_top_vec = quatRotate(st.rotation, target_to_top_vec) + st.translation;
514  target_to_top_quat = st.rotation * target_to_top_quat;
515  }
516  }
517 
518  void finalize(WalkEnding end, fawkes::Time _time)
519  {
520  switch (end)
521  {
522  case Identity:
523  break;
524  case TargetParentOfSource:
525  result_vec = source_to_top_vec;
526  result_quat = source_to_top_quat;
527  break;
528  case SourceParentOfTarget:
529  {
530  Quaternion inv_target_quat = target_to_top_quat.inverse();
531  Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
532  result_vec = inv_target_vec;
533  result_quat = inv_target_quat;
534  break;
535  }
536  case FullPath:
537  {
538  Quaternion inv_target_quat = target_to_top_quat.inverse();
539  Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
540 
541  result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
542  result_quat = inv_target_quat * source_to_top_quat;
543  }
544  break;
545  };
546 
547  time = _time;
548  }
549 
550  TransformStorage st;
551  fawkes::Time time;
552  Quaternion source_to_top_quat;
553  Vector3 source_to_top_vec;
554  Quaternion target_to_top_quat;
555  Vector3 target_to_top_vec;
556 
557  Quaternion result_quat;
558  Vector3 result_vec;
559 };
560 ///@endcond
561 
562 /** Lookup transform.
563  * @param target_frame target frame ID
564  * @param source_frame source frame ID
565  * @param time time for which to get the transform, set to (0,0) to get latest
566  * common time frame
567  * @param transform upon return contains the transform
568  * @exception ConnectivityException thrown if no connection between
569  * the source and target frame could be found in the tree.
570  * @exception ExtrapolationException returning a value would have
571  * required extrapolation beyond current limits.
572  * @exception LookupException at least one of the two given frames is
573  * unknown
574  */
575 void
576 BufferCore::lookup_transform(const std::string& target_frame,
577  const std::string& source_frame,
578  const fawkes::Time& time,
579  StampedTransform& transform) const
580 {
581  std::unique_lock<std::mutex> lock(frame_mutex_);
582 
583  if (target_frame == source_frame) {
584  transform.setIdentity();
585  transform.frame_id = target_frame;
586  transform.child_frame_id = source_frame;
587 
588  if (time == fawkes::Time(0,0)) {
589  CompactFrameID target_id = lookup_frame_number(target_frame);
590  TimeCacheInterfacePtr cache = get_frame(target_id);
591  if (cache)
592  transform.stamp = cache->get_latest_timestamp();
593  else
594  transform.stamp = time;
595  } else {
596  transform.stamp = time;
597  }
598  }
599 
600  //Identify case does not need to be validated above
601  CompactFrameID target_id = validate_frame_id("lookup_transform argument target_frame", target_frame);
602  CompactFrameID source_id = validate_frame_id("lookup_transform argument source_frame", source_frame);
603 
604  std::string error_string;
605  TransformAccum accum;
606  int retval = walk_to_top_parent(accum, time, target_id, source_id, &error_string);
607  if (retval != NO_ERROR)
608  {
609  switch (retval)
610  {
611  case CONNECTIVITY_ERROR:
612  throw ConnectivityException("%s", error_string.c_str());
613  case EXTRAPOLATION_ERROR:
614  throw ExtrapolationException("%s", error_string.c_str());
615  case LOOKUP_ERROR:
616  throw LookupException("%s", error_string.c_str());
617  default:
618  //logError("Unknown error code: %d", retval);
619  throw TransformException();
620  }
621  }
622 
623  transform.setOrigin(accum.result_vec);
624  transform.setRotation(accum.result_quat);
625  transform.child_frame_id = source_frame;
626  transform.frame_id = target_frame;
627  transform.stamp = accum.time;
628 }
629 
630 
631 /** Lookup transform assuming a fixed frame.
632  * This will lookup a transformation from source to target, assuming
633  * that there is a fixed frame, by first finding the transform of the
634  * source frame in the fixed frame, and then a transformation from the
635  * fixed frame to the target frame.
636  * @param target_frame target frame ID
637  * @param target_time time for the target frame
638  * @param source_frame source frame ID
639  * @param source_time time in the source frame
640  * @param fixed_frame ID of fixed frame
641  * @param transform upon return contains the transform
642  * @exception ConnectivityException thrown if no connection between
643  * the source and target frame could be found in the tree.
644  * @exception ExtrapolationException returning a value would have
645  * required extrapolation beyond current limits.
646  * @exception LookupException at least one of the two given frames is
647  * unknown
648  */
649 void
650 BufferCore::lookup_transform(const std::string& target_frame, const fawkes::Time& target_time,
651  const std::string& source_frame, const fawkes::Time& source_time,
652  const std::string& fixed_frame, StampedTransform& transform) const
653 {
654  validate_frame_id("lookup_transform argument target_frame", target_frame);
655  validate_frame_id("lookup_transform argument source_frame", source_frame);
656  validate_frame_id("lookup_transform argument fixed_frame", fixed_frame);
657 
658  StampedTransform temp1, temp2;
659  lookup_transform(fixed_frame, source_frame, source_time, temp1);
660  lookup_transform(target_frame, fixed_frame, target_time, temp2);
661  transform.set_data(temp2 * temp1);
662  transform.stamp = temp2.stamp;
663  transform.frame_id = target_frame;
664  transform.child_frame_id = source_frame;
665 }
666 
667 /// @cond INTERNAL
668 struct CanTransformAccum
669 {
670  CompactFrameID gather(TimeCacheInterfacePtr cache, fawkes::Time time, std::string* error_string)
671  {
672  return cache->get_parent(time, error_string);
673  }
674 
675  void accum(bool source)
676  {
677  }
678 
679  void finalize(WalkEnding end, fawkes::Time _time)
680  {
681  }
682 
683  TransformStorage st;
684 };
685 /// @endcond
686 
687 /** Test if a transform is possible.
688  * Internal check that does not lock the frame mutex.
689  * @param target_id The frame number into which to transform
690  * @param source_id The frame number from which to transform
691  * @param time The time at which to transform
692  * @param error_msg passed to walk_to_top_parent
693  * @return true if the transformation can be calculated, false otherwise
694  */
695 bool
696 BufferCore::can_transform_no_lock(CompactFrameID target_id, CompactFrameID source_id,
697  const fawkes::Time& time, std::string* error_msg) const
698 {
699  if (target_id == 0 || source_id == 0)
700  {
701  return false;
702  }
703 
704  if (target_id == source_id)
705  {
706  return true;
707  }
708 
709  CanTransformAccum accum;
710  if (walk_to_top_parent(accum, time, target_id, source_id, error_msg) == NO_ERROR)
711  {
712  return true;
713  }
714 
715  return false;
716 }
717 
718 /** Test if a transform is possible.
719  * Internal check that does lock the frame mutex.
720  * @param target_id The frame number into which to transform
721  * @param source_id The frame number from which to transform
722  * @param time The time at which to transform
723  * @param error_msg passed to walk_to_top_parent
724  * @return true if the transformation can be calculated, false otherwise
725  */
726 bool
727 BufferCore::can_transform_internal(CompactFrameID target_id, CompactFrameID source_id,
728  const fawkes::Time& time, std::string* error_msg) const
729 {
730  std::unique_lock<std::mutex> lock(frame_mutex_);
731  return can_transform_no_lock(target_id, source_id, time, error_msg);
732 }
733 
734 /** Test if a transform is possible
735  * @param target_frame The frame into which to transform
736  * @param source_frame The frame from which to transform
737  * @param time The time at which to transform
738  * @param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL
739  * @return True if the transform is possible, false otherwise
740  */
741 bool
742 BufferCore::can_transform(const std::string& target_frame, const std::string& source_frame,
743  const fawkes::Time& time, std::string* error_msg) const
744 {
745  // Short circuit if target_frame == source_frame
746  if (target_frame == source_frame)
747  return true;
748 
749  if (warn_frame_id("canTransform argument target_frame", target_frame))
750  return false;
751  if (warn_frame_id("canTransform argument source_frame", source_frame))
752  return false;
753 
754  std::unique_lock<std::mutex> lock(frame_mutex_);
755 
756  CompactFrameID target_id = lookup_frame_number(target_frame);
757  CompactFrameID source_id = lookup_frame_number(source_frame);
758 
759  return can_transform_no_lock(target_id, source_id, time, error_msg);
760 }
761 
762 /** Test if a transform is possible.
763  * @param target_frame The frame into which to transform
764  * @param target_time The time into which to transform
765  * @param source_frame The frame from which to transform
766  * @param source_time The time from which to transform
767  * @param fixed_frame The frame in which to treat the transform as constant in time
768  * @param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL
769  * @return true if the transform is possible, false otherwise
770  */
771 bool
772 BufferCore::can_transform(const std::string& target_frame, const fawkes::Time& target_time,
773  const std::string& source_frame, const fawkes::Time& source_time,
774  const std::string& fixed_frame, std::string* error_msg) const
775 {
776  if (warn_frame_id("canTransform argument target_frame", target_frame))
777  return false;
778  if (warn_frame_id("canTransform argument source_frame", source_frame))
779  return false;
780  if (warn_frame_id("canTransform argument fixed_frame", fixed_frame))
781  return false;
782 
783  return
784  can_transform(target_frame, fixed_frame, target_time) &&
785  can_transform(fixed_frame, source_frame, source_time, error_msg);
786 }
787 
788 
789 /** Accessor to get frame cache.
790  * This is an internal function which will get the pointer to the
791  * frame associated with the frame id
792  * @param frame_id The frameID of the desired Reference Frame
793  * @return shared pointer to time cache
794  */
795 TimeCacheInterfacePtr
796 BufferCore::get_frame(CompactFrameID frame_id) const
797 {
798  if (frame_id >= frames_.size())
799  return TimeCacheInterfacePtr();
800  else
801  {
802  return frames_[frame_id];
803  }
804 }
805 
806 /** Get compact ID for frame.
807  * @param frameid_str frame ID string
808  * @return compact frame ID, zero if frame unknown
809  */
810 CompactFrameID
811 BufferCore::lookup_frame_number(const std::string& frameid_str) const
812 {
813  CompactFrameID retval;
814  M_StringToCompactFrameID::const_iterator map_it = frameIDs_.find(frameid_str);
815  if (map_it == frameIDs_.end())
816  {
817  retval = CompactFrameID(0);
818  }
819  else
820  retval = map_it->second;
821  return retval;
822 }
823 
824 /** Get compact ID for frame or create if not existant.
825  * @param frameid_str frame ID string
826  * @return compact frame ID
827  */
828 CompactFrameID
829 BufferCore::lookup_or_insert_frame_number(const std::string& frameid_str)
830 {
831  CompactFrameID retval = 0;
832  M_StringToCompactFrameID::iterator map_it = frameIDs_.find(frameid_str);
833  if (map_it == frameIDs_.end())
834  {
835  retval = CompactFrameID(frames_.size());
836  frames_.push_back(TimeCacheInterfacePtr());//Just a place holder for iteration
837  frameIDs_[frameid_str] = retval;
838  frameIDs_reverse.push_back(frameid_str);
839  }
840  else
841  retval = frameIDs_[frameid_str];
842 
843  return retval;
844 }
845 
846 /** Get frame string for compact frame ID.
847  * @param frame_id_num compact frame ID
848  * @return string for frame ID
849  * @throw LookupException thrown if compact frame ID unknown
850  */
851 const std::string&
852 BufferCore::lookup_frame_string(CompactFrameID frame_id_num) const
853 {
854  if (frame_id_num >= frameIDs_reverse.size())
855  {
856  throw LookupException("Reverse lookup of frame id %u failed!", frame_id_num);
857  }
858  else
859  return frameIDs_reverse[frame_id_num];
860 }
861 
862 /** Create error string.
863  * @param source_frame compact ID of source frame
864  * @param target_frame compact ID of target frame
865  * @param out upon return contains error string if non-NULL
866  */
867 void
869  CompactFrameID target_frame, std::string* out) const
870 {
871  if (!out)
872  {
873  return;
874  }
875  *out = std::string("Could not find a connection between '"+lookup_frame_string(target_frame)+"' and '"+
876  lookup_frame_string(source_frame)+"' because they are not part of the same tree."+
877  "Tf has two or more unconnected trees.");
878 }
879 /// @endcond
880 
881 /** A way to see what frames have been cached.
882  * Useful for debugging
883  * @return all current frames as a single string
884  */
885 std::string
887 {
888  std::unique_lock<std::mutex> lock(frame_mutex_);
889  return this->all_frames_as_string_no_lock();
890 }
891 
892 /** A way to see what frames have been cached.
893  * Useful for debugging. Use this call internally.
894  * @return all current frames as a single string
895  */
896 std::string
898 {
899  std::stringstream mstream;
900 
901  TransformStorage temp;
902 
903  // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it)
904 
905  ///regular transforms
906  for (unsigned int counter = 1; counter < frames_.size(); counter ++)
907  {
908  TimeCacheInterfacePtr frame_ptr = get_frame(CompactFrameID(counter));
909  if (frame_ptr == NULL)
910  continue;
911  CompactFrameID frame_id_num;
912  if( frame_ptr->get_data(fawkes::Time(0,0), temp))
913  frame_id_num = temp.frame_id;
914  else
915  {
916  frame_id_num = 0;
917  }
918  mstream << "Frame "<< frameIDs_reverse[counter] << " exists with parent " << frameIDs_reverse[frame_id_num] << "." <<std::endl;
919  }
920 
921  return mstream.str();
922 }
923 
924 /// @cond INTERNAL
925 struct TimeAndFrameIDFrameComparator
926 {
927  TimeAndFrameIDFrameComparator(CompactFrameID id)
928  : id(id)
929  {}
930 
931  bool operator()(const P_TimeAndFrameID& rhs) const
932  {
933  return rhs.second == id;
934  }
935 
936  CompactFrameID id;
937 };
938 /// @endcond
939 
940 
941 /** Get latest common time of two frames.
942  * @param target_id target frame number
943  * @param source_id source frame number
944  * @param time upon return contains latest common timestamp
945  * @param error_string upon error contains accumulated error message.
946  * @return value from ErrorValues
947  */
948 int
949 BufferCore::get_latest_common_time(CompactFrameID target_id, CompactFrameID source_id,
950  fawkes::Time & time, std::string * error_string) const
951 {
952  // Error if one of the frames don't exist.
953  if (source_id == 0 || target_id == 0) return LOOKUP_ERROR;
954 
955  if (source_id == target_id)
956  {
957  TimeCacheInterfacePtr cache = get_frame(source_id);
958  //Set time to latest timestamp of frameid in case of target and source frame id are the same
959  if (cache)
960  time = cache->get_latest_timestamp();
961  else
962  time = fawkes::Time(0,0);
963  return NO_ERROR;
964  }
965 
966  std::vector<P_TimeAndFrameID> lct_cache;
967 
968  // Walk the tree to its root from the source frame, accumulating the list of parent/time as well as the latest time
969  // in the target is a direct parent
970  CompactFrameID frame = source_id;
971  P_TimeAndFrameID temp;
972  uint32_t depth = 0;
973  fawkes::Time common_time = fawkes::TIME_MAX;
974  while (frame != 0)
975  {
976  TimeCacheInterfacePtr cache = get_frame(frame);
977 
978  if (!cache)
979  {
980  // There will be no cache for the very root of the tree
981  break;
982  }
983 
984  P_TimeAndFrameID latest = cache->get_latest_time_and_parent();
985 
986  if (latest.second == 0)
987  {
988  // Just break out here... there may still be a path from source -> target
989  break;
990  }
991 
992  if (!latest.first.is_zero())
993  {
994  common_time = std::min(latest.first, common_time);
995  }
996 
997  lct_cache.push_back(latest);
998 
999  frame = latest.second;
1000 
1001  // Early out... target frame is a direct parent of the source frame
1002  if (frame == target_id)
1003  {
1004  time = common_time;
1005  if (time == fawkes::TIME_MAX)
1006  {
1007  time = fawkes::Time(0,0);
1008  }
1009  return NO_ERROR;
1010  }
1011 
1012  ++depth;
1013  if (depth > MAX_GRAPH_DEPTH)
1014  {
1015  if (error_string)
1016  {
1017  std::stringstream ss;
1018  ss<<"The tf tree is invalid because it contains a loop." << std::endl
1019  << all_frames_as_string_no_lock() << std::endl;
1020  *error_string = ss.str();
1021  }
1022  return LOOKUP_ERROR;
1023  }
1024  }
1025 
1026  // Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent
1027  frame = target_id;
1028  depth = 0;
1029  common_time = fawkes::TIME_MAX;
1030  CompactFrameID common_parent = 0;
1031  while (true)
1032  {
1033  TimeCacheInterfacePtr cache = get_frame(frame);
1034 
1035  if (!cache)
1036  {
1037  break;
1038  }
1039 
1040  P_TimeAndFrameID latest = cache->get_latest_time_and_parent();
1041 
1042  if (latest.second == 0)
1043  {
1044  break;
1045  }
1046 
1047  if (!latest.first.is_zero())
1048  {
1049  common_time = std::min(latest.first, common_time);
1050  }
1051 
1052  std::vector<P_TimeAndFrameID>::iterator it =
1053  std::find_if(lct_cache.begin(), lct_cache.end(),
1054  /* Nice version, but requires rather recent compiler
1055  [&latest](const P_TimeAndFrameID& rhs){
1056  return rhs.second == latest.second;
1057  });
1058  */
1059  TimeAndFrameIDFrameComparator(latest.second));
1060 
1061 
1062 
1063  if (it != lct_cache.end()) // found a common parent
1064  {
1065  common_parent = it->second;
1066  break;
1067  }
1068 
1069  frame = latest.second;
1070 
1071  // Early out... source frame is a direct parent of the target frame
1072  if (frame == source_id)
1073  {
1074  time = common_time;
1075  if (time == fawkes::TIME_MAX)
1076  {
1077  time = fawkes::Time(0,0);
1078  }
1079  return NO_ERROR;
1080  }
1081 
1082  ++depth;
1083  if (depth > MAX_GRAPH_DEPTH)
1084  {
1085  if (error_string)
1086  {
1087  std::stringstream ss;
1088  ss<<"The tf tree is invalid because it contains a loop." << std::endl
1089  << all_frames_as_string_no_lock() << std::endl;
1090  *error_string = ss.str();
1091  }
1092  return LOOKUP_ERROR;
1093  }
1094  }
1095 
1096  if (common_parent == 0)
1097  {
1098  create_connectivity_error_string(source_id, target_id, error_string);
1099  return CONNECTIVITY_ERROR;
1100  }
1101 
1102  // Loop through the source -> root list until we hit the common parent
1103  {
1104  std::vector<P_TimeAndFrameID>::iterator it = lct_cache.begin();
1105  std::vector<P_TimeAndFrameID>::iterator end = lct_cache.end();
1106  for (; it != end; ++it)
1107  {
1108  if (!it->first.is_zero())
1109  {
1110  common_time = std::min(common_time, it->first);
1111  }
1112 
1113  if (it->second == common_parent)
1114  {
1115  break;
1116  }
1117  }
1118  }
1119 
1120  if (common_time == fawkes::TIME_MAX)
1121  {
1122  common_time = fawkes::Time(0,0);
1123  }
1124 
1125  time = common_time;
1126  return NO_ERROR;
1127 }
1128 
1129 /** A way to see what frames have been cached in yaml format
1130  * Useful for debugging tools.
1131  * @param current_time current time to compute delay
1132  * @return YAML string
1133  */
1134 std::string
1135 BufferCore::all_frames_as_YAML(double current_time) const
1136 {
1137  std::stringstream mstream;
1138  std::unique_lock<std::mutex> lock(frame_mutex_);
1139 
1140  TransformStorage temp;
1141 
1142  if (frames_.size() ==1)
1143  mstream <<"[]";
1144 
1145  mstream.precision(3);
1146  mstream.setf(std::ios::fixed,std::ios::floatfield);
1147 
1148  // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it)
1149  for (unsigned int counter = 1; counter < frames_.size(); counter ++)//one referenced for 0 is no frame
1150  {
1151  CompactFrameID cfid = CompactFrameID(counter);
1152  CompactFrameID frame_id_num;
1153  TimeCacheInterfacePtr cache = get_frame(cfid);
1154  if (!cache)
1155  {
1156  continue;
1157  }
1158 
1159  if(!cache->get_data(fawkes::Time(0,0), temp))
1160  {
1161  continue;
1162  }
1163 
1164  frame_id_num = temp.frame_id;
1165 
1166  std::string authority = "no recorded authority";
1167  std::map<CompactFrameID, std::string>::const_iterator it = frame_authority_.find(cfid);
1168  if (it != frame_authority_.end()) {
1169  authority = it->second;
1170  }
1171 
1172  double rate = cache->get_list_length() / std::max((cache->get_latest_timestamp().in_sec() -
1173  cache->get_oldest_timestamp().in_sec() ), 0.0001);
1174 
1175  mstream << std::fixed; //fixed point notation
1176  mstream.precision(3); //3 decimal places
1177  mstream << frameIDs_reverse[cfid] << ": " << std::endl;
1178  mstream << " parent: '" << frameIDs_reverse[frame_id_num] << "'" << std::endl;
1179  mstream << " broadcaster: '" << authority << "'" << std::endl;
1180  mstream << " rate: " << rate << std::endl;
1181  mstream << " most_recent_transform: " << (cache->get_latest_timestamp()).in_sec() << std::endl;
1182  mstream << " oldest_transform: " << (cache->get_oldest_timestamp()).in_sec() << std::endl;
1183  if ( current_time > 0 ) {
1184  mstream << " transform_delay: " << current_time - cache->get_latest_timestamp().in_sec() << std::endl;
1185  }
1186  mstream << " buffer_length: " << (cache->get_latest_timestamp() - cache->get_oldest_timestamp()).in_sec() << std::endl;
1187  }
1188 
1189  return mstream.str();
1190 }
1191 
1192 /** Get latest frames as YAML.
1193  * @return YAML string
1194  */
1195 std::string
1197 {
1198  return this->all_frames_as_YAML(0.0);
1199 }
1200 
1201 
1202 } // end namespace tf
1203 } // end namespace fawkes
double in_sec() const
Convet time to seconds.
Definition: time.cpp:232
CompactFrameID lookup_or_insert_frame_number(const std::string &frameid_str)
String to number for frame lookup with dynamic allocation of new frames.
void set_data(const tf::Transform &input)
Set the inherited Transform data.
Definition: types.h:126
CompactFrameID validate_frame_id(const char *function_name_arg, const std::string &frame_id) const
Check if frame ID is valid and return compact ID.
std::string all_frames_as_string() const
A way to see what frames have been cached.
Base class for fawkes tf exceptions.
Definition: exceptions.h:34
Fawkes library namespace.
V_TimeCacheInterface frames_
The pointers to potential frames that the tree can be made of.
Definition: buffer_core.h:171
Storage for transforms and their parent.
bool can_transform_no_lock(CompactFrameID target_id, CompactFrameID source_id, const fawkes::Time &time, std::string *error_msg) const
Test if a transform is possible.
int get_latest_common_time(CompactFrameID target_frame, CompactFrameID source_frame, fawkes::Time &time, std::string *error_string) const
Get latest common time of two frames.
bool set_transform(const StampedTransform &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
void clear()
Clear all data.
CompactFrameID lookup_frame_number(const std::string &frameid_str) const
String to number for frame lookup with dynamic allocation of new frames.
bool warn_frame_id(const char *function_name_arg, const std::string &frame_id) const
Warn if an illegal frame_id was passed.
Definition: buffer_core.cpp:79
fawkes::Time stamp
Timestamp of this transform.
Definition: types.h:100
bool can_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, std::string *error_msg=NULL) const
Test if a transform is possible.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
A class for handling time.
Definition: time.h:91
Passed argument was invalid.
Definition: exceptions.h:58
std::map< CompactFrameID, std::string > frame_authority_
A map to lookup the most recent authority for a given frame.
Definition: buffer_core.h:183
Request would have required extrapolation beyond current limits.
Definition: exceptions.h:52
No connection between two frames in tree.
Definition: exceptions.h:40
std::string all_frames_as_YAML() const
Get latest frames as YAML.
Time based transform cache.
Definition: time_cache.h:98
std::string all_frames_as_string_no_lock() const
A way to see what frames have been cached.
CompactFrameID frame_id
parent/reference frame number
void create_connectivity_error_string(CompactFrameID source_frame, CompactFrameID target_frame, std::string *out) const
Create error string.
Transform cache for static transforms.
Definition: time_cache.h:142
TimeCacheInterfacePtr get_frame(CompactFrameID c_frame_id) const
Accessor to get frame cache.
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
const std::string & lookup_frame_string(CompactFrameID frame_id_num) const
Number to string frame lookup may throw LookupException if number invalid.
std::string child_frame_id
Frame ID of child frame, e.g.
Definition: types.h:105
std::mutex frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
Definition: buffer_core.h:174
M_StringToCompactFrameID frameIDs_
Mapping from frame string IDs to compact IDs.
Definition: buffer_core.h:179
float cache_time_
How long to cache transform history.
Definition: buffer_core.h:187
const Time TIME_MAX
Instance of Time denoting the maximum value possible.
Definition: time.cpp:47
std::string frame_id
Parent/reference frame ID.
Definition: types.h:102
TimeCacheInterfacePtr allocate_frame(CompactFrameID cfid, bool is_static)
Allocate a new frame cache.
A frame could not be looked up.
Definition: exceptions.h:46
bool can_transform_internal(CompactFrameID target_id, CompactFrameID source_id, const fawkes::Time &time, std::string *error_msg) const
Test if a transform is possible.
BufferCore(float cache_time=DEFAULT_CACHE_TIME)
assuming the tree has a loop
int walk_to_top_parent(F &f, fawkes::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string *error_string) const
Traverse transform tree: walk from frame to top-parent of both.
std::vector< std::string > frameIDs_reverse
A map from CompactFrameID frame_id_numbers to string for debugging and output.
Definition: buffer_core.h:181
static const uint32_t MAX_GRAPH_DEPTH
Maximum number of times to recurse before.
Definition: buffer_core.h:116