52 #include <tf/buffer_core.h> 53 #include <tf/time_cache.h> 54 #include <tf/exceptions.h> 69 typedef std::pair<fawkes::Time, CompactFrameID> P_TimeAndFrameID;
81 if (frame_id.size() == 0) {
82 printf(
"Invalid argument passed to %s in tf2 frame_ids cannot be empty", function_name_arg);
86 if (starts_with_slash(frame_id))
88 printf(
"Invalid argument '%s' passed to %s in tf2 frame_ids cannot start with a '/'",
89 frame_id.c_str(), function_name_arg);
107 if (frame_id.empty())
113 if (starts_with_slash(frame_id))
116 "In tf2 frame_ids cannot start with a '/'",
117 frame_id.c_str(), function_name_arg);
124 frame_id.c_str(), function_name_arg);
137 frames_.push_back(TimeCacheInterfacePtr());
141 BufferCore::~BufferCore()
153 for (std::vector<TimeCacheInterfacePtr>::iterator cache_it =
frames_.begin() + 1; cache_it !=
frames_.end(); ++cache_it)
156 (*cache_it)->clear_list();
172 const std::string& authority,
bool is_static)
178 bool error_exists =
false;
181 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(), stripped.
child_frame_id.c_str());
187 printf(
"TF_NO_CHILD_FRAME_ID: Ignoring transform from authority \"%s\" because child_frame_id not set ", authority.c_str());
193 printf(
"TF_NO_FRAME_ID: Ignoring transform with child_frame_id \"%s\" from authority \"%s\" because frame_id not set", stripped.
child_frame_id.c_str(), authority.c_str());
197 if (std::isnan(stripped.getOrigin().x()) ||
198 std::isnan(stripped.getOrigin().y()) ||
199 std::isnan(stripped.getOrigin().z()) ||
200 std::isnan(stripped.getRotation().x()) ||
201 std::isnan(stripped.getRotation().y()) ||
202 std::isnan(stripped.getRotation().z()) ||
203 std::isnan(stripped.getRotation().w()))
205 printf(
"TF_NAN_INPUT: Ignoring transform for child_frame_id \"%s\" " 206 "from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)",
208 stripped.getOrigin().x(), stripped.getOrigin().y(), stripped.getOrigin().z(),
209 stripped.getRotation().x(), stripped.getRotation().y(),
210 stripped.getRotation().z(), stripped.getRotation().w());
220 TimeCacheInterfacePtr frame =
get_frame(frame_number);
230 printf(
"TF_OLD_DATA ignoring data from the past for frame %s " 231 "at time %g according to authority %s\n" 232 "Possible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained",
248 TimeCacheInterfacePtr
251 TimeCacheInterfacePtr frame_ptr =
frames_[cfid];
264 TargetParentOfSource,
265 SourceParentOfTarget,
279 CompactFrameID target_id, CompactFrameID source_id,
280 std::string* error_string)
const 297 CompactFrameID source_id, std::string* error_string,
298 std::vector<CompactFrameID> *frame_chain)
const 301 frame_chain->clear();
304 if (source_id == target_id)
306 f.finalize(Identity, time);
314 if (retval != NO_ERROR)
321 CompactFrameID frame = source_id;
322 CompactFrameID top_parent = frame;
325 std::string extrapolation_error_string;
326 bool extrapolation_might_have_occurred =
false;
330 TimeCacheInterfacePtr cache =
get_frame(frame);
332 frame_chain->push_back(frame);
341 CompactFrameID parent = f.gather(cache, time, &extrapolation_error_string);
346 extrapolation_might_have_occurred =
true;
351 if (frame == target_id)
353 f.finalize(TargetParentOfSource, time);
367 std::stringstream ss;
368 ss <<
"The tf tree is invalid because it contains a loop." << std::endl
370 *error_string = ss.str();
379 std::vector<CompactFrameID> reverse_frame_chain;
381 while (frame != top_parent)
383 TimeCacheInterfacePtr cache =
get_frame(frame);
385 reverse_frame_chain.push_back(frame);
392 CompactFrameID parent = f.gather(cache, time, error_string);
397 std::stringstream ss;
399 *error_string = ss.str();
402 return EXTRAPOLATION_ERROR;
406 if (frame == source_id)
408 f.finalize(SourceParentOfTarget, time);
411 frame_chain->swap(reverse_frame_chain);
425 std::stringstream ss;
426 ss <<
"The tf tree is invalid because it contains a loop." << std::endl
428 *error_string = ss.str();
434 if (frame != top_parent)
436 if (extrapolation_might_have_occurred)
440 std::stringstream ss;
442 *error_string = ss.str();
445 return EXTRAPOLATION_ERROR;
450 return CONNECTIVITY_ERROR;
453 f.finalize(FullPath, time);
457 ssize_t m = reverse_frame_chain.size()-1;
458 ssize_t n = frame_chain->size()-1;
459 for (; m >= 0 && n >= 0; --m, --n)
461 if ((*frame_chain)[n] != reverse_frame_chain[m])
466 frame_chain->erase(frame_chain->begin() + (n-1), frame_chain->end());
468 if (m < (ssize_t)reverse_frame_chain.size())
470 for (
int i = m; i >= 0; --i)
472 frame_chain->push_back(reverse_frame_chain[i]);
482 struct TransformAccum
485 : source_to_top_quat(0.0, 0.0, 0.0, 1.0)
486 , source_to_top_vec(0.0, 0.0, 0.0)
487 , target_to_top_quat(0.0, 0.0, 0.0, 1.0)
488 , target_to_top_vec(0.0, 0.0, 0.0)
489 , result_quat(0.0, 0.0, 0.0, 1.0)
490 , result_vec(0.0, 0.0, 0.0)
494 CompactFrameID gather(TimeCacheInterfacePtr cache,
fawkes::Time time, std::string* error_string)
496 if (!cache->get_data(time, st, error_string))
504 void accum(
bool source)
508 source_to_top_vec = quatRotate(st.rotation, source_to_top_vec) + st.translation;
509 source_to_top_quat = st.rotation * source_to_top_quat;
513 target_to_top_vec = quatRotate(st.rotation, target_to_top_vec) + st.translation;
514 target_to_top_quat = st.rotation * target_to_top_quat;
524 case TargetParentOfSource:
525 result_vec = source_to_top_vec;
526 result_quat = source_to_top_quat;
528 case SourceParentOfTarget:
530 Quaternion inv_target_quat = target_to_top_quat.inverse();
531 Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
532 result_vec = inv_target_vec;
533 result_quat = inv_target_quat;
538 Quaternion inv_target_quat = target_to_top_quat.inverse();
539 Vector3 inv_target_vec = quatRotate(inv_target_quat, -target_to_top_vec);
541 result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
542 result_quat = inv_target_quat * source_to_top_quat;
552 Quaternion source_to_top_quat;
553 Vector3 source_to_top_vec;
554 Quaternion target_to_top_quat;
555 Vector3 target_to_top_vec;
557 Quaternion result_quat;
577 const std::string& source_frame,
583 if (target_frame == source_frame) {
584 transform.setIdentity();
590 TimeCacheInterfacePtr cache =
get_frame(target_id);
592 transform.
stamp = cache->get_latest_timestamp();
594 transform.
stamp = time;
596 transform.
stamp = time;
601 CompactFrameID target_id =
validate_frame_id(
"lookup_transform argument target_frame", target_frame);
602 CompactFrameID source_id =
validate_frame_id(
"lookup_transform argument source_frame", source_frame);
604 std::string error_string;
605 TransformAccum accum;
607 if (retval != NO_ERROR)
611 case CONNECTIVITY_ERROR:
613 case EXTRAPOLATION_ERROR:
623 transform.setOrigin(accum.result_vec);
624 transform.setRotation(accum.result_quat);
627 transform.
stamp = accum.time;
651 const std::string& source_frame,
const fawkes::Time& source_time,
668 struct CanTransformAccum
670 CompactFrameID gather(TimeCacheInterfacePtr cache,
fawkes::Time time, std::string* error_string)
672 return cache->get_parent(time, error_string);
675 void accum(
bool source)
699 if (target_id == 0 || source_id == 0)
704 if (target_id == source_id)
709 CanTransformAccum accum;
746 if (target_frame == source_frame)
749 if (
warn_frame_id(
"canTransform argument target_frame", target_frame))
751 if (
warn_frame_id(
"canTransform argument source_frame", source_frame))
773 const std::string& source_frame,
const fawkes::Time& source_time,
774 const std::string& fixed_frame, std::string* error_msg)
const 776 if (
warn_frame_id(
"canTransform argument target_frame", target_frame))
778 if (
warn_frame_id(
"canTransform argument source_frame", source_frame))
780 if (
warn_frame_id(
"canTransform argument fixed_frame", fixed_frame))
785 can_transform(fixed_frame, source_frame, source_time, error_msg);
795 TimeCacheInterfacePtr
798 if (frame_id >=
frames_.size())
799 return TimeCacheInterfacePtr();
813 CompactFrameID retval;
814 M_StringToCompactFrameID::const_iterator map_it =
frameIDs_.find(frameid_str);
817 retval = CompactFrameID(0);
820 retval = map_it->second;
831 CompactFrameID retval = 0;
832 M_StringToCompactFrameID::iterator map_it =
frameIDs_.find(frameid_str);
835 retval = CompactFrameID(
frames_.size());
836 frames_.push_back(TimeCacheInterfacePtr());
856 throw LookupException(
"Reverse lookup of frame id %u failed!", frame_id_num);
869 CompactFrameID target_frame, std::string* out)
const 875 *out = std::string(
"Could not find a connection between '"+
lookup_frame_string(target_frame)+
"' and '"+
877 "Tf has two or more unconnected trees.");
899 std::stringstream mstream;
906 for (
unsigned int counter = 1; counter <
frames_.size(); counter ++)
908 TimeCacheInterfacePtr frame_ptr =
get_frame(CompactFrameID(counter));
909 if (frame_ptr == NULL)
911 CompactFrameID frame_id_num;
921 return mstream.str();
925 struct TimeAndFrameIDFrameComparator
927 TimeAndFrameIDFrameComparator(CompactFrameID
id)
931 bool operator()(
const P_TimeAndFrameID& rhs)
const 933 return rhs.second == id;
953 if (source_id == 0 || target_id == 0)
return LOOKUP_ERROR;
955 if (source_id == target_id)
957 TimeCacheInterfacePtr cache =
get_frame(source_id);
960 time = cache->get_latest_timestamp();
966 std::vector<P_TimeAndFrameID> lct_cache;
970 CompactFrameID frame = source_id;
971 P_TimeAndFrameID temp;
976 TimeCacheInterfacePtr cache =
get_frame(frame);
984 P_TimeAndFrameID latest = cache->get_latest_time_and_parent();
986 if (latest.second == 0)
992 if (!latest.first.is_zero())
994 common_time = std::min(latest.first, common_time);
997 lct_cache.push_back(latest);
999 frame = latest.second;
1002 if (frame == target_id)
1017 std::stringstream ss;
1018 ss<<
"The tf tree is invalid because it contains a loop." << std::endl
1020 *error_string = ss.str();
1022 return LOOKUP_ERROR;
1030 CompactFrameID common_parent = 0;
1033 TimeCacheInterfacePtr cache =
get_frame(frame);
1040 P_TimeAndFrameID latest = cache->get_latest_time_and_parent();
1042 if (latest.second == 0)
1047 if (!latest.first.is_zero())
1049 common_time = std::min(latest.first, common_time);
1052 std::vector<P_TimeAndFrameID>::iterator it =
1053 std::find_if(lct_cache.begin(), lct_cache.end(),
1059 TimeAndFrameIDFrameComparator(latest.second));
1063 if (it != lct_cache.end())
1065 common_parent = it->second;
1069 frame = latest.second;
1072 if (frame == source_id)
1087 std::stringstream ss;
1088 ss<<
"The tf tree is invalid because it contains a loop." << std::endl
1090 *error_string = ss.str();
1092 return LOOKUP_ERROR;
1096 if (common_parent == 0)
1099 return CONNECTIVITY_ERROR;
1104 std::vector<P_TimeAndFrameID>::iterator it = lct_cache.begin();
1105 std::vector<P_TimeAndFrameID>::iterator end = lct_cache.end();
1106 for (; it != end; ++it)
1108 if (!it->first.is_zero())
1110 common_time = std::min(common_time, it->first);
1113 if (it->second == common_parent)
1137 std::stringstream mstream;
1145 mstream.precision(3);
1146 mstream.setf(std::ios::fixed,std::ios::floatfield);
1149 for (
unsigned int counter = 1; counter <
frames_.size(); counter ++)
1151 CompactFrameID cfid = CompactFrameID(counter);
1152 CompactFrameID frame_id_num;
1153 TimeCacheInterfacePtr cache =
get_frame(cfid);
1166 std::string authority =
"no recorded authority";
1167 std::map<CompactFrameID, std::string>::const_iterator it =
frame_authority_.find(cfid);
1169 authority = it->second;
1172 double rate = cache->get_list_length() / std::max((cache->get_latest_timestamp().in_sec() -
1173 cache->get_oldest_timestamp().in_sec() ), 0.0001);
1175 mstream << std::fixed;
1176 mstream.precision(3);
1178 mstream <<
" parent: '" <<
frameIDs_reverse[frame_id_num] <<
"'" << std::endl;
1179 mstream <<
" broadcaster: '" << authority <<
"'" << std::endl;
1180 mstream <<
" rate: " << rate << std::endl;
1181 mstream <<
" most_recent_transform: " << (cache->get_latest_timestamp()).in_sec() << std::endl;
1182 mstream <<
" oldest_transform: " << (cache->get_oldest_timestamp()).in_sec() << std::endl;
1183 if ( current_time > 0 ) {
1184 mstream <<
" transform_delay: " << current_time - cache->get_latest_timestamp().in_sec() << std::endl;
1186 mstream <<
" buffer_length: " << (cache->get_latest_timestamp() - cache->get_oldest_timestamp()).in_sec() << std::endl;
1189 return mstream.str();
double in_sec() const
Convet time to seconds.
CompactFrameID lookup_or_insert_frame_number(const std::string &frameid_str)
String to number for frame lookup with dynamic allocation of new frames.
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.
std::string all_frames_as_string() const
A way to see what frames have been cached.
Fawkes library namespace.
V_TimeCacheInterface frames_
The pointers to potential frames that the tree can be made of.
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 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.
bool set_transform(const StampedTransform &transform, const std::string &authority, bool is_static=false)
Add transform information to the tf data structure.
void clear()
Clear all data.
CompactFrameID lookup_frame_number(const std::string &frameid_str) const
String to number for frame lookup with dynamic allocation of new frames.
bool warn_frame_id(const char *function_name_arg, const std::string &frame_id) const
Warn if an illegal frame_id was passed.
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.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
A class for handling time.
Passed argument was invalid.
std::map< CompactFrameID, std::string > frame_authority_
A map to lookup the most recent authority for a given frame.
No connection between two frames in tree.
std::string all_frames_as_YAML() const
Get latest frames as YAML.
Time based transform cache.
std::string all_frames_as_string_no_lock() const
A way to see what frames have been cached.
void create_connectivity_error_string(CompactFrameID source_frame, CompactFrameID target_frame, std::string *out) const
Create error string.
Transform cache for static transforms.
TimeCacheInterfacePtr get_frame(CompactFrameID c_frame_id) const
Accessor to get frame cache.
const std::string & lookup_frame_string(CompactFrameID frame_id_num) const
Number to string frame lookup may throw LookupException if number invalid.
std::mutex frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
M_StringToCompactFrameID frameIDs_
Mapping from frame string IDs to compact IDs.
float cache_time_
How long to cache transform history.
const Time TIME_MAX
Instance of Time denoting the maximum value possible.
TimeCacheInterfacePtr allocate_frame(CompactFrameID cfid, bool is_static)
Allocate a new frame cache.
A frame could not be looked up.
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.
BufferCore(float cache_time=DEFAULT_CACHE_TIME)
assuming the tree has a loop
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.
std::vector< std::string > frameIDs_reverse
A map from CompactFrameID frame_id_numbers to string for debugging and output.
static const uint32_t MAX_GRAPH_DEPTH
Maximum number of times to recurse before.