Fawkes API  Fawkes Development Version
transformer.cpp
1 /***************************************************************************
2  * transformer.cpp - Fawkes tf transformer (based on ROS tf)
3  *
4  * Created: Thu Oct 20 10:56:25 2011
5  * Copyright 2011 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 tf with the following copyright and license:
23  *
24  * Copyright (c) 2008, 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/transformer.h>
53 #include <tf/time_cache.h>
54 #include <tf/exceptions.h>
55 
56 #include <core/threading/mutex_locker.h>
57 #include <iostream>
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 /** @class Transformer <tf/transformer.h>
69  * Coordinate transforms between any two frames in a system.
70  *
71  * This class provides a simple interface to allow recording and
72  * lookup of relationships between arbitrary frames of the system.
73  *
74  * TF assumes that there is a tree of coordinate frame transforms
75  * which define the relationship between all coordinate frames. For
76  * example your typical robot would have a transform from global to
77  * real world. And then from base to hand, and from base to head.
78  * But Base to Hand really is composed of base to shoulder to elbow to
79  * wrist to hand. TF is designed to take care of all the intermediate
80  * steps for you.
81  *
82  * Internal Representation TF will store frames with the parameters
83  * necessary for generating the transform into that frame from it's
84  * parent and a reference to the parent frame. Frames are designated
85  * using an std::string 0 is a frame without a parent (the top of a
86  * tree) The positions of frames over time must be pushed in.
87  *
88  * All function calls which pass frame ids can potentially throw the
89  * exception fawkes::tf::LookupException
90  *
91  *
92  * @fn bool Transformer::is_enabled() const
93  * Check if transformer is enabled.
94  * @return true if enabled, false otherwise
95  *
96  * @var static const unsigned int Transformer::MAX_GRAPH_DEPTH
97  * Maximum number of times to recurse before assuming the tree
98  * has a loop
99  *
100  * @param static const int64_t Transformer::DEFAULT_MAX_EXTRAPOLATION_DISTANCE
101  * The default amount of time to extrapolate.
102  */
103 
104 /** The default amount of time to extrapolate. */
105 const float Transformer::DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 0.f;
106 
107 /// Flag to advise accumulator finalization.
108 enum WalkEnding
109 {
110  Identity, ///< identity
111  TargetParentOfSource, ///< Target is parent of source
112  SourceParentOfTarget, ///< Source is parent of target
113  FullPath, ///< Full path between source and target.
114 };
115 
116 /// Internal error values
117 enum ErrorValues {
118  NO_ERROR = 0, ///< No error occured.
119  LOOKUP_ERROR, ///< Frame ID lookup error
120  CONNECTIVITY_ERROR, ///< No connection between frames found
121  EXTRAPOLATION_ERROR ///< Extrapolation required out of limits.
122 };
123 
124 /** Accumulator to test for transformability.
125  * Operations are basically noops.
126  */
128 {
129  /** Gather frame number.
130  * @param cache cache
131  * @param time time
132  * @param error_string string containing error message in case of failure
133  * @return parent frame number
134  */
135  CompactFrameID
136  gather(TimeCache* cache, fawkes::Time time, std::string* error_string)
137  {
138  return cache->get_parent(time, error_string);
139  }
140 
141  /** Accumulate.
142  * @param source true if looking from source
143  */
144  void accum(bool source)
145  {
146  }
147 
148  /** Finalize accumulation.
149  * @param end flag how the walk ended
150  * @param _time time
151  */
152  void finalize(WalkEnding end, fawkes::Time _time)
153  {
154  }
155 
156  /// Transform storage to use.
158 };
159 
160 /** Accumulator to accumulate transforms between two frames.
161  */
163 {
164  /** Constructor. */
166  : source_to_top_quat(0.0, 0.0, 0.0, 1.0)
167  , source_to_top_vec(0.0, 0.0, 0.0)
168  , target_to_top_quat(0.0, 0.0, 0.0, 1.0)
169  , target_to_top_vec(0.0, 0.0, 0.0)
170  , result_quat(0.0, 0.0, 0.0, 1.0)
171  , result_vec(0.0, 0.0, 0.0)
172  {
173  }
174 
175  /** Gather frame number.
176  * @param cache cache
177  * @param time time
178  * @param error_string string containing error message in case of failure
179  * @return parent frame number
180  */
181  CompactFrameID
182  gather(TimeCache* cache, fawkes::Time time, std::string* error_string)
183  {
184  if (!cache->get_data(time, st, error_string))
185  {
186  return 0;
187  }
188 
189  return st.frame_id_;
190  }
191 
192  /** Accumulate.
193  * @param source true if looking from source
194  */
195  void accum(bool source)
196  {
197  if (source)
198  {
199  source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_;
200  source_to_top_quat = st.rotation_ * source_to_top_quat;
201  }
202  else
203  {
204  target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_;
205  target_to_top_quat = st.rotation_ * target_to_top_quat;
206  }
207  }
208 
209  /** Finalize accumulation.
210  * @param end flag how the walk ended
211  * @param _time time
212  */
213  void finalize(WalkEnding end, fawkes::Time _time)
214  {
215  switch (end)
216  {
217  case Identity:
218  break;
219  case TargetParentOfSource:
220  result_vec = source_to_top_vec;
221  result_quat = source_to_top_quat;
222  break;
223  case SourceParentOfTarget:
224  {
225  btQuaternion inv_target_quat = target_to_top_quat.inverse();
226  btVector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
227  result_vec = inv_target_vec;
228  result_quat = inv_target_quat;
229  break;
230  }
231  case FullPath:
232  {
233  btQuaternion inv_target_quat = target_to_top_quat.inverse();
234  btVector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
235 
236  result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
237  result_quat = inv_target_quat * source_to_top_quat;
238  }
239  break;
240  };
241 
242  time = _time;
243  }
244 
245  /// Transform storage.
247  /// time stamp
249  /// Source to top accumulated quaternion
250  btQuaternion source_to_top_quat;
251  /// Source to top accumulated vector
252  btVector3 source_to_top_vec;
253  /// Target to top accumulated quaternion
254  btQuaternion target_to_top_quat;
255  /// Target to top accumulated vector
256  btVector3 target_to_top_vec;
257 
258  /// After finalize contains result quaternion.
259  btQuaternion result_quat;
260  /// After finalize contains result vector.
261  btVector3 result_vec;
262 };
263 
264 /** Constructor.
265  * @param cache_time time in seconds to cache incoming transforms
266  */
267 Transformer::Transformer(float cache_time)
268  : cache_time_(cache_time),
269  fall_back_to_wall_time_(false)
270 {
272  frameIDs_["NO_PARENT"] = 0;
273  //unused but needed for iteration over all elements
274  frames_.push_back(NULL);
275  frameIDs_reverse.push_back("NO_PARENT");
276 
277  frame_mutex_ = new Mutex();
278 }
279 
280 
281 /** Destructor. */
283 {
284  // deallocate all frames
286  std::vector<TimeCache*>::iterator cache_it;
287  for (cache_it = frames_.begin(); cache_it != frames_.end(); ++cache_it)
288  {
289  delete (*cache_it);
290  }
291 
292 };
293 
294 
295 /** Set transformer enabled or disabled.
296  * @param enabled true to enable, false to disable
297  */
298 void
300 {
301  enabled_ = enabled;
302 }
303 
304 /** Clear cached transforms. */
305 void
307 {
309  if ( frames_.size() > 1 )
310  {
311  std::vector< TimeCache*>::iterator cache_it;
312  for (cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it)
313  {
314  (*cache_it)->clear_list();
315  }
316  }
317 }
318 
319 
320 /** Check if frame exists.
321  * @param frame_id_str frame ID
322  * @result true if frame exists, false otherwise
323  */
324 bool
325 Transformer::frame_exists(const std::string& frame_id_str) const
326 {
328  std::string frame_id_resolveped = /*assert_resolved(tf_prefix_,*/ frame_id_str;
329 
330  return frameIDs_.count(frame_id_resolveped);
331 }
332 
333 void
334 Transformer::create_connectivity_error_string(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const
335 {
336  if (!out) return;
337 
338  *out = std::string("Could not find a connection between '"+lookup_frame_string(target_frame)+"' and '"+
339  lookup_frame_string(source_frame)+"' because they are not part of the same tree."+
340  "Tf has two or more unconnected trees.");
341 }
342 
343 
344 /** Walk from frame to top-parent of both.
345  * @param f accumulator
346  * @param time timestamp
347  * @param target_id frame number of target
348  * @param source_id frame number of source
349  * @param error_string accumulated error string
350  * @return error flag from ErrorValues
351  */
352 template<typename F>
353 int
354 Transformer::walk_to_top_parent(F& f, fawkes::Time time,
355  CompactFrameID target_id, CompactFrameID source_id,
356  std::string* error_string) const
357 {
358  // Short circuit if zero length transform to allow lookups on non existant links
359  if (source_id == target_id)
360  {
361  f.finalize(Identity, time);
362  return NO_ERROR;
363  }
364 
365  //If getting the latest get the latest common time
366  if (time == fawkes::Time(0,0))
367  {
368  int retval = get_latest_common_time(target_id, source_id, time, error_string);
369  if (retval != NO_ERROR)
370  {
371  return retval;
372  }
373  }
374 
375  // Walk the tree to its root from the source frame, accumulating the transform
376  CompactFrameID frame = source_id;
377  CompactFrameID top_parent = frame;
378  uint32_t depth = 0;
379  while (frame != 0)
380  {
381  TimeCache* cache = get_frame(frame);
382 
383  if (!cache)
384  {
385  // There will be no cache for the very root of the tree
386  top_parent = frame;
387  break;
388  }
389 
390  CompactFrameID parent = f.gather(cache, time, 0);
391  if (parent == 0)
392  {
393  // Just break out here... there may still be a path from source -> target
394  top_parent = frame;
395  break;
396  }
397 
398  // Early out... target frame is a direct parent of the source frame
399  if (frame == target_id)
400  {
401  f.finalize(TargetParentOfSource, time);
402  return NO_ERROR;
403  }
404 
405  f.accum(true);
406 
407  top_parent = frame;
408  frame = parent;
409 
410  ++depth;
411  if (depth > MAX_GRAPH_DEPTH)
412  {
413  if (error_string)
414  {
415  std::stringstream ss;
416  //ss << "The tf tree is invalid because it contains a loop." << std::endl
417  // << allFramesAsString() << std::endl;
418  *error_string = ss.str();
419  }
420  return LOOKUP_ERROR;
421  }
422  }
423 
424  // Now walk to the top parent from the target frame, accumulating its transform
425  frame = target_id;
426  depth = 0;
427  while (frame != top_parent)
428  {
429  TimeCache* cache = get_frame(frame);
430 
431  if (!cache)
432  {
433  break;
434  }
435 
436  CompactFrameID parent = f.gather(cache, time, error_string);
437  if (parent == 0)
438  {
439  if (error_string)
440  {
441  std::stringstream ss;
442  ss << "Looking up transform from frame [" << lookup_frame_string(source_id) <<
443  "] to frame [" << lookup_frame_string(target_id) << "] failed";
444  *error_string = ss.str();
445  }
446 
447  return EXTRAPOLATION_ERROR;
448  }
449 
450  // Early out... source frame is a direct parent of the target frame
451  if (frame == source_id)
452  {
453  f.finalize(SourceParentOfTarget, time);
454  return NO_ERROR;
455  }
456 
457  f.accum(false);
458 
459  frame = parent;
460 
461  ++depth;
462  if (depth > MAX_GRAPH_DEPTH)
463  {
464  if (error_string)
465  {
466  std::stringstream ss;
467  //ss << "The tf tree is invalid because it contains a loop." << std::endl
468  // << allFramesAsString() << std::endl;
469  *error_string = "The tf tree is invalid because it contains a loop.";
470  }
471  return LOOKUP_ERROR;
472  }
473  }
474 
475  if (frame != top_parent)
476  {
477  create_connectivity_error_string(source_id, target_id, error_string);
478  return CONNECTIVITY_ERROR;
479  }
480 
481  f.finalize(FullPath, time);
482 
483  return NO_ERROR;
484 }
485 
486 /** Time and frame ID comparator class. */
488 {
489  /** Constructor.
490  * @param id frame number to look for
491  */
492  TimeAndFrameIDFrameComparator(CompactFrameID id)
493  : id(id)
494  {}
495 
496  /** Application operator.
497  * @param rhs right-hand side to compare to
498  * @return true if the frame numbers are equal, false otherwise
499  */
500  bool operator()(const P_TimeAndFrameID& rhs) const
501  {
502  return rhs.second == id;
503  }
504 
505  /// Frame number to look for.
506  CompactFrameID id;
507 };
508 
509 
510 /** Get latest common time of two frames.
511  * @param target_id target frame number
512  * @param source_id source frame number
513  * @param time upon return contains latest common timestamp
514  * @param error_string upon error contains accumulated error message.
515  * @return value from ErrorValues
516  */
517 int
518 Transformer::get_latest_common_time(CompactFrameID target_id, CompactFrameID source_id,
519  fawkes::Time & time, std::string *error_string) const
520 {
521  if (source_id == target_id)
522  {
523  //Set time to latest timestamp of frameid in case of target and source frame id are the same
524  time.stamp();
525  return NO_ERROR;
526  }
527 
528  std::vector<P_TimeAndFrameID> lct_cache;
529 
530  // Walk the tree to its root from the source frame, accumulating the list of parent/time as well as the latest time
531  // in the target is a direct parent
532  CompactFrameID frame = source_id;
533  P_TimeAndFrameID temp;
534  uint32_t depth = 0;
535  fawkes::Time common_time = fawkes::TIME_MAX;
536  while (frame != 0)
537  {
538  TimeCache* cache = get_frame(frame);
539 
540  if (!cache) {
541  // There will be no cache for the very root of the tree
542  break;
543  }
544 
545  P_TimeAndFrameID latest = cache->get_latest_time_and_parent();
546 
547  if (latest.second == 0) {
548  // Just break out here... there may still be a path from source -> target
549  break;
550  }
551 
552  if (!latest.first.is_zero()) {
553  common_time = std::min(latest.first, common_time);
554  }
555 
556  lct_cache.push_back(latest);
557 
558  frame = latest.second;
559 
560  // Early out... target frame is a direct parent of the source frame
561  if (frame == target_id) {
562  time = common_time;
563  if (time == fawkes::TIME_MAX) {
564  time = fawkes::Time();
565  }
566  return NO_ERROR;
567  }
568 
569  ++depth;
570  if (depth > MAX_GRAPH_DEPTH) {
571  if (error_string) {
572  /*
573  std::stringstream ss;
574  ss<<"The tf tree is invalid because it contains a loop." << std::endl
575  << allFramesAsString() << std::endl;
576  *error_string = ss.str();
577  */
578  }
579  return LOOKUP_ERROR;
580  }
581  }
582 
583  // Now walk to the top parent from the target frame, accumulating the latest time and looking for a common parent
584  frame = target_id;
585  depth = 0;
586  common_time = fawkes::TIME_MAX;
587  CompactFrameID common_parent = 0;
588  while (true)
589  {
590  TimeCache* cache = get_frame(frame);
591 
592  if (!cache) {
593  break;
594  }
595 
596  P_TimeAndFrameID latest = cache->get_latest_time_and_parent();
597 
598  if (latest.second == 0) {
599  break;
600  }
601 
602  if (!latest.first.is_zero()) {
603  common_time = std::min(latest.first, common_time);
604  }
605 
606  std::vector<P_TimeAndFrameID>::iterator it =
607  std::find_if(lct_cache.begin(), lct_cache.end(),
608  TimeAndFrameIDFrameComparator(latest.second));
609 
610  if (it != lct_cache.end()) { // found a common parent
611  common_parent = it->second;
612  break;
613  }
614 
615  frame = latest.second;
616 
617  // Early out... source frame is a direct parent of the target frame
618  if (frame == source_id) {
619  time = common_time;
620  if (time == fawkes::TIME_MAX) {
621  time = fawkes::Time();
622  }
623  return NO_ERROR;
624  }
625 
626  ++depth;
627  if (depth > MAX_GRAPH_DEPTH) {
628  if (error_string) {
629  std::stringstream ss;
630  //ss<<"The tf tree is invalid because it contains a loop." << std::endl
631  // << allFramesAsString() << std::endl;
632  *error_string = ss.str();
633  }
634  return LOOKUP_ERROR;
635  }
636  }
637 
638  if (common_parent == 0) {
639  create_connectivity_error_string(source_id, target_id, error_string);
640  return CONNECTIVITY_ERROR;
641  }
642 
643  // Loop through the source -> root list until we hit the common parent
644  {
645  std::vector<P_TimeAndFrameID>::iterator it = lct_cache.begin();
646  std::vector<P_TimeAndFrameID>::iterator end = lct_cache.end();
647  for (; it != end; ++it) {
648  if (!it->first.is_zero()) {
649  common_time = std::min(common_time, it->first);
650  }
651 
652  if (it->second == common_parent) {
653  break;
654  }
655  }
656  }
657 
658  if (common_time == fawkes::TIME_MAX) {
659  common_time = fawkes::Time();
660  }
661 
662  time = common_time;
663  return NO_ERROR;
664 }
665 
666 /** Get latest common time of two frames.
667  * @param source_frame source frame ID
668  * @param target_frame frame target frame ID
669  * @param time upon return contains latest common timestamp
670  * @param error_string upon error contains accumulated error message.
671  * @return value from ErrorValues
672  */
673 int
674 Transformer::get_latest_common_time(const std::string &source_frame,
675  const std::string &target_frame,
676  fawkes::Time& time,
677  std::string* error_string) const
678 {
679  std::string mapped_tgt = /*assert_resolved(tf_prefix_,*/ target_frame;
680  std::string mapped_src = /*assert_resolved(tf_prefix_,*/ source_frame;
681 
682  if (!frame_exists(mapped_tgt) || !frame_exists(mapped_src)) {
683  time = fawkes::Time();
684  return LOOKUP_ERROR;
685  }
686 
687  CompactFrameID source_id = lookup_frame_number(mapped_src);
688  CompactFrameID target_id = lookup_frame_number(mapped_tgt);
689  return get_latest_common_time(source_id, target_id, time, error_string);
690 }
691 
692 /** Set a transform.
693  * @param transform transform to set
694  * @param authority authority from which the transform was received.
695  * @return true on success, false otherwise
696  */
697 bool
698 Transformer::set_transform(const StampedTransform &transform, const std::string &authority)
699 {
700  StampedTransform mapped_transform((btTransform)transform, transform.stamp,
701  transform.frame_id, transform.child_frame_id);
702  mapped_transform.child_frame_id = /*assert_resolved(tf_prefix_, */ transform.child_frame_id;
703  mapped_transform.frame_id = /*assert_resolved(tf_prefix_,*/ transform.frame_id;
704 
705 
706  bool error_exists = false;
707  if (mapped_transform.child_frame_id == mapped_transform.frame_id)
708  {
709  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(), mapped_transform.child_frame_id.c_str());
710  error_exists = true;
711  }
712 
713  if (mapped_transform.child_frame_id == "/")//empty frame id will be mapped to "/"
714  {
715  printf("TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str());
716  error_exists = true;
717  }
718 
719  if (mapped_transform.frame_id == "/")//empty parent id will be mapped to "/"
720  {
721  printf("TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"%s\" from authority \"%s\" because frame_id not set", mapped_transform.child_frame_id.c_str(), authority.c_str());
722  error_exists = true;
723  }
724 
725  if (std::isnan(mapped_transform.getOrigin().x()) || std::isnan(mapped_transform.getOrigin().y()) || std::isnan(mapped_transform.getOrigin().z())||
726  std::isnan(mapped_transform.getRotation().x()) || std::isnan(mapped_transform.getRotation().y()) || std::isnan(mapped_transform.getRotation().z()) || std::isnan(mapped_transform.getRotation().w()))
727  {
728  printf("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)",
729  mapped_transform.child_frame_id.c_str(), authority.c_str(),
730  mapped_transform.getOrigin().x(), mapped_transform.getOrigin().y(), mapped_transform.getOrigin().z(),
731  mapped_transform.getRotation().x(), mapped_transform.getRotation().y(), mapped_transform.getRotation().z(), mapped_transform.getRotation().w()
732  );
733  error_exists = true;
734  }
735 
736  if (error_exists)
737  return false;
738 
739  {
741  CompactFrameID frame_number = lookup_or_insert_frame_number(mapped_transform.child_frame_id);
742  TimeCache* frame = get_frame(frame_number);
743  if (frame == NULL) {
744  frames_[frame_number] = new TimeCache(cache_time_);
745  frame = frames_[frame_number];
746  }
747 
748  if (frame->insert_data(TransformStorage(mapped_transform, lookup_or_insert_frame_number(mapped_transform.frame_id), frame_number)))
749  {
750  frame_authority_[frame_number] = authority;
751  } else {
752  //ROS_WARN("TF_OLD_DATA ignoring data from the past for frame %s at time %g according to authority %s\nPossible reasons are listed at ", mapped_transform.child_frame_id.c_str(), mapped_transform.stamp.toSec(), authority.c_str());
753  return false;
754  }
755  }
756 
757  return true;
758 };
759 
760 
761 /** An accessor to get a frame.
762  * This is an internal function which will get the pointer to the
763  * frame associated with the frame id
764  * @param frame_number The frameID of the desired reference frame
765  * @return time cache for given frame number
766  * @exception LookupException thrown if lookup fails
767  */
768 TimeCache *
769 Transformer::get_frame(unsigned int frame_number) const
770 {
771  if (frame_number == 0) /// @todo check larger values too
772  return NULL;
773  else
774  return frames_[frame_number];
775 }
776 
777 
778 /** An accessor to get access to the frame cache.
779  * @param frame_id frame ID
780  * @return time cache for given frame ID
781  * @exception LookupException thrown if lookup fails
782  */
783 const TimeCache *
784 Transformer::get_frame_cache(const std::string &frame_id) const
785 {
786  CompactFrameID frame_number = lookup_frame_number(frame_id);
787  if (frame_number == 0) {
788  throw LookupException("Failed to lookup frame %s", frame_id.c_str());
789  } else {
790  return frames_[frame_number];
791  }
792 }
793 
794 
795 /** String to number for frame lookup.
796  * @param frameid_str frame ID
797  * @return frame number
798  * @exception LookupException if frame ID is unknown
799  */
800 CompactFrameID
801 Transformer::lookup_frame_number(const std::string &frameid_str) const
802 {
803  unsigned int retval = 0;
804  //MutexLocker lock(frame_mutex_);
805  M_StringToCompactFrameID::const_iterator map_it = frameIDs_.find(frameid_str);
806  if (map_it == frameIDs_.end())
807  {
808  throw LookupException("Frame id %s does not exist! Frames (%zu): %s",
809  frameid_str.c_str(), frameIDs_.size(), "" /*allFramesAsString()*/);
810  }
811  else
812  retval = map_it->second;
813  return retval;
814 };
815 
816 
817 /** String to number for frame lookup with dynamic allocation of new
818  * frames.
819  * @param frameid_str frame ID
820  * @return frame number, if none existed for the given frame ID a new
821  * one is created
822  */
823 CompactFrameID
824 Transformer::lookup_or_insert_frame_number(const std::string &frameid_str)
825 {
826  unsigned int retval = 0;
827  //MutexLocker lock(frame_mutex_);
828  M_StringToCompactFrameID::iterator map_it = frameIDs_.find(frameid_str);
829  if (map_it == frameIDs_.end())
830  {
831  retval = frames_.size();
832  frameIDs_[frameid_str] = retval;
833  frames_.push_back( new TimeCache(cache_time_));
834  frameIDs_reverse.push_back(frameid_str);
835  }
836  else
837  retval = frameIDs_[frameid_str];
838  return retval;
839 };
840 
841 /** Get frame ID given its number.
842  * @param frame_num frame number
843  * @return frame ID string
844  * @exception LookupException thrown if number invalid
845  */
846 std::string
847 Transformer::lookup_frame_string(unsigned int frame_num) const
848 {
849  if (frame_num >= frameIDs_reverse.size()) {
850  throw LookupException("Reverse lookup of frame id %u failed!", frame_num);
851  }
852  else return frameIDs_reverse[frame_num];
853 }
854 
855 
856 /** Lookup transform.
857  * @param target_frame target frame ID
858  * @param source_frame source frame ID
859  * @param time time for which to get the transform, set to (0,0) to get latest
860  * common time frame
861  * @param transform upon return contains the transform
862  * @exception ConnectivityException thrown if no connection between
863  * the source and target frame could be found in the tree.
864  * @exception ExtrapolationException returning a value would have
865  * required extrapolation beyond current limits.
866  * @exception LookupException at least one of the two given frames is
867  * unknown
868  */
869 void Transformer::lookup_transform(const std::string& target_frame,
870  const std::string& source_frame,
871  const fawkes::Time& time,
872  StampedTransform& transform) const
873 {
874  if (! enabled_) {
875  throw DisabledException("Transformer has been disabled");
876  }
877 
878  std::string mapped_tgt = /*assert_resolved(tf_prefix_,*/ target_frame;
879  std::string mapped_src = /*assert_resolved(tf_prefix_,*/ source_frame;
880 
881  if (mapped_tgt == mapped_src) {
882  transform.setIdentity();
883  transform.child_frame_id = mapped_src;
884  transform.frame_id = mapped_tgt;
885  transform.stamp = fawkes::Time();
886  return;
887  }
888 
890 
891  CompactFrameID target_id = lookup_frame_number(mapped_tgt);
892  CompactFrameID source_id = lookup_frame_number(mapped_src);
893 
894  std::string error_string;
895  TransformAccum accum;
896  int rv = walk_to_top_parent(accum, time, target_id, source_id, &error_string);
897  if (rv != NO_ERROR)
898  {
899  switch (rv)
900  {
901  case CONNECTIVITY_ERROR:
902  throw ConnectivityException("Connectivity: %s", error_string.c_str());
903  case EXTRAPOLATION_ERROR:
904  throw ExtrapolationException("Extrapolation: %s", error_string.c_str());
905  case LOOKUP_ERROR:
906  throw LookupException("Lookup: %s", error_string.c_str());
907  default:
908  throw Exception("lookup_transform: unknown error code: %d", rv);
909  break;
910  }
911  }
912 
913  transform.setOrigin(accum.result_vec);
914  transform.setRotation(accum.result_quat);
915  transform.child_frame_id = mapped_src;
916  transform.frame_id = mapped_tgt;
917  transform.stamp = accum.time;
918 }
919 
920 
921 /** Lookup transform at latest common time.
922  * @param target_frame target frame ID
923  * @param source_frame source frame ID
924  * @param transform upon return contains the transform
925  * @exception ConnectivityException thrown if no connection between
926  * the source and target frame could be found in the tree.
927  * @exception ExtrapolationException returning a value would have
928  * required extrapolation beyond current limits.
929  * @exception LookupException at least one of the two given frames is
930  * unknown
931  */
932 void Transformer::lookup_transform(const std::string& target_frame,
933  const std::string& source_frame,
934  StampedTransform& transform) const
935 {
936  lookup_transform(target_frame, source_frame, fawkes::Time(0,0), transform);
937 }
938 
939 
940 /** Lookup transform assuming a fixed frame.
941  * This will lookup a transformation from source to target, assuming
942  * that there is a fixed frame, by first finding the transform of the
943  * source frame in the fixed frame, and then a transformation from the
944  * fixed frame to the target frame.
945  * @param target_frame target frame ID
946  * @param target_time time for the target frame
947  * @param source_frame source frame ID
948  * @param source_time time in the source frame
949  * @param fixed_frame ID of fixed frame
950  * @param transform upon return contains the transform
951  * @exception ConnectivityException thrown if no connection between
952  * the source and target frame could be found in the tree.
953  * @exception ExtrapolationException returning a value would have
954  * required extrapolation beyond current limits.
955  * @exception LookupException at least one of the two given frames is
956  * unknown
957  */
958 void
959 Transformer::lookup_transform(const std::string& target_frame,
960  const fawkes::Time& target_time,
961  const std::string& source_frame,
962  const fawkes::Time& source_time,
963  const std::string& fixed_frame,
964  StampedTransform& transform) const
965 {
966  StampedTransform temp1, temp2;
967  lookup_transform(fixed_frame, source_frame, source_time, temp1);
968  lookup_transform(target_frame, fixed_frame, target_time, temp2);
969  transform.set_data(temp2 * temp1);
970  transform.stamp = temp2.stamp;
971  transform.frame_id = target_frame;
972  transform.child_frame_id = source_frame;
973 }
974 
975 /** Test if a transform is possible.
976  * @param target_frame The frame into which to transform
977  * @param source_frame The frame from which to transform
978  * @param time The time at which to transform
979  * @return true if the transformation can be calculated, false otherwise
980  */
981 bool
982 Transformer::can_transform(const std::string& target_frame,
983  const std::string& source_frame,
984  const fawkes::Time& time) const
985 {
986  std::string mapped_tgt = /*assert_resolved(tf_prefix_,*/ target_frame;
987  std::string mapped_src = /*assert_resolved(tf_prefix_,*/ source_frame;
988 
989  if (mapped_tgt == mapped_src) return true;
990 
991  if (!frame_exists(mapped_tgt) || !frame_exists(mapped_src)) return false;
992 
994  CompactFrameID target_id = lookup_frame_number(mapped_tgt);
995  CompactFrameID source_id = lookup_frame_number(mapped_src);
996 
997  return can_transform_no_lock(target_id, source_id, time);
998 }
999 
1000 
1001 /** Test if a transform is possible.
1002  * @param target_frame The frame into which to transform
1003  * @param target_time The time into which to transform
1004  * @param source_frame The frame from which to transform
1005  * @param source_time The time from which to transform
1006  * @param fixed_frame The frame in which to treat the transform as
1007  * constant in time
1008  * @return true if the transformation can be calculated, false otherwise
1009  */
1010 bool
1011 Transformer::can_transform(const std::string& target_frame,
1012  const fawkes::Time& target_time,
1013  const std::string& source_frame,
1014  const fawkes::Time& source_time,
1015  const std::string& fixed_frame) const
1016 {
1017  return
1018  can_transform(target_frame, fixed_frame, target_time) &&
1019  can_transform(fixed_frame, source_frame, source_time);
1020 }
1021 
1022 
1023 /** Test if a transform is possible.
1024  * Internal check that does not lock the frame mutex.
1025  * @param target_id The frame number into which to transform
1026  * @param source_id The frame number from which to transform
1027  * @param time The time at which to transform
1028  * @return true if the transformation can be calculated, false otherwise
1029  */
1030 bool
1031 Transformer::can_transform_no_lock(CompactFrameID target_id,
1032  CompactFrameID source_id,
1033  const fawkes::Time& time) const
1034 {
1035  if (target_id == 0 || source_id == 0) return false;
1036 
1037  CanTransformAccum accum;
1038  if (walk_to_top_parent(accum, time, target_id, source_id, NULL) == NO_ERROR)
1039  return true;
1040  else
1041  return false;
1042 }
1043 
1044 
1045 /** Transform a stamped Quaternion into the target frame.
1046  * This transforms the quaternion relative to the frame set in the
1047  * stamped quaternion into the target frame.
1048  * @param target_frame frame into which to transform
1049  * @param stamped_in stamped quaternion, defines source frame and time
1050  * @param stamped_out stamped output quaternion in target_frame
1051  * @exception ConnectivityException thrown if no connection between
1052  * the source and target frame could be found in the tree.
1053  * @exception ExtrapolationException returning a value would have
1054  * required extrapolation beyond current limits.
1055  * @exception LookupException at least one of the two given frames is
1056  * unknown
1057  * @exception InvalidArgument thrown if the Quaternion is invalid, most
1058  * likely an uninitialized Quaternion (0,0,0,0).
1059  */
1060 void
1061 Transformer::transform_quaternion(const std::string& target_frame,
1062  const Stamped<Quaternion>& stamped_in,
1063  Stamped<Quaternion>& stamped_out) const
1064 {
1065  assert_quaternion_valid(stamped_in);
1066 
1067  StampedTransform transform;
1068  lookup_transform(target_frame, stamped_in.frame_id,
1069  stamped_in.stamp, transform);
1070 
1071  stamped_out.set_data(transform * stamped_in);
1072  stamped_out.stamp = transform.stamp;
1073  stamped_out.frame_id = target_frame;
1074 }
1075 
1076 /** Transform a stamped vector into the target frame.
1077  * This transforms the vector given relative to the frame set in the
1078  * stamped vector into the target frame.
1079  * @param target_frame frame into which to transform
1080  * @param stamped_in stamped vector, defines source frame and time
1081  * @param stamped_out stamped output vector in target_frame
1082  * @exception ConnectivityException thrown if no connection between
1083  * the source and target frame could be found in the tree.
1084  * @exception ExtrapolationException returning a value would have
1085  * required extrapolation beyond current limits.
1086  * @exception LookupException at least one of the two given frames is
1087  * unknown
1088  */
1089 void
1090 Transformer::transform_vector(const std::string& target_frame,
1091  const Stamped<Vector3>& stamped_in,
1092  Stamped<Vector3>& stamped_out) const
1093 {
1094  StampedTransform transform;
1095  lookup_transform(target_frame, stamped_in.frame_id,
1096  stamped_in.stamp, transform);
1097 
1098  // may not be most efficient
1099  Vector3 end = stamped_in;
1100  Vector3 origin = Vector3(0,0,0);
1101  Vector3 output = (transform * end) - (transform * origin);
1102  stamped_out.set_data(output);
1103 
1104  stamped_out.stamp = transform.stamp;
1105  stamped_out.frame_id = target_frame;
1106 }
1107 
1108 /** Transform a stamped point into the target frame.
1109  * This transforms the point given relative to the frame set in the
1110  * stamped point into the target frame.
1111  * @param target_frame frame into which to transform
1112  * @param stamped_in stamped point, defines source frame and time
1113  * @param stamped_out stamped output point in target_frame
1114  * @exception ConnectivityException thrown if no connection between
1115  * the source and target frame could be found in the tree.
1116  * @exception ExtrapolationException returning a value would have
1117  * required extrapolation beyond current limits.
1118  * @exception LookupException at least one of the two given frames is
1119  * unknown
1120  */
1121 void
1122 Transformer::transform_point(const std::string& target_frame,
1123  const Stamped<Point>& stamped_in,
1124  Stamped<Point>& stamped_out) const
1125 {
1126  StampedTransform transform;
1127  lookup_transform(target_frame, stamped_in.frame_id,
1128  stamped_in.stamp, transform);
1129 
1130  stamped_out.set_data(transform * stamped_in);
1131  stamped_out.stamp = transform.stamp;
1132  stamped_out.frame_id = target_frame;
1133 
1134 }
1135 
1136 /** Transform a stamped pose into the target frame.
1137  * This transforms the pose given relative to the frame set in the
1138  * stamped vector into the target frame.
1139  * @param target_frame frame into which to transform
1140  * @param stamped_in stamped pose, defines source frame and time
1141  * @param stamped_out stamped output pose in target_frame
1142  * @exception ConnectivityException thrown if no connection between
1143  * the source and target frame could be found in the tree.
1144  * @exception ExtrapolationException returning a value would have
1145  * required extrapolation beyond current limits.
1146  * @exception LookupException at least one of the two given frames is
1147  * unknown
1148  */
1149 void
1150 Transformer::transform_pose(const std::string& target_frame,
1151  const Stamped<Pose>& stamped_in,
1152  Stamped<Pose>& stamped_out) const
1153 {
1154  StampedTransform transform;
1155  lookup_transform(target_frame, stamped_in.frame_id,
1156  stamped_in.stamp, transform);
1157 
1158  stamped_out.set_data(transform * stamped_in);
1159  stamped_out.stamp = transform.stamp;
1160  stamped_out.frame_id = target_frame;
1161 }
1162 
1163 
1164 /** Transform a stamped Quaternion into the target frame assuming a fixed frame.
1165  * This transforms the quaternion relative to the frame set in the
1166  * stamped quaternion into the target frame.
1167  * This will transform the quaternion from source to target, assuming
1168  * that there is a fixed frame, by first finding the transform of the
1169  * source frame in the fixed frame, and then a transformation from the
1170  * fixed frame into the target frame.
1171  * @param target_frame frame into which to transform
1172  * @param target_time desired time in the target frame
1173  * @param stamped_in stamped quaternion, defines source frame and time
1174  * @param fixed_frame ID of fixed frame
1175  * @param stamped_out stamped output quaternion in target_frame
1176  * @exception ConnectivityException thrown if no connection between
1177  * the source and target frame could be found in the tree.
1178  * @exception ExtrapolationException returning a value would have
1179  * required extrapolation beyond current limits.
1180  * @exception LookupException at least one of the two given frames is
1181  * unknown
1182  * @exception InvalidArgument thrown if the Quaternion is invalid, most
1183  * likely an uninitialized Quaternion (0,0,0,0).
1184  */
1185 void
1186 Transformer::transform_quaternion(const std::string& target_frame,
1187  const fawkes::Time& target_time,
1188  const Stamped<Quaternion>& stamped_in,
1189  const std::string& fixed_frame,
1190  Stamped<Quaternion>& stamped_out) const
1191 {
1192  assert_quaternion_valid(stamped_in);
1193  StampedTransform transform;
1194  lookup_transform(target_frame, target_time,
1195  stamped_in.frame_id, stamped_in.stamp,
1196  fixed_frame, transform);
1197 
1198  stamped_out.set_data(transform * stamped_in);
1199  stamped_out.stamp = transform.stamp;
1200  stamped_out.frame_id = target_frame;
1201 }
1202 
1203 /** Transform a stamped vector into the target frame assuming a fixed frame.
1204  * This transforms the vector relative to the frame set in the
1205  * stamped quaternion into the target frame.
1206  * This will transform the vector from source to target, assuming
1207  * that there is a fixed frame, by first finding the transform of the
1208  * source frame in the fixed frame, and then a transformation from the
1209  * fixed frame into the target frame.
1210  * @param target_frame frame into which to transform
1211  * @param target_time desired time in the target frame
1212  * @param stamped_in stamped vector, defines source frame and time
1213  * @param fixed_frame ID of fixed frame
1214  * @param stamped_out stamped output vector in target_frame
1215  * @exception ConnectivityException thrown if no connection between
1216  * the source and target frame could be found in the tree.
1217  * @exception ExtrapolationException returning a value would have
1218  * required extrapolation beyond current limits.
1219  * @exception LookupException at least one of the two given frames is
1220  * unknown
1221  */
1222 void
1223 Transformer::transform_vector(const std::string& target_frame,
1224  const fawkes::Time& target_time,
1225  const Stamped<Vector3>& stamped_in,
1226  const std::string& fixed_frame,
1227  Stamped<Vector3>& stamped_out) const
1228 {
1229  StampedTransform transform;
1230  lookup_transform(target_frame, target_time,
1231  stamped_in.frame_id,stamped_in.stamp,
1232  fixed_frame, transform);
1233 
1234  // may not be most efficient
1235  Vector3 end = stamped_in;
1236  Vector3 origin(0,0,0);
1237  Vector3 output = (transform * end) - (transform * origin);
1238  stamped_out.set_data( output);
1239 
1240  stamped_out.stamp = transform.stamp;
1241  stamped_out.frame_id = target_frame;
1242 
1243 }
1244 
1245 /** Transform a stamped point into the target frame assuming a fixed frame.
1246  * This transforms the point relative to the frame set in the
1247  * stamped quaternion into the target frame.
1248  * This will transform the point from source to target, assuming
1249  * that there is a fixed frame, by first finding the transform of the
1250  * source frame in the fixed frame, and then a transformation from the
1251  * fixed frame into the target frame.
1252  * @param target_frame frame into which to transform
1253  * @param target_time desired time in the target frame
1254  * @param stamped_in stamped point, defines source frame and time
1255  * @param fixed_frame ID of fixed frame
1256  * @param stamped_out stamped output point in target_frame
1257  * @exception ConnectivityException thrown if no connection between
1258  * the source and target frame could be found in the tree.
1259  * @exception ExtrapolationException returning a value would have
1260  * required extrapolation beyond current limits.
1261  * @exception LookupException at least one of the two given frames is
1262  * unknown
1263  */
1264 void
1265 Transformer::transform_point(const std::string& target_frame,
1266  const fawkes::Time& target_time,
1267  const Stamped<Point>& stamped_in,
1268  const std::string& fixed_frame,
1269  Stamped<Point>& stamped_out) const
1270 {
1271  StampedTransform transform;
1272  lookup_transform(target_frame, target_time,
1273  stamped_in.frame_id, stamped_in.stamp,
1274  fixed_frame, transform);
1275 
1276  stamped_out.set_data(transform * stamped_in);
1277  stamped_out.stamp = transform.stamp;
1278  stamped_out.frame_id = target_frame;
1279 }
1280 
1281 /** Transform a stamped pose into the target frame assuming a fixed frame.
1282  * This transforms the pose relative to the frame set in the
1283  * stamped quaternion into the target frame.
1284  * This will transform the pose from source to target, assuming
1285  * that there is a fixed frame, by first finding the transform of the
1286  * source frame in the fixed frame, and then a transformation from the
1287  * fixed frame into the target frame.
1288  * @param target_frame frame into which to transform
1289  * @param target_time desired time in the target frame
1290  * @param stamped_in stamped pose, defines source frame and time
1291  * @param fixed_frame ID of fixed frame
1292  * @param stamped_out stamped output pose in target_frame
1293  * @exception ConnectivityException thrown if no connection between
1294  * the source and target frame could be found in the tree.
1295  * @exception ExtrapolationException returning a value would have
1296  * required extrapolation beyond current limits.
1297  * @exception LookupException at least one of the two given frames is
1298  * unknown
1299  */
1300 void
1301 Transformer::transform_pose(const std::string& target_frame,
1302  const fawkes::Time& target_time,
1303  const Stamped<Pose>& stamped_in,
1304  const std::string& fixed_frame,
1305  Stamped<Pose>& stamped_out) const
1306 {
1307  StampedTransform transform;
1308  lookup_transform(target_frame, target_time,
1309  stamped_in.frame_id, stamped_in.stamp,
1310  fixed_frame, transform);
1311 
1312  stamped_out.set_data(transform * stamped_in);
1313  stamped_out.stamp = transform.stamp;
1314  stamped_out.frame_id = target_frame;
1315 }
1316 
1317 } // end namespace tf
1318 } // end namespace fawkes
bool insert_data(const TransformStorage &new_data)
Insert data.
Definition: time_cache.cpp:323
std::string frame_id
The frame_id associated this data.
Definition: types.h:132
bool get_data(fawkes::Time time, TransformStorage &data_out, std::string *error_str=0)
Get data.
Definition: time_cache.cpp:276
CompactFrameID get_parent(fawkes::Time time, std::string *error_str)
Get parent frame number.
Definition: time_cache.cpp:304
static const unsigned int MAX_GRAPH_DEPTH
Maximum number of times to recurse before assuming the tree has a loop.
Definition: transformer.h:82
std::map< CompactFrameID, std::string > frame_authority_
Map to lookup the most recent authority for a given frame.
Definition: transformer.h:179
void set_data(const tf::Transform &input)
Set the inherited Transform data.
Definition: types.h:122
bool set_transform(const StampedTransform &transform, const std::string &authority="default_authority")
Set a transform.
CompactFrameID gather(TimeCache *cache, fawkes::Time time, std::string *error_string)
Gather frame number.
Fawkes library namespace.
Storage for transforms and their parent.
Definition: time_cache.h:77
Mutex locking helper.
Definition: mutex_locker.h:33
CompactFrameID id
Frame number to look for.
std::vector< TimeCache * > frames_
The pointers to potential frames that the tree can be made of.
Definition: transformer.h:183
TimeCache * get_frame(unsigned int frame_number) const
An accessor to get a frame.
btVector3 source_to_top_vec
Source to top accumulated vector.
float max_extrapolation_distance_
whether or not to allow extrapolation
Definition: transformer.h:192
fawkes::Time stamp
Timestamp of this transform.
Definition: types.h:96
void set_enabled(bool enabled)
Set transformer enabled or disabled.
A class for handling time.
Definition: time.h:91
Mutex * frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
Definition: transformer.h:186
Request would have required extrapolation beyond current limits.
Definition: exceptions.h:52
No connection between two frames in tree.
Definition: exceptions.h:40
btQuaternion target_to_top_quat
Target to top accumulated quaternion.
Accumulator to test for transformability.
void transform_pose(const std::string &target_frame, const Stamped< Pose > &stamped_in, Stamped< Pose > &stamped_out) const
Transform a stamped pose into the target frame.
TransformStorage st
Transform storage to use.
void accum(bool source)
Accumulate.
Time based transform cache.
Definition: time_cache.h:104
fawkes::Time stamp
The timestamp associated with this data.
Definition: types.h:131
void transform_point(const std::string &target_frame, const Stamped< Point > &stamped_in, Stamped< Point > &stamped_out) const
Transform a stamped point into the target frame.
bool enabled_
Flag to mark the transformer as disabled.
Definition: transformer.h:167
std::vector< std::string > frameIDs_reverse
Map from CompactFrameID frame_id_numbers to string for debugging and output.
Definition: transformer.h:177
TimeAndFrameIDFrameComparator(CompactFrameID id)
Constructor.
static const float DEFAULT_MAX_EXTRAPOLATION_DISTANCE
The default amount of time to extrapolate.
Definition: transformer.h:83
int get_latest_common_time(const std::string &source_frame, const std::string &target_frame, fawkes::Time &time, std::string *error_string=0) const
Get latest common time of two frames.
bool can_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time) const
Test if a transform is possible.
Base class for exceptions in Fawkes.
Definition: exception.h:36
fawkes::Time time
time stamp
virtual ~Transformer(void)
Destructor.
btQuaternion result_quat
After finalize contains result quaternion.
CompactFrameID gather(TimeCache *cache, fawkes::Time time, std::string *error_string)
Gather frame number.
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
void transform_vector(const std::string &target_frame, const Stamped< Vector3 > &stamped_in, Stamped< Vector3 > &stamped_out) const
Transform a stamped vector into the target frame.
Accumulator to accumulate transforms between two frames.
CompactFrameID lookup_frame_number(const std::string &frameid_str) const
String to number for frame lookup.
std::string child_frame_id
Frame ID of child frame, e.g.
Definition: types.h:101
void finalize(WalkEnding end, fawkes::Time _time)
Finalize accumulation.
void finalize(WalkEnding end, fawkes::Time _time)
Finalize accumulation.
btVector3 target_to_top_vec
Target to top accumulated vector.
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:129
btVector3 result_vec
After finalize contains result vector.
CompactFrameID lookup_or_insert_frame_number(const std::string &frameid_str)
String to number for frame lookup with dynamic allocation of new frames.
bool operator()(const P_TimeAndFrameID &rhs) const
Application operator.
TransformAccum()
Constructor.
std::string lookup_frame_string(unsigned int frame_id_num) const
Get frame ID given its number.
const Time TIME_MAX
Instance of Time denoting the maximum value possible.
Definition: time.cpp:47
The requested feature is disabled.
Definition: exceptions.h:64
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
std::string frame_id
Parent/reference frame ID.
Definition: types.h:98
void transform_quaternion(const std::string &target_frame, const Stamped< Quaternion > &stamped_in, Stamped< Quaternion > &stamped_out) const
Transform a stamped Quaternion into the target frame.
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
M_StringToCompactFrameID frameIDs_
Map from frame IDs to frame numbers.
Definition: transformer.h:175
btQuaternion source_to_top_quat
Source to top accumulated quaternion.
Time and frame ID comparator class.
A frame could not be looked up.
Definition: exceptions.h:46
Mutex mutual exclusion lock.
Definition: mutex.h:32
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
const TimeCache * get_frame_cache(const std::string &frame_id) const
An accessor to get access to the frame cache.
void clear()
Clear cached transforms.
void set_data(const T &input)
Set the data element.
Definition: types.h:151
TransformStorage st
Transform storage.
float cache_time_
How long to cache transform history.
Definition: transformer.h:189
void accum(bool source)
Accumulate.