52 #include <tf/transformer.h>
53 #include <tf/time_cache.h>
54 #include <tf/exceptions.h>
56 #include <core/threading/mutex_locker.h>
105 const float Transformer::DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 0.f;
111 TargetParentOfSource,
112 SourceParentOfTarget,
144 void accum(
bool source)
166 : source_to_top_quat(0.0, 0.0, 0.0, 1.0)
167 , source_to_top_vec(0.0, 0.0, 0.0)
168 , target_to_top_quat(0.0, 0.0, 0.0, 1.0)
169 , target_to_top_vec(0.0, 0.0, 0.0)
170 , result_quat(0.0, 0.0, 0.0, 1.0)
171 , result_vec(0.0, 0.0, 0.0)
184 if (!cache->
get_data(time, st, error_string))
195 void accum(
bool source)
199 source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_;
200 source_to_top_quat = st.rotation_ * source_to_top_quat;
204 target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_;
205 target_to_top_quat = st.rotation_ * target_to_top_quat;
219 case TargetParentOfSource:
220 result_vec = source_to_top_vec;
221 result_quat = source_to_top_quat;
223 case SourceParentOfTarget:
225 btQuaternion inv_target_quat = target_to_top_quat.inverse();
226 btVector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
227 result_vec = inv_target_vec;
228 result_quat = inv_target_quat;
233 btQuaternion inv_target_quat = target_to_top_quat.inverse();
234 btVector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
236 result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
237 result_quat = inv_target_quat * source_to_top_quat;
267 Transformer::Transformer(
float cache_time)
268 : cache_time_(cache_time),
269 fall_back_to_wall_time_(false)
286 std::vector<TimeCache*>::iterator cache_it;
287 for (cache_it =
frames_.begin(); cache_it !=
frames_.end(); ++cache_it)
311 std::vector< TimeCache*>::iterator cache_it;
312 for (cache_it =
frames_.begin() + 1; cache_it !=
frames_.end(); ++cache_it)
314 (*cache_it)->clear_list();
328 std::string frame_id_resolveped = frame_id_str;
330 return frameIDs_.count(frame_id_resolveped);
334 Transformer::create_connectivity_error_string(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out)
const
338 *out = std::string(
"Could not find a connection between '"+
lookup_frame_string(target_frame)+
"' and '"+
340 "Tf has two or more unconnected trees.");
354 Transformer::walk_to_top_parent(F& f,
fawkes::Time time,
355 CompactFrameID target_id, CompactFrameID source_id,
356 std::string* error_string)
const
359 if (source_id == target_id)
361 f.finalize(Identity, time);
369 if (retval != NO_ERROR)
376 CompactFrameID frame = source_id;
377 CompactFrameID top_parent = frame;
390 CompactFrameID parent = f.gather(cache, time, 0);
399 if (frame == target_id)
401 f.finalize(TargetParentOfSource, time);
415 std::stringstream ss;
418 *error_string = ss.str();
427 while (frame != top_parent)
436 CompactFrameID parent = f.gather(cache, time, error_string);
441 std::stringstream ss;
444 *error_string = ss.str();
447 return EXTRAPOLATION_ERROR;
451 if (frame == source_id)
453 f.finalize(SourceParentOfTarget, time);
466 std::stringstream ss;
469 *error_string =
"The tf tree is invalid because it contains a loop.";
475 if (frame != top_parent)
477 create_connectivity_error_string(source_id, target_id, error_string);
478 return CONNECTIVITY_ERROR;
481 f.finalize(FullPath, time);
502 return rhs.second ==
id;
521 if (source_id == target_id)
528 std::vector<P_TimeAndFrameID> lct_cache;
532 CompactFrameID frame = source_id;
533 P_TimeAndFrameID temp;
545 P_TimeAndFrameID latest = cache->get_latest_time_and_parent();
547 if (latest.second == 0) {
552 if (!latest.first.is_zero()) {
553 common_time = std::min(latest.first, common_time);
556 lct_cache.push_back(latest);
558 frame = latest.second;
561 if (frame == target_id) {
587 CompactFrameID common_parent = 0;
596 P_TimeAndFrameID latest = cache->get_latest_time_and_parent();
598 if (latest.second == 0) {
602 if (!latest.first.is_zero()) {
603 common_time = std::min(latest.first, common_time);
606 std::vector<P_TimeAndFrameID>::iterator it =
607 std::find_if(lct_cache.begin(), lct_cache.end(),
608 TimeAndFrameIDFrameComparator(latest.second));
610 if (it != lct_cache.end()) {
611 common_parent = it->second;
615 frame = latest.second;
618 if (frame == source_id) {
629 std::stringstream ss;
632 *error_string = ss.str();
638 if (common_parent == 0) {
639 create_connectivity_error_string(source_id, target_id, error_string);
640 return CONNECTIVITY_ERROR;
645 std::vector<P_TimeAndFrameID>::iterator it = lct_cache.begin();
646 std::vector<P_TimeAndFrameID>::iterator end = lct_cache.end();
647 for (; it != end; ++it) {
648 if (!it->first.is_zero()) {
649 common_time = std::min(common_time, it->first);
652 if (it->second == common_parent) {
675 const std::string &target_frame,
677 std::string* error_string)
const
679 std::string mapped_tgt = target_frame;
680 std::string mapped_src = source_frame;
701 transform.frame_id, transform.child_frame_id);
703 mapped_transform.
frame_id = transform.frame_id;
706 bool error_exists =
false;
709 printf(
"TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with frame_id and child_frame_id \"%s\" because they are the same", authority.c_str(), mapped_transform.
child_frame_id.c_str());
715 printf(
"TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str());
719 if (mapped_transform.
frame_id ==
"/")
721 printf(
"TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"%s\" from authority \"%s\" because frame_id not set", mapped_transform.
child_frame_id.c_str(), authority.c_str());
725 if (std::isnan(mapped_transform.getOrigin().x()) || std::isnan(mapped_transform.getOrigin().y()) || std::isnan(mapped_transform.getOrigin().z())||
726 std::isnan(mapped_transform.getRotation().x()) || std::isnan(mapped_transform.getRotation().y()) || std::isnan(mapped_transform.getRotation().z()) || std::isnan(mapped_transform.getRotation().w()))
728 printf(
"TF_NAN_INPUT: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)",
730 mapped_transform.getOrigin().x(), mapped_transform.getOrigin().y(), mapped_transform.getOrigin().z(),
731 mapped_transform.getRotation().x(), mapped_transform.getRotation().y(), mapped_transform.getRotation().z(), mapped_transform.getRotation().w()
771 if (frame_number == 0)
787 if (frame_number == 0) {
803 unsigned int retval = 0;
805 M_StringToCompactFrameID::const_iterator map_it =
frameIDs_.find(frameid_str);
809 frameid_str.c_str(),
frameIDs_.size(),
"" );
812 retval = map_it->second;
826 unsigned int retval = 0;
828 M_StringToCompactFrameID::iterator map_it =
frameIDs_.find(frameid_str);
850 throw LookupException(
"Reverse lookup of frame id %u failed!", frame_num);
870 const std::string& source_frame,
878 std::string mapped_tgt = target_frame;
879 std::string mapped_src = source_frame;
881 if (mapped_tgt == mapped_src) {
882 transform.setIdentity();
894 std::string error_string;
896 int rv = walk_to_top_parent(accum, time, target_id, source_id, &error_string);
901 case CONNECTIVITY_ERROR:
903 case EXTRAPOLATION_ERROR:
908 throw Exception(
"lookup_transform: unknown error code: %d", rv);
933 const std::string& source_frame,
961 const std::string& source_frame,
963 const std::string& fixed_frame,
983 const std::string& source_frame,
986 std::string mapped_tgt = target_frame;
987 std::string mapped_src = source_frame;
989 if (mapped_tgt == mapped_src)
return true;
997 return can_transform_no_lock(target_id, source_id, time);
1013 const std::string& source_frame,
1015 const std::string& fixed_frame)
const
1031 Transformer::can_transform_no_lock(CompactFrameID target_id,
1032 CompactFrameID source_id,
1035 if (target_id == 0 || source_id == 0)
return false;
1038 if (walk_to_top_parent(accum, time, target_id, source_id, NULL) == NO_ERROR)
1065 assert_quaternion_valid(stamped_in);
1069 stamped_in.
stamp, transform);
1071 stamped_out.
set_data(transform * stamped_in);
1073 stamped_out.
frame_id = target_frame;
1096 stamped_in.
stamp, transform);
1099 Vector3 end = stamped_in;
1100 Vector3 origin = Vector3(0,0,0);
1101 Vector3 output = (transform * end) - (transform * origin);
1105 stamped_out.
frame_id = target_frame;
1128 stamped_in.
stamp, transform);
1130 stamped_out.
set_data(transform * stamped_in);
1132 stamped_out.
frame_id = target_frame;
1156 stamped_in.
stamp, transform);
1158 stamped_out.
set_data(transform * stamped_in);
1160 stamped_out.
frame_id = target_frame;
1189 const std::string& fixed_frame,
1192 assert_quaternion_valid(stamped_in);
1196 fixed_frame, transform);
1198 stamped_out.
set_data(transform * stamped_in);
1200 stamped_out.
frame_id = target_frame;
1226 const std::string& fixed_frame,
1232 fixed_frame, transform);
1235 Vector3 end = stamped_in;
1236 Vector3 origin(0,0,0);
1237 Vector3 output = (transform * end) - (transform * origin);
1241 stamped_out.
frame_id = target_frame;
1268 const std::string& fixed_frame,
1274 fixed_frame, transform);
1276 stamped_out.
set_data(transform * stamped_in);
1278 stamped_out.
frame_id = target_frame;
1304 const std::string& fixed_frame,
1310 fixed_frame, transform);
1312 stamped_out.
set_data(transform * stamped_in);
1314 stamped_out.
frame_id = target_frame;