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 #include <tf/utils.h>
56 
57 #include <core/threading/mutex_locker.h>
58 #include <core/macros.h>
59 #include <iostream>
60 #include <sstream>
61 #include <algorithm>
62 
63 namespace fawkes {
64  namespace tf {
65 #if 0 /* just to make Emacs auto-indent happy */
66  }
67 }
68 #endif
69 
70 /** @class Transformer <tf/transformer.h>
71  * Coordinate transforms between any two frames in a system.
72  *
73  * This class provides a simple interface to allow recording and
74  * lookup of relationships between arbitrary frames of the system.
75  *
76  * TF assumes that there is a tree of coordinate frame transforms
77  * which define the relationship between all coordinate frames. For
78  * example your typical robot would have a transform from global to
79  * real world. And then from base to hand, and from base to head.
80  * But Base to Hand really is composed of base to shoulder to elbow to
81  * wrist to hand. TF is designed to take care of all the intermediate
82  * steps for you.
83  *
84  * Internal Representation TF will store frames with the parameters
85  * necessary for generating the transform into that frame from it's
86  * parent and a reference to the parent frame. Frames are designated
87  * using an std::string 0 is a frame without a parent (the top of a
88  * tree) The positions of frames over time must be pushed in.
89  *
90  * All function calls which pass frame ids can potentially throw the
91  * exception fawkes::tf::LookupException
92  *
93  *
94  * @fn bool Transformer::is_enabled() const
95  * Check if transformer is enabled.
96  * @return true if enabled, false otherwise
97  *
98  */
99 
100 /** Constructor.
101  * @param cache_time time in seconds to cache incoming transforms
102  */
103 Transformer::Transformer(float cache_time)
104  : BufferCore(cache_time),
105  enabled_(true)
106 {
107 }
108 
109 
110 /** Destructor. */
112 {
113 };
114 
115 
116 /** Set enabled status of transformer.
117  * @param enabled true to enable, false to disable
118  */
119 void
121 {
122  enabled_ = enabled;
123 }
124 
125 /** Check if transformer is enabled.
126  * @return true if enabled, false otherwise
127  */
128 bool
130 {
131  return enabled_;
132 }
133 
134 /** Get cache time.
135  * @return time in seconds caches will date back.
136  */
137 float
139 {
140  return cache_time_;
141 }
142 
143 /** Lock transformer.
144  * No new transforms can be added and no lookups can be performed
145  * while the lock is held.
146  */
147 void
149 {
150  frame_mutex_.lock();
151 }
152 
153 
154 /** Try to acquire lock.
155  * @return true if lock has been acquired, alse otherwise
156  */
157 bool
159 {
160  return frame_mutex_.try_lock();
161 }
162 
163 
164 /** Unlock.
165  * Release currently held lock.
166  */
167 void
169 {
170  frame_mutex_.unlock();
171 }
172 
173 
174 /** Check if frame exists.
175  * @param frame_id_str frame ID
176  * @result true if frame exists, false otherwise
177  */
178 bool
179 Transformer::frame_exists(const std::string& frame_id_str) const
180 {
181  std::unique_lock<std::mutex> lock(frame_mutex_);
182 
183  return (frameIDs_.count(frame_id_str) > 0);
184 }
185 
186 
187 /** Get cache for specific frame.
188  * @param frame_id ID of frame
189  * @return pointer to time cache for frame
190  */
191 TimeCacheInterfacePtr
192 Transformer::get_frame_cache(const std::string& frame_id) const
193 {
194  return get_frame(lookup_frame_number(frame_id));
195 }
196 
197 /** Get all currently existing caches.
198  * @return vector of pointer to time caches
199  */
200 std::vector<TimeCacheInterfacePtr>
202 {
203  return frames_;
204 }
205 
206 
207 /** Get mappings from frame ID to names.
208  * @return vector of mappings from frame IDs to names
209  */
210 std::vector<std::string>
212 {
213  return frameIDs_reverse;
214 }
215 
216 
217 /** Test if a transform is possible.
218  * @param target_frame The frame into which to transform
219  * @param source_frame The frame from which to transform
220  * @param time The time at which to transform
221  * @param error_msg A pointer to a string which will be filled with
222  * why the transform failed, if not NULL
223  * @return true if the transformation can be calculated, false otherwise
224  */
225 bool
226 Transformer::can_transform(const std::string& target_frame,
227  const std::string& source_frame,
228  const fawkes::Time& time,
229  std::string* error_msg) const
230 {
231  if (likely(enabled_)) {
232  std::string stripped_target = strip_slash(target_frame);
233  std::string stripped_source = strip_slash(source_frame);
234  return BufferCore::can_transform(stripped_target, stripped_source, time, error_msg);
235  }
236  else return false;
237 }
238 
239 /** Test if a transform is possible.
240  * @param target_frame The frame into which to transform
241  * @param target_time The time into which to transform
242  * @param source_frame The frame from which to transform
243  * @param source_time The time from which to transform
244  * @param fixed_frame The frame in which to treat the transform as
245  * constant in time
246  * @param error_msg A pointer to a string which will be filled with
247  * why the transform failed, if not NULL
248  * @return true if the transformation can be calculated, false otherwise
249  */
250 bool
251 Transformer::can_transform(const std::string& target_frame,
252  const fawkes::Time& target_time,
253  const std::string& source_frame,
254  const fawkes::Time& source_time,
255  const std::string& fixed_frame,
256  std::string* error_msg) const
257 {
258  if (likely(enabled_)) {
259  std::string stripped_target = strip_slash(target_frame);
260  std::string stripped_source = strip_slash(source_frame);
261  return BufferCore::can_transform(stripped_target, target_time,
262  stripped_source, source_time,
263  fixed_frame, error_msg);
264  }
265  else return false;
266 }
267 
268 /** Lookup transform.
269  * @param target_frame target frame ID
270  * @param source_frame source frame ID
271  * @param time time for which to get the transform, set to (0,0) to get latest
272  * common time frame
273  * @param transform upon return contains the transform
274  * @exception ConnectivityException thrown if no connection between
275  * the source and target frame could be found in the tree.
276  * @exception ExtrapolationException returning a value would have
277  * required extrapolation beyond current limits.
278  * @exception LookupException at least one of the two given frames is
279  * unknown
280  */
281 void
282 Transformer::lookup_transform(const std::string& target_frame,
283  const std::string& source_frame,
284  const fawkes::Time& time,
285  StampedTransform& transform) const
286 {
287  if (! enabled_) {
288  throw DisabledException("Transformer has been disabled");
289  }
290 
291  std::string stripped_target = strip_slash(target_frame);
292  std::string stripped_source = strip_slash(source_frame);
293 
294  BufferCore::lookup_transform(stripped_target, stripped_source, time, transform);
295 }
296 
297 /** Lookup transform assuming a fixed frame.
298  * This will lookup a transformation from source to target, assuming
299  * that there is a fixed frame, by first finding the transform of the
300  * source frame in the fixed frame, and then a transformation from the
301  * fixed frame to the target frame.
302  * @param target_frame target frame ID
303  * @param target_time time for the target frame
304  * @param source_frame source frame ID
305  * @param source_time time in the source frame
306  * @param fixed_frame ID of fixed frame
307  * @param transform upon return contains the transform
308  * @exception ConnectivityException thrown if no connection between
309  * the source and target frame could be found in the tree.
310  * @exception ExtrapolationException returning a value would have
311  * required extrapolation beyond current limits.
312  * @exception LookupException at least one of the two given frames is
313  * unknown
314  */
315 void
316 Transformer::lookup_transform(const std::string& target_frame,
317  const fawkes::Time& target_time,
318  const std::string& source_frame,
319  const fawkes::Time& source_time,
320  const std::string& fixed_frame,
321  StampedTransform& transform) const
322 {
323  if (! enabled_) {
324  throw DisabledException("Transformer has been disabled");
325  }
326  std::string stripped_target = strip_slash(target_frame);
327  std::string stripped_source = strip_slash(source_frame);
328 
329  BufferCore::lookup_transform(stripped_target, target_time,
330  stripped_source, source_time,
331  fixed_frame, transform);
332 }
333 
334 /** Lookup transform at latest common time.
335  * @param target_frame target frame ID
336  * @param source_frame source frame ID
337  * @param transform upon return contains the transform
338  * @exception ConnectivityException thrown if no connection between
339  * the source and target frame could be found in the tree.
340  * @exception ExtrapolationException returning a value would have
341  * required extrapolation beyond current limits.
342  * @exception LookupException at least one of the two given frames is
343  * unknown
344  */
345 void Transformer::lookup_transform(const std::string& target_frame,
346  const std::string& source_frame,
347  StampedTransform& transform) const
348 {
349  lookup_transform(target_frame, source_frame, fawkes::Time(0,0), transform);
350 }
351 
352 
353 /** Transform a stamped Quaternion into the target frame.
354  * This transforms the quaternion relative to the frame set in the
355  * stamped quaternion into the target frame.
356  * @param target_frame frame into which to transform
357  * @param stamped_in stamped quaternion, defines source frame and time
358  * @param stamped_out stamped output quaternion in target_frame
359  * @exception ConnectivityException thrown if no connection between
360  * the source and target frame could be found in the tree.
361  * @exception ExtrapolationException returning a value would have
362  * required extrapolation beyond current limits.
363  * @exception LookupException at least one of the two given frames is
364  * unknown
365  * @exception InvalidArgument thrown if the Quaternion is invalid, most
366  * likely an uninitialized Quaternion (0,0,0,0).
367  */
368 void
369 Transformer::transform_quaternion(const std::string& target_frame,
370  const Stamped<Quaternion>& stamped_in,
371  Stamped<Quaternion>& stamped_out) const
372 {
373  assert_quaternion_valid(stamped_in);
374 
375  StampedTransform transform;
376  lookup_transform(target_frame, stamped_in.frame_id,
377  stamped_in.stamp, transform);
378 
379  stamped_out.set_data(transform * stamped_in);
380  stamped_out.stamp = transform.stamp;
381  stamped_out.frame_id = target_frame;
382 }
383 
384 /** Transform a stamped vector into the target frame.
385  * This transforms the vector given relative to the frame set in the
386  * stamped vector into the target frame.
387  * @param target_frame frame into which to transform
388  * @param stamped_in stamped vector, defines source frame and time
389  * @param stamped_out stamped output vector in target_frame
390  * @exception ConnectivityException thrown if no connection between
391  * the source and target frame could be found in the tree.
392  * @exception ExtrapolationException returning a value would have
393  * required extrapolation beyond current limits.
394  * @exception LookupException at least one of the two given frames is
395  * unknown
396  */
397 void
398 Transformer::transform_vector(const std::string& target_frame,
399  const Stamped<Vector3>& stamped_in,
400  Stamped<Vector3>& stamped_out) const
401 {
402  StampedTransform transform;
403  lookup_transform(target_frame, stamped_in.frame_id,
404  stamped_in.stamp, transform);
405 
406  // may not be most efficient
407  Vector3 end = stamped_in;
408  Vector3 origin = Vector3(0,0,0);
409  Vector3 output = (transform * end) - (transform * origin);
410  stamped_out.set_data(output);
411 
412  stamped_out.stamp = transform.stamp;
413  stamped_out.frame_id = target_frame;
414 }
415 
416 /** Transform a stamped point into the target frame.
417  * This transforms the point given relative to the frame set in the
418  * stamped point into the target frame.
419  * @param target_frame frame into which to transform
420  * @param stamped_in stamped point, defines source frame and time
421  * @param stamped_out stamped output point in target_frame
422  * @exception ConnectivityException thrown if no connection between
423  * the source and target frame could be found in the tree.
424  * @exception ExtrapolationException returning a value would have
425  * required extrapolation beyond current limits.
426  * @exception LookupException at least one of the two given frames is
427  * unknown
428  */
429 void
430 Transformer::transform_point(const std::string& target_frame,
431  const Stamped<Point>& stamped_in,
432  Stamped<Point>& stamped_out) const
433 {
434  StampedTransform transform;
435  lookup_transform(target_frame, stamped_in.frame_id,
436  stamped_in.stamp, transform);
437 
438  stamped_out.set_data(transform * stamped_in);
439  stamped_out.stamp = transform.stamp;
440  stamped_out.frame_id = target_frame;
441 
442 }
443 
444 /** Transform a stamped pose into the target frame.
445  * This transforms the pose given relative to the frame set in the
446  * stamped vector into the target frame.
447  * @param target_frame frame into which to transform
448  * @param stamped_in stamped pose, defines source frame and time
449  * @param stamped_out stamped output pose in target_frame
450  * @exception ConnectivityException thrown if no connection between
451  * the source and target frame could be found in the tree.
452  * @exception ExtrapolationException returning a value would have
453  * required extrapolation beyond current limits.
454  * @exception LookupException at least one of the two given frames is
455  * unknown
456  */
457 void
458 Transformer::transform_pose(const std::string& target_frame,
459  const Stamped<Pose>& stamped_in,
460  Stamped<Pose>& stamped_out) const
461 {
462  StampedTransform transform;
463  lookup_transform(target_frame, stamped_in.frame_id,
464  stamped_in.stamp, transform);
465 
466  stamped_out.set_data(transform * stamped_in);
467  stamped_out.stamp = transform.stamp;
468  stamped_out.frame_id = target_frame;
469 }
470 
471 
472 /** Transform ident pose from one frame to another.
473  * This utility method can be used to transform the ident pose,
474  * i.e. the origin of one frame, into another. Note that this method
475  * does not throw an exception on error, rather the success of the
476  * transformation is indicated by a return value.
477  *
478  * For example, if the source frame is the base frame
479  * (e.g. /base_link), and the target frame is the global frame
480  * (e.g. /map), then the result would be the robot's global position.
481  * @param source_frame frame whose origin to transform
482  * @param target_frame frame in which you want to know the position of
483  * the origin of the source frame
484  * @param stamped_out upon successful completion (check return value)
485  * stamped pose of origin of source frame in target frame
486  * @param time time for when to do the transformation, by default take
487  * the latest matching transformation.
488  * @return true if the transform could be successfully determined,
489  * false otherwise.
490  */
491 bool
492 Transformer::transform_origin(const std::string& source_frame,
493  const std::string& target_frame,
494  Stamped<Pose>& stamped_out, const fawkes::Time time) const
495 {
496  tf::Stamped<tf::Pose> ident = tf::ident(source_frame, time);
497  try {
498  transform_pose(target_frame, ident, stamped_out);
499  return true;
500  } catch (Exception &e) {
501  return false;
502  }
503 }
504 
505 
506 /** Transform a stamped Quaternion into the target frame assuming a fixed frame.
507  * This transforms the quaternion relative to the frame set in the
508  * stamped quaternion into the target frame.
509  * This will transform the quaternion from source to target, assuming
510  * that there is a fixed frame, by first finding the transform of the
511  * source frame in the fixed frame, and then a transformation from the
512  * fixed frame into the target frame.
513  * @param target_frame frame into which to transform
514  * @param target_time desired time in the target frame
515  * @param stamped_in stamped quaternion, defines source frame and time
516  * @param fixed_frame ID of fixed frame
517  * @param stamped_out stamped output quaternion in target_frame
518  * @exception ConnectivityException thrown if no connection between
519  * the source and target frame could be found in the tree.
520  * @exception ExtrapolationException returning a value would have
521  * required extrapolation beyond current limits.
522  * @exception LookupException at least one of the two given frames is
523  * unknown
524  * @exception InvalidArgument thrown if the Quaternion is invalid, most
525  * likely an uninitialized Quaternion (0,0,0,0).
526  */
527 void
528 Transformer::transform_quaternion(const std::string& target_frame,
529  const fawkes::Time& target_time,
530  const Stamped<Quaternion>& stamped_in,
531  const std::string& fixed_frame,
532  Stamped<Quaternion>& stamped_out) const
533 {
534  assert_quaternion_valid(stamped_in);
535  StampedTransform transform;
536  lookup_transform(target_frame, target_time,
537  stamped_in.frame_id, stamped_in.stamp,
538  fixed_frame, transform);
539 
540  stamped_out.set_data(transform * stamped_in);
541  stamped_out.stamp = transform.stamp;
542  stamped_out.frame_id = target_frame;
543 }
544 
545 /** Transform a stamped vector into the target frame assuming a fixed frame.
546  * This transforms the vector relative to the frame set in the
547  * stamped quaternion into the target frame.
548  * This will transform the vector from source to target, assuming
549  * that there is a fixed frame, by first finding the transform of the
550  * source frame in the fixed frame, and then a transformation from the
551  * fixed frame into the target frame.
552  * @param target_frame frame into which to transform
553  * @param target_time desired time in the target frame
554  * @param stamped_in stamped vector, defines source frame and time
555  * @param fixed_frame ID of fixed frame
556  * @param stamped_out stamped output vector in target_frame
557  * @exception ConnectivityException thrown if no connection between
558  * the source and target frame could be found in the tree.
559  * @exception ExtrapolationException returning a value would have
560  * required extrapolation beyond current limits.
561  * @exception LookupException at least one of the two given frames is
562  * unknown
563  */
564 void
565 Transformer::transform_vector(const std::string& target_frame,
566  const fawkes::Time& target_time,
567  const Stamped<Vector3>& stamped_in,
568  const std::string& fixed_frame,
569  Stamped<Vector3>& stamped_out) const
570 {
571  StampedTransform transform;
572  lookup_transform(target_frame, target_time,
573  stamped_in.frame_id,stamped_in.stamp,
574  fixed_frame, transform);
575 
576  // may not be most efficient
577  Vector3 end = stamped_in;
578  Vector3 origin(0,0,0);
579  Vector3 output = (transform * end) - (transform * origin);
580  stamped_out.set_data( output);
581 
582  stamped_out.stamp = transform.stamp;
583  stamped_out.frame_id = target_frame;
584 
585 }
586 
587 /** Transform a stamped point into the target frame assuming a fixed frame.
588  * This transforms the point relative to the frame set in the
589  * stamped quaternion into the target frame.
590  * This will transform the point from source to target, assuming
591  * that there is a fixed frame, by first finding the transform of the
592  * source frame in the fixed frame, and then a transformation from the
593  * fixed frame into the target frame.
594  * @param target_frame frame into which to transform
595  * @param target_time desired time in the target frame
596  * @param stamped_in stamped point, defines source frame and time
597  * @param fixed_frame ID of fixed frame
598  * @param stamped_out stamped output point in target_frame
599  * @exception ConnectivityException thrown if no connection between
600  * the source and target frame could be found in the tree.
601  * @exception ExtrapolationException returning a value would have
602  * required extrapolation beyond current limits.
603  * @exception LookupException at least one of the two given frames is
604  * unknown
605  */
606 void
607 Transformer::transform_point(const std::string& target_frame,
608  const fawkes::Time& target_time,
609  const Stamped<Point>& stamped_in,
610  const std::string& fixed_frame,
611  Stamped<Point>& stamped_out) const
612 {
613  StampedTransform transform;
614  lookup_transform(target_frame, target_time,
615  stamped_in.frame_id, stamped_in.stamp,
616  fixed_frame, transform);
617 
618  stamped_out.set_data(transform * stamped_in);
619  stamped_out.stamp = transform.stamp;
620  stamped_out.frame_id = target_frame;
621 }
622 
623 /** Transform a stamped pose into the target frame assuming a fixed frame.
624  * This transforms the pose relative to the frame set in the
625  * stamped quaternion into the target frame.
626  * This will transform the pose from source to target, assuming
627  * that there is a fixed frame, by first finding the transform of the
628  * source frame in the fixed frame, and then a transformation from the
629  * fixed frame into the target frame.
630  * @param target_frame frame into which to transform
631  * @param target_time desired time in the target frame
632  * @param stamped_in stamped pose, defines source frame and time
633  * @param fixed_frame ID of fixed frame
634  * @param stamped_out stamped output pose in target_frame
635  * @exception ConnectivityException thrown if no connection between
636  * the source and target frame could be found in the tree.
637  * @exception ExtrapolationException returning a value would have
638  * required extrapolation beyond current limits.
639  * @exception LookupException at least one of the two given frames is
640  * unknown
641  */
642 void
643 Transformer::transform_pose(const std::string& target_frame,
644  const fawkes::Time& target_time,
645  const Stamped<Pose>& stamped_in,
646  const std::string& fixed_frame,
647  Stamped<Pose>& stamped_out) const
648 {
649  StampedTransform transform;
650  lookup_transform(target_frame, target_time,
651  stamped_in.frame_id, stamped_in.stamp,
652  fixed_frame, transform);
653 
654  stamped_out.set_data(transform * stamped_in);
655  stamped_out.stamp = transform.stamp;
656  stamped_out.frame_id = target_frame;
657 }
658 
659 
660 /** Get DOT graph of all frames.
661  * @param print_time true to add the time of the transform as graph label
662  * @param time if not NULL will be assigned the time of the graph generation
663  * @return string representation of DOT graph
664  */
665 std::string
666 Transformer::all_frames_as_dot(bool print_time, fawkes::Time *time) const
667 {
668  std::unique_lock<std::mutex> lock(frame_mutex_);
669 
670  fawkes::Time current_time;
671  if (time) *time = current_time;
672 
673  std::stringstream mstream;
674  mstream << std::fixed; //fixed point notation
675  mstream.precision(3); //3 decimal places
676  mstream << "digraph { graph [fontsize=14";
677  if (print_time) {
678  mstream << ", label=\"\\nRecorded at time: "
679  << current_time.str() << " (" << current_time.in_sec() << ")\"";
680  }
681  mstream << "]; node [fontsize=12]; edge [fontsize=12]; " << std::endl;
682 
683  TransformStorage temp;
684 
685  if (frames_.size() == 1)
686  mstream <<"\"no tf data received\"";
687 
688  mstream.precision(3);
689  mstream.setf(std::ios::fixed, std::ios::floatfield);
690 
691  // for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it)
692  for (unsigned int cnt = 1; cnt < frames_.size(); ++cnt) //one referenced for 0 is no frame
693  {
694  std::shared_ptr<TimeCacheInterface> cache = get_frame(cnt);
695  if (! cache) continue;
696 
697  unsigned int frame_id_num;
698  if(cache->get_data(fawkes::Time(0,0), temp)) {
699  frame_id_num = temp.frame_id;
700  } else {
701  frame_id_num = 0;
702  }
703  if (frame_id_num != 0) {
704  std::string authority = "no recorded authority";
705  std::map<unsigned int, std::string>::const_iterator it = frame_authority_.find(cnt);
706  if (it != frame_authority_.end())
707  authority = it->second;
708 
709  double rate = cache->get_list_length() /
710  std::max((cache->get_latest_timestamp().in_sec() -
711  cache->get_oldest_timestamp().in_sec()), 0.0001);
712 
713  mstream << "\"" << frameIDs_reverse[frame_id_num] << "\"" << " -> "
714  << "\"" << frameIDs_reverse[cnt] << "\"" << "[label=\"";
715 
716  std::shared_ptr<StaticCache> static_cache =
717  std::dynamic_pointer_cast<StaticCache>(cache);
718  if (static_cache) {
719  mstream << "Static";
720  } else {
721  //<< "Broadcaster: " << authority << "\\n"
722  mstream << "Average rate: " << rate << " Hz\\n"
723  << "Most recent transform: "
724  << (current_time - cache->get_latest_timestamp()).in_sec() << " sec old \\n"
725  << "Buffer length: "
726  << (cache->get_latest_timestamp() - cache->get_oldest_timestamp()).in_sec()
727  << " sec\\n";
728  }
729  mstream <<"\"];" <<std::endl;
730  }
731  }
732 
733  mstream << "}";
734  return mstream.str();
735 }
736 
737 } // end namespace tf
738 } // end namespace fawkes
std::string frame_id
The frame_id associated this data.
Definition: types.h:136
double in_sec() const
Convet time to seconds.
Definition: time.cpp:232
const char * str(bool utc=false) const
Output function.
Definition: time.cpp:872
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.
std::vector< std::string > get_frame_id_mappings() const
Get mappings from frame ID to names.
Fawkes library namespace.
V_TimeCacheInterface frames_
The pointers to potential frames that the tree can be made of.
Definition: buffer_core.h:171
Storage for transforms and their parent.
CompactFrameID lookup_frame_number(const std::string &frameid_str) const
String to number for frame lookup with dynamic allocation of new frames.
fawkes::Time stamp
Timestamp of this transform.
Definition: types.h:100
bool can_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, std::string *error_msg=NULL) const
Test if a transform is possible.
bool is_enabled() const
Check if transformer is enabled.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
void set_enabled(bool enabled)
Set enabled status of transformer.
A class for handling time.
Definition: time.h:91
void lock()
Lock transformer.
std::map< CompactFrameID, std::string > frame_authority_
A map to lookup the most recent authority for a given frame.
Definition: buffer_core.h:183
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.
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.
TimeCacheInterfacePtr get_frame_cache(const std::string &frame_id) const
Get cache for specific frame.
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.
A Class which provides coordinate transforms between any two frames in a system.
Definition: buffer_core.h:112
fawkes::Time stamp
The timestamp associated with this data.
Definition: types.h:135
float get_cache_time() const
Get cache time.
bool try_lock()
Try to acquire lock.
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.
Base class for exceptions in Fawkes.
Definition: exception.h:36
CompactFrameID frame_id
parent/reference frame number
virtual ~Transformer(void)
Destructor.
Transform cache for static transforms.
Definition: time_cache.h:142
TimeCacheInterfacePtr get_frame(CompactFrameID c_frame_id) const
Accessor to get frame cache.
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
std::string all_frames_as_dot(bool print_time, fawkes::Time *time=0) const
Get DOT graph of all frames.
std::mutex frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
Definition: buffer_core.h:174
bool transform_origin(const std::string &source_frame, const std::string &target_frame, Stamped< Pose > &stamped_out, const fawkes::Time time=fawkes::Time(0, 0)) const
Transform ident pose from one frame to another.
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:133
M_StringToCompactFrameID frameIDs_
Mapping from frame string IDs to compact IDs.
Definition: buffer_core.h:179
Transformer(float cache_time_sec=BufferCore::DEFAULT_CACHE_TIME)
Constructor.
float cache_time_
How long to cache transform history.
Definition: buffer_core.h:187
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::vector< TimeCacheInterfacePtr > get_frame_caches() const
Get all currently existing caches.
void set_data(const T &input)
Set the data element.
Definition: types.h:157
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
std::vector< std::string > frameIDs_reverse
A map from CompactFrameID frame_id_numbers to string for debugging and output.
Definition: buffer_core.h:181