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