52 #include <tf/transformer.h> 53 #include <tf/time_cache.h> 54 #include <tf/exceptions.h> 57 #include <core/threading/mutex_locker.h> 58 #include <core/macros.h> 183 return (
frameIDs_.count(frame_id_str) > 0);
191 TimeCacheInterfacePtr
200 std::vector<TimeCacheInterfacePtr>
210 std::vector<std::string>
227 const std::string& source_frame,
229 std::string* error_msg)
const 231 if (likely(enabled_)) {
232 std::string stripped_target = strip_slash(target_frame);
233 std::string stripped_source = strip_slash(source_frame);
253 const std::string& source_frame,
255 const std::string& fixed_frame,
256 std::string* error_msg)
const 258 if (likely(enabled_)) {
259 std::string stripped_target = strip_slash(target_frame);
260 std::string stripped_source = strip_slash(source_frame);
262 stripped_source, source_time,
263 fixed_frame, error_msg);
283 const std::string& source_frame,
291 std::string stripped_target = strip_slash(target_frame);
292 std::string stripped_source = strip_slash(source_frame);
318 const std::string& source_frame,
320 const std::string& fixed_frame,
326 std::string stripped_target = strip_slash(target_frame);
327 std::string stripped_source = strip_slash(source_frame);
330 stripped_source, source_time,
331 fixed_frame, transform);
346 const std::string& source_frame,
373 assert_quaternion_valid(stamped_in);
377 stamped_in.
stamp, transform);
379 stamped_out.
set_data(transform * stamped_in);
381 stamped_out.
frame_id = target_frame;
404 stamped_in.
stamp, transform);
407 Vector3 end = stamped_in;
408 Vector3 origin = Vector3(0,0,0);
409 Vector3 output = (transform * end) - (transform * origin);
413 stamped_out.
frame_id = target_frame;
436 stamped_in.
stamp, transform);
438 stamped_out.
set_data(transform * stamped_in);
440 stamped_out.
frame_id = target_frame;
464 stamped_in.
stamp, transform);
466 stamped_out.
set_data(transform * stamped_in);
468 stamped_out.
frame_id = target_frame;
493 const std::string& target_frame,
531 const std::string& fixed_frame,
534 assert_quaternion_valid(stamped_in);
538 fixed_frame, transform);
540 stamped_out.
set_data(transform * stamped_in);
542 stamped_out.
frame_id = target_frame;
568 const std::string& fixed_frame,
574 fixed_frame, transform);
577 Vector3 end = stamped_in;
578 Vector3 origin(0,0,0);
579 Vector3 output = (transform * end) - (transform * origin);
583 stamped_out.
frame_id = target_frame;
610 const std::string& fixed_frame,
616 fixed_frame, transform);
618 stamped_out.
set_data(transform * stamped_in);
620 stamped_out.
frame_id = target_frame;
646 const std::string& fixed_frame,
652 fixed_frame, transform);
654 stamped_out.
set_data(transform * stamped_in);
656 stamped_out.
frame_id = target_frame;
671 if (time) *time = current_time;
673 std::stringstream mstream;
674 mstream << std::fixed;
675 mstream.precision(3);
676 mstream <<
"digraph { graph [fontsize=14";
678 mstream <<
", label=\"\\nRecorded at time: " 679 << current_time.
str() <<
" (" << current_time.
in_sec() <<
")\"";
681 mstream <<
"]; node [fontsize=12]; edge [fontsize=12]; " << std::endl;
686 mstream <<
"\"no tf data received\"";
688 mstream.precision(3);
689 mstream.setf(std::ios::fixed, std::ios::floatfield);
692 for (
unsigned int cnt = 1; cnt <
frames_.size(); ++cnt)
694 std::shared_ptr<TimeCacheInterface> cache =
get_frame(cnt);
695 if (! cache)
continue;
697 unsigned int frame_id_num;
703 if (frame_id_num != 0) {
704 std::string authority =
"no recorded authority";
705 std::map<unsigned int, std::string>::const_iterator it =
frame_authority_.find(cnt);
707 authority = it->second;
709 double rate = cache->get_list_length() /
710 std::max((cache->get_latest_timestamp().in_sec() -
711 cache->get_oldest_timestamp().in_sec()), 0.0001);
716 std::shared_ptr<StaticCache> static_cache =
722 mstream <<
"Average rate: " << rate <<
" Hz\\n" 723 <<
"Most recent transform: " 724 << (current_time - cache->get_latest_timestamp()).in_sec() <<
" sec old \\n" 726 << (cache->get_latest_timestamp() - cache->get_oldest_timestamp()).in_sec()
729 mstream <<
"\"];" <<std::endl;
734 return mstream.str();
std::string frame_id
The frame_id associated this data.
double in_sec() const
Convet time to seconds.
const char * str(bool utc=false) const
Output function.
Fawkes library namespace.
V_TimeCacheInterface frames_
The pointers to potential frames that the tree can be made of.
CompactFrameID lookup_frame_number(const std::string &frameid_str) const
String to number for frame lookup with dynamic allocation of new frames.
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.
std::map< CompactFrameID, std::string > frame_authority_
A map to lookup the most recent authority for a given frame.
A Class which provides coordinate transforms between any two frames in a system.
fawkes::Time stamp
The timestamp associated with this data.
Base class for exceptions in Fawkes.
Transform cache for static transforms.
TimeCacheInterfacePtr get_frame(CompactFrameID c_frame_id) const
Accessor to get frame cache.
std::mutex frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
Wrapper class to add time stamp and frame ID to base types.
M_StringToCompactFrameID frameIDs_
Mapping from frame string IDs to compact IDs.
float cache_time_
How long to cache transform history.
The requested feature is disabled.
void set_data(const T &input)
Set the data element.
std::vector< std::string > frameIDs_reverse
A map from CompactFrameID frame_id_numbers to string for debugging and output.