52 #include <core/macros.h>
53 #include <core/threading/mutex_locker.h>
54 #include <tf/exceptions.h>
55 #include <tf/time_cache.h>
56 #include <tf/transformer.h>
170 return (
frameIDs_.count(frame_id_str) > 0);
177 TimeCacheInterfacePtr
186 std::vector<TimeCacheInterfacePtr>
195 std::vector<std::string>
211 const std::string & source_frame,
213 std::string * error_msg)
const
215 if (likely(enabled_)) {
216 std::string stripped_target = strip_slash(target_frame);
217 std::string stripped_source = strip_slash(source_frame);
237 const std::string & source_frame,
239 const std::string & fixed_frame,
240 std::string * error_msg)
const
242 if (likely(enabled_)) {
243 std::string stripped_target = strip_slash(target_frame);
244 std::string stripped_source = strip_slash(source_frame);
246 stripped_target, target_time, stripped_source, source_time, fixed_frame, error_msg);
266 const std::string & source_frame,
274 std::string stripped_target = strip_slash(target_frame);
275 std::string stripped_source = strip_slash(source_frame);
301 const std::string & source_frame,
303 const std::string & fixed_frame,
309 std::string stripped_target = strip_slash(target_frame);
310 std::string stripped_source = strip_slash(source_frame);
313 stripped_target, target_time, stripped_source, source_time, fixed_frame, transform);
329 const std::string &source_frame,
355 assert_quaternion_valid(stamped_in);
360 stamped_out.
set_data(transform * stamped_in);
362 stamped_out.
frame_id = target_frame;
387 Vector3 end = stamped_in;
388 Vector3 origin = Vector3(0, 0, 0);
389 Vector3 output = (transform * end) - (transform * origin);
393 stamped_out.
frame_id = target_frame;
417 stamped_out.
set_data(transform * stamped_in);
419 stamped_out.
frame_id = target_frame;
443 stamped_out.
set_data(transform * stamped_in);
445 stamped_out.
frame_id = target_frame;
469 const std::string &target_frame,
507 const std::string & fixed_frame,
510 assert_quaternion_valid(stamped_in);
513 target_frame, target_time, stamped_in.
frame_id, stamped_in.
stamp, fixed_frame, transform);
515 stamped_out.
set_data(transform * stamped_in);
517 stamped_out.
frame_id = target_frame;
543 const std::string & fixed_frame,
548 target_frame, target_time, stamped_in.
frame_id, stamped_in.
stamp, fixed_frame, transform);
551 Vector3 end = stamped_in;
552 Vector3 origin(0, 0, 0);
553 Vector3 output = (transform * end) - (transform * origin);
557 stamped_out.
frame_id = target_frame;
583 const std::string & fixed_frame,
588 target_frame, target_time, stamped_in.
frame_id, stamped_in.
stamp, fixed_frame, transform);
590 stamped_out.
set_data(transform * stamped_in);
592 stamped_out.
frame_id = target_frame;
618 const std::string & fixed_frame,
623 target_frame, target_time, stamped_in.
frame_id, stamped_in.
stamp, fixed_frame, transform);
625 stamped_out.
set_data(transform * stamped_in);
627 stamped_out.
frame_id = target_frame;
642 *time = current_time;
644 std::stringstream mstream;
645 mstream << std::fixed;
646 mstream.precision(3);
647 mstream <<
"digraph { graph [fontsize=14";
649 mstream <<
", label=\"\\nRecorded at time: " << current_time.
str() <<
" ("
650 << current_time.
in_sec() <<
")\"";
652 mstream <<
"]; node [fontsize=12]; edge [fontsize=12]; " << std::endl;
657 mstream <<
"\"no tf data received\"";
659 mstream.precision(3);
660 mstream.setf(std::ios::fixed, std::ios::floatfield);
663 for (
unsigned int cnt = 1; cnt <
frames_.size(); ++cnt)
665 std::shared_ptr<TimeCacheInterface> cache =
get_frame(cnt);
669 unsigned int frame_id_num;
675 if (frame_id_num != 0) {
676 std::string authority =
"no recorded authority";
677 std::map<unsigned int, std::string>::const_iterator it =
frame_authority_.find(cnt);
679 authority = it->second;
681 double rate = cache->get_list_length()
682 / std::max((cache->get_latest_timestamp().in_sec()
683 - cache->get_oldest_timestamp().in_sec()),
691 std::shared_ptr<StaticCache> static_cache = std::dynamic_pointer_cast<StaticCache>(cache);
696 mstream <<
"Average rate: " << rate <<
" Hz\\n"
697 <<
"Most recent transform: "
698 << (current_time - cache->get_latest_timestamp()).in_sec() <<
" sec old \\n"
700 << (cache->get_latest_timestamp() - cache->get_oldest_timestamp()).in_sec()
703 mstream <<
"\"];" << std::endl;
708 return mstream.str();
Base class for exceptions in Fawkes.
A class for handling time.
double in_sec() const
Convet time to seconds.
const char * str(bool utc=false) const
Output function.
A Class which provides coordinate transforms between any two frames in a system.
float cache_time_
How long to cache transform history.
CompactFrameID lookup_frame_number(const std::string &frameid_str) const
String to number for frame lookup with dynamic allocation of new frames.
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
std::map< CompactFrameID, std::string > frame_authority_
A map to lookup the most recent authority for a given frame.
M_StringToCompactFrameID frameIDs_
Mapping from frame string IDs to compact IDs.
std::mutex frame_mutex_
A mutex to protect testing and allocating new frames on the above vector.
TimeCacheInterfacePtr get_frame(CompactFrameID c_frame_id) const
Accessor to get frame cache.
V_TimeCacheInterface frames_
The pointers to potential frames that the tree can be made of.
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.
std::vector< std::string > frameIDs_reverse
A map from CompactFrameID frame_id_numbers to string for debugging and output.
The requested feature is disabled.
Wrapper class to add time stamp and frame ID to base types.
std::string frame_id
The frame_id associated this data.
fawkes::Time stamp
The timestamp associated with this data.
void set_data(const T &input)
Set the data element.
Fawkes library namespace.