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
63namespace fawkes {
64namespace 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 */
99Transformer::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 */
109void
111{
112 enabled_ = enabled;
113}
114
115/** Check if transformer is enabled.
116 * @return true if enabled, false otherwise
117 */
118bool
120{
121 return enabled_;
122}
123
124/** Get cache time.
125 * @return time in seconds caches will date back.
126 */
127float
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 */
137void
139{
140 frame_mutex_.lock();
141}
142
143/** Try to acquire lock.
144 * @return true if lock has been acquired, alse otherwise
145 */
146bool
148{
149 return frame_mutex_.try_lock();
150}
151
152/** Unlock.
153 * Release currently held lock.
154 */
155void
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 */
165bool
166Transformer::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 */
177TimeCacheInterfacePtr
178Transformer::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 */
186std::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 */
195std::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 */
209bool
210Transformer::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 */
234bool
235Transformer::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 */
264void
265Transformer::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 */
298void
299Transformer::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 */
327void
328Transformer::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 */
350void
351Transformer::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 */
378void
379Transformer::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 */
409void
410Transformer::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 */
435void
436Transformer::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 */
467bool
468Transformer::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 */
503void
504Transformer::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 */
539void
540Transformer::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 */
579void
580Transformer::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 */
614void
615Transformer::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 */
635std::string
636Transformer::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
Base class for exceptions in Fawkes.
Definition: exception.h:36
A class for handling time.
Definition: time.h:93
double in_sec() const
Convet time to seconds.
Definition: time.cpp:219
const char * str(bool utc=false) const
Output function.
Definition: time.cpp:790
A Class which provides coordinate transforms between any two frames in a system.
Definition: buffer_core.h:107
float cache_time_
How long to cache transform history.
Definition: buffer_core.h:187
CompactFrameID lookup_frame_number(const std::string &frameid_str) const
String to number for frame lookup with dynamic allocation of new frames.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
std::map< CompactFrameID, std::string > frame_authority_
A map to lookup the most recent authority for a given frame.
Definition: buffer_core.h:184
M_StringToCompactFrameID frameIDs_
Mapping from frame string IDs to compact IDs.
Definition: buffer_core.h:180
std::mutex frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
Definition: buffer_core.h:175
TimeCacheInterfacePtr get_frame(CompactFrameID c_frame_id) const
Accessor to get frame cache.
V_TimeCacheInterface frames_
The pointers to potential frames that the tree can be made of.
Definition: buffer_core.h:172
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.
std::vector< std::string > frameIDs_reverse
A map from CompactFrameID frame_id_numbers to string for debugging and output.
Definition: buffer_core.h:182
The requested feature is disabled.
Definition: exceptions.h:61
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
fawkes::Time stamp
Timestamp of this transform.
Definition: types.h:95
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
std::string frame_id
The frame_id associated this data.
Definition: types.h:133
fawkes::Time stamp
The timestamp associated with this data.
Definition: types.h:132
void set_data(const T &input)
Set the data element.
Definition: types.h:152
Storage for transforms and their parent.
CompactFrameID frame_id
parent/reference frame number
bool is_enabled() const
Check if transformer is enabled.
float get_cache_time() const
Get cache time.
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 lock()
Lock transformer.
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.
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.
void set_enabled(bool enabled)
Set enabled status of transformer.
bool frame_exists(const std::string &frame_id_str) const
Check if frame exists.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
TimeCacheInterfacePtr get_frame_cache(const std::string &frame_id) const
Get cache for specific frame.
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.
Transformer(float cache_time_sec=BufferCore::DEFAULT_CACHE_TIME)
Constructor.
Definition: transformer.cpp:99
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.
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.
bool try_lock()
Try to acquire lock.
std::vector< TimeCacheInterfacePtr > get_frame_caches() const
Get all currently existing caches.
std::string all_frames_as_dot(bool print_time, fawkes::Time *time=0) const
Get DOT graph of all frames.
virtual ~Transformer(void)
Destructor.
std::vector< std::string > get_frame_id_mappings() const
Get mappings from frame ID to names.
Fawkes library namespace.