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