52 #include <tf/buffer_core.h>
53 #include <tf/exceptions.h>
54 #include <tf/time_cache.h>
65 typedef std::pair<fawkes::Time, CompactFrameID> P_TimeAndFrameID;
76 if (frame_id.size() == 0) {
77 printf(
"Invalid argument passed to %s in tf2 frame_ids cannot be empty", function_name_arg);
81 if (starts_with_slash(frame_id)) {
82 printf(
"Invalid argument '%s' passed to %s in tf2 frame_ids cannot start with a '/'",
102 if (frame_id.empty()) {
103 throw InvalidArgumentException(
"Invalid argument passed to %s in tf2 frame_ids cannot be empty",
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 '/'",
130 frames_.push_back(TimeCacheInterfacePtr());
134 BufferCore::~BufferCore()
145 for (std::vector<TimeCacheInterfacePtr>::iterator cache_it =
frames_.begin() + 1;
149 (*cache_it)->clear_list();
164 const std::string & authority,
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);
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",
176 stripped.child_frame_id.c_str());
180 if (stripped.child_frame_id ==
"") {
181 printf(
"TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id "
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(),
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(),
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());
219 TimeCacheInterfacePtr frame =
get_frame(frame_number);
223 if (frame->insert_data(TransformStorage(stripped,
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(),
248 TimeCacheInterfacePtr
251 TimeCacheInterfacePtr frame_ptr =
frames_[cfid];
253 frames_[cfid] = TimeCacheInterfacePtr(
new StaticCache());
263 TargetParentOfSource,
264 SourceParentOfTarget,
276 template <
typename F>
280 CompactFrameID target_id,
281 CompactFrameID source_id,
282 std::string * error_string)
const
297 template <
typename F>
301 CompactFrameID target_id,
302 CompactFrameID source_id,
303 std::string * error_string,
304 std::vector<CompactFrameID> *frame_chain)
const
307 frame_chain->clear();
310 if (source_id == target_id) {
311 f.finalize(Identity, time);
318 if (retval != NO_ERROR) {
324 CompactFrameID frame = source_id;
325 CompactFrameID top_parent = frame;
328 std::string extrapolation_error_string;
329 bool extrapolation_might_have_occurred =
false;
332 TimeCacheInterfacePtr cache =
get_frame(frame);
334 frame_chain->push_back(frame);
342 CompactFrameID parent = f.gather(cache, time, &extrapolation_error_string);
346 extrapolation_might_have_occurred =
true;
351 if (frame == target_id) {
352 f.finalize(TargetParentOfSource, time);
364 std::stringstream ss;
365 ss <<
"The tf tree is invalid because it contains a loop." << std::endl
367 *error_string = ss.str();
376 std::vector<CompactFrameID> reverse_frame_chain;
378 while (frame != top_parent) {
379 TimeCacheInterfacePtr cache =
get_frame(frame);
381 reverse_frame_chain.push_back(frame);
387 CompactFrameID parent = f.gather(cache, time, error_string);
390 std::stringstream ss;
391 ss << *error_string <<
", when looking up transform from frame ["
394 *error_string = ss.str();
397 return EXTRAPOLATION_ERROR;
401 if (frame == source_id) {
402 f.finalize(SourceParentOfTarget, time);
404 frame_chain->swap(reverse_frame_chain);
416 std::stringstream ss;
417 ss <<
"The tf tree is invalid because it contains a loop." << std::endl
419 *error_string = ss.str();
425 if (frame != top_parent) {
426 if (extrapolation_might_have_occurred) {
428 std::stringstream ss;
429 ss << extrapolation_error_string <<
", when looking up transform from frame ["
432 *error_string = ss.str();
435 return EXTRAPOLATION_ERROR;
439 return CONNECTIVITY_ERROR;
442 f.finalize(FullPath, time);
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])
453 frame_chain->erase(frame_chain->begin() + (n - 1), frame_chain->end());
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]);
466 struct 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)
479 gather(TimeCacheInterfacePtr cache,
fawkes::Time time, std::string *error_string)
481 if (!cache->get_data(time, st, error_string)) {
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;
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;
504 case Identity:
break;
505 case TargetParentOfSource:
506 result_vec = source_to_top_vec;
507 result_quat = source_to_top_quat;
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;
517 Quaternion inv_target_quat = target_to_top_quat.inverse();
518 Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
520 result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
521 result_quat = inv_target_quat * source_to_top_quat;
530 Quaternion source_to_top_quat;
531 Vector3 source_to_top_vec;
532 Quaternion target_to_top_quat;
533 Vector3 target_to_top_vec;
535 Quaternion result_quat;
555 const std::string & source_frame,
557 StampedTransform & transform)
const
561 if (target_frame == source_frame) {
562 transform.setIdentity();
563 transform.frame_id = target_frame;
564 transform.child_frame_id = source_frame;
568 TimeCacheInterfacePtr cache =
get_frame(target_id);
570 transform.stamp = cache->get_latest_timestamp();
572 transform.stamp = time;
574 transform.
stamp = time;
579 CompactFrameID target_id =
581 CompactFrameID source_id =
584 std::string error_string;
585 TransformAccum accum;
587 if (retval != NO_ERROR) {
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());
594 throw TransformException();
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;
626 const std::string & source_frame,
628 const std::string & fixed_frame,
629 StampedTransform & transform)
const
638 transform.set_data(temp2 * temp1);
639 transform.stamp = temp2.
stamp;
640 transform.frame_id = target_frame;
641 transform.child_frame_id = source_frame;
645 struct CanTransformAccum
648 gather(TimeCacheInterfacePtr cache,
fawkes::Time time, std::string *error_string)
650 return cache->get_parent(time, error_string);
677 CompactFrameID source_id,
679 std::string * error_msg)
const
681 if (target_id == 0 || source_id == 0) {
685 if (target_id == source_id) {
689 CanTransformAccum accum;
707 CompactFrameID source_id,
709 std::string * error_msg)
const
724 const std::string & source_frame,
726 std::string * error_msg)
const
729 if (target_frame == source_frame)
732 if (
warn_frame_id(
"canTransform argument target_frame", target_frame))
734 if (
warn_frame_id(
"canTransform argument source_frame", source_frame))
757 const std::string & source_frame,
759 const std::string & fixed_frame,
760 std::string * error_msg)
const
762 if (
warn_frame_id(
"canTransform argument target_frame", target_frame))
764 if (
warn_frame_id(
"canTransform argument source_frame", source_frame))
766 if (
warn_frame_id(
"canTransform argument fixed_frame", fixed_frame))
770 &&
can_transform(fixed_frame, source_frame, source_time, error_msg);
779 TimeCacheInterfacePtr
782 if (frame_id >=
frames_.size())
783 return TimeCacheInterfacePtr();
796 CompactFrameID retval;
797 M_StringToCompactFrameID::const_iterator map_it =
frameIDs_.find(frameid_str);
799 retval = CompactFrameID(0);
801 retval = map_it->second;
812 CompactFrameID retval = 0;
813 M_StringToCompactFrameID::iterator map_it =
frameIDs_.find(frameid_str);
815 retval = CompactFrameID(
frames_.size());
816 frames_.push_back(TimeCacheInterfacePtr());
834 throw LookupException(
"Reverse lookup of frame id %u failed!", frame_id_num);
846 CompactFrameID target_frame,
847 std::string * out)
const
852 *out = std::string(
"Could not find a connection between '" +
lookup_frame_string(target_frame)
854 +
"' because they are not part of the same tree."
855 +
"Tf has two or more unconnected trees.");
877 std::stringstream mstream;
884 for (
unsigned int counter = 1; counter <
frames_.size(); counter++) {
885 TimeCacheInterfacePtr frame_ptr =
get_frame(CompactFrameID(counter));
886 if (frame_ptr == NULL)
888 CompactFrameID frame_id_num;
898 return mstream.str();
902 struct TimeAndFrameIDFrameComparator
904 TimeAndFrameIDFrameComparator(CompactFrameID
id) : id(id)
909 operator()(
const P_TimeAndFrameID &rhs)
const
911 return rhs.second == id;
927 CompactFrameID source_id,
929 std::string * error_string)
const
932 if (source_id == 0 || target_id == 0)
935 if (source_id == target_id) {
939 time = cache->get_latest_timestamp();
945 std::vector<P_TimeAndFrameID> lct_cache;
949 CompactFrameID frame = source_id;
950 P_TimeAndFrameID temp;
954 TimeCacheInterfacePtr cache =
get_frame(frame);
961 P_TimeAndFrameID latest = cache->get_latest_time_and_parent();
963 if (latest.second == 0) {
968 if (!latest.first.is_zero()) {
969 common_time = std::min(latest.first, common_time);
972 lct_cache.push_back(latest);
974 frame = latest.second;
977 if (frame == target_id) {
988 std::stringstream ss;
989 ss <<
"The tf tree is invalid because it contains a loop." << std::endl
991 *error_string = ss.str();
1001 CompactFrameID common_parent = 0;
1003 TimeCacheInterfacePtr cache =
get_frame(frame);
1009 P_TimeAndFrameID latest = cache->get_latest_time_and_parent();
1011 if (latest.second == 0) {
1015 if (!latest.first.is_zero()) {
1016 common_time = std::min(latest.first, common_time);
1019 std::vector<P_TimeAndFrameID>::iterator it =
1020 std::find_if(lct_cache.begin(),
1027 TimeAndFrameIDFrameComparator(latest.second));
1029 if (it != lct_cache.end())
1031 common_parent = it->second;
1035 frame = latest.second;
1038 if (frame == source_id) {
1049 std::stringstream ss;
1050 ss <<
"The tf tree is invalid because it contains a loop." << std::endl
1052 *error_string = ss.str();
1054 return LOOKUP_ERROR;
1058 if (common_parent == 0) {
1060 return CONNECTIVITY_ERROR;
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);
1072 if (it->second == common_parent) {
1094 std::stringstream mstream;
1097 TransformStorage temp;
1102 mstream.precision(3);
1103 mstream.setf(std::ios::fixed, std::ios::floatfield);
1106 for (
unsigned int counter = 1; counter <
frames_.size();
1109 CompactFrameID cfid = CompactFrameID(counter);
1110 CompactFrameID frame_id_num;
1111 TimeCacheInterfacePtr cache =
get_frame(cfid);
1120 frame_id_num = temp.frame_id;
1122 std::string authority =
"no recorded authority";
1123 std::map<CompactFrameID, std::string>::const_iterator it =
frame_authority_.find(cfid);
1125 authority = it->second;
1129 cache->get_list_length()
1130 / std::max((cache->get_latest_timestamp().in_sec() - cache->get_oldest_timestamp().in_sec()),
1133 mstream << std::fixed;
1134 mstream.precision(3);
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()
1145 mstream <<
" buffer_length: "
1146 << (cache->get_latest_timestamp() - cache->get_oldest_timestamp()).in_sec()
1150 return mstream.str();