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,
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))
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;
bool insert_data(const TransformStorage &new_data)
Insert data.
std::string frame_id
The frame_id associated this data.
bool get_data(fawkes::Time time, TransformStorage &data_out, std::string *error_str=0)
Get data.
CompactFrameID get_parent(fawkes::Time time, std::string *error_str)
Get parent frame number.
Fawkes library namespace.
CompactFrameID id
Frame number to look for.
A class for handling time.
No connection between two frames in tree.
Time based transform cache.
fawkes::Time stamp
The timestamp associated with this data.
TimeAndFrameIDFrameComparator(CompactFrameID id)
Constructor.
Base class for exceptions in Fawkes.
Wrapper class to add time stamp and frame ID to base types.
bool operator()(const P_TimeAndFrameID &rhs) const
Application operator.
const Time TIME_MAX
Instance of Time denoting the maximum value possible.
The requested feature is disabled.
Time & stamp()
Set this time to the current time.
Time and frame ID comparator class.
A frame could not be looked up.
Mutex mutual exclusion lock.
void set_data(const T &input)
Set the data element.