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