Fawkes API
Fawkes Development Version
|
A Class which provides coordinate transforms between any two frames in a system. More...
#include <buffer_core.h>
Public Member Functions | |
BufferCore (float cache_time=DEFAULT_CACHE_TIME) | |
assuming the tree has a loop More... | |
void | clear () |
Clear all data. More... | |
bool | set_transform (const StampedTransform &transform, const std::string &authority, bool is_static=false) |
Add transform information to the tf data structure. More... | |
void | lookup_transform (const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const |
Lookup transform. More... | |
void | lookup_transform (const std::string &target_frame, const fawkes::Time &target_time, const std::string &source_frame, const fawkes::Time &source_time, const std::string &fixed_frame, StampedTransform &transform) const |
Lookup transform assuming a fixed frame. More... | |
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. More... | |
bool | can_transform (const std::string &target_frame, const fawkes::Time &target_time, const std::string &source_frame, const fawkes::Time &source_time, const std::string &fixed_frame, std::string *error_msg=NULL) const |
Test if a transform is possible. More... | |
std::string | all_frames_as_YAML (double current_time) const |
A way to see what frames have been cached in yaml format Useful for debugging tools. More... | |
std::string | all_frames_as_YAML () const |
Get latest frames as YAML. More... | |
std::string | all_frames_as_string () const |
A way to see what frames have been cached. More... | |
float | get_cache_length () |
Get the duration over which this transformer will cache. More... | |
Static Public Attributes | |
static const int | DEFAULT_CACHE_TIME = 10 |
The default amount of time to cache data in seconds. More... | |
static const uint32_t | MAX_GRAPH_DEPTH = 1000UL |
Maximum number of times to recurse before. More... | |
Protected Types | |
typedef std::vector< TimeCacheInterfacePtr > | V_TimeCacheInterface |
Vector data type for frame caches. More... | |
typedef std::unordered_map< std::string, CompactFrameID > | M_StringToCompactFrameID |
A map from string frame ids to CompactFrameID. More... | |
Protected Member Functions | |
std::string | all_frames_as_string_no_lock () const |
A way to see what frames have been cached. More... | |
TimeCacheInterfacePtr | get_frame (CompactFrameID c_frame_id) const |
Accessor to get frame cache. More... | |
TimeCacheInterfacePtr | allocate_frame (CompactFrameID cfid, bool is_static) |
Allocate a new frame cache. More... | |
bool | warn_frame_id (const char *function_name_arg, const std::string &frame_id) const |
Warn if an illegal frame_id was passed. More... | |
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. More... | |
CompactFrameID | lookup_frame_number (const std::string &frameid_str) const |
String to number for frame lookup with dynamic allocation of new frames. More... | |
CompactFrameID | lookup_or_insert_frame_number (const std::string &frameid_str) |
String to number for frame lookup with dynamic allocation of new frames. More... | |
const std::string & | lookup_frame_string (CompactFrameID frame_id_num) const |
Number to string frame lookup may throw LookupException if number invalid. More... | |
void | create_connectivity_error_string (CompactFrameID source_frame, CompactFrameID target_frame, std::string *out) const |
Create error string. More... | |
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. More... | |
template<typename F > | |
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. More... | |
template<typename F > | |
int | walk_to_top_parent (F &f, fawkes::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string *error_string, std::vector< CompactFrameID > *frame_chain) const |
Traverse transform tree: walk from frame to top-parent of both. More... | |
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. More... | |
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. More... | |
Protected Attributes | |
V_TimeCacheInterface | frames_ |
The pointers to potential frames that the tree can be made of. More... | |
std::mutex | frame_mutex_ |
A mutex to protect testing and allocating new frames on the above vector. More... | |
M_StringToCompactFrameID | frameIDs_ |
Mapping from frame string IDs to compact IDs. More... | |
std::vector< std::string > | frameIDs_reverse |
A map from CompactFrameID frame_id_numbers to string for debugging and output. More... | |
std::map< CompactFrameID, std::string > | frame_authority_ |
A map to lookup the most recent authority for a given frame. More... | |
float | cache_time_ |
How long to cache transform history. More... | |
A Class which provides coordinate transforms between any two frames in a system.
This class provides a simple interface to allow recording and lookup of relationships between arbitrary frames of the system.
libTF assumes that there is a tree of coordinate frame transforms which define the relationship between all coordinate frames. For example your typical robot would have a transform from global to real world. And then from base to hand, and from base to head. But Base to Hand really is composed of base to shoulder to elbow to wrist to hand. libTF is designed to take care of all the intermediate steps for you.
Internal Representation libTF will store frames with the parameters necessary for generating the transform into that frame from it's parent and a reference to the parent frame. Frames are designated using an std::string 0 is a frame without a parent (the top of a tree) The positions of frames over time must be pushed in.
All function calls which pass frame ids can potentially throw the exception tf::LookupException
Definition at line 112 of file buffer_core.h.
|
protected |
A map from string frame ids to CompactFrameID.
Definition at line 177 of file buffer_core.h.
|
protected |
Vector data type for frame caches.
Definition at line 167 of file buffer_core.h.
fawkes::tf::BufferCore::BufferCore | ( | float | cache_time = DEFAULT_CACHE_TIME | ) |
assuming the tree has a loop
Constructor.
cache_time | How long to keep a history of transforms in nanoseconds |
Definition at line 133 of file buffer_core.cpp.
References frameIDs_, frameIDs_reverse, and frames_.
std::string fawkes::tf::BufferCore::all_frames_as_string | ( | ) | const |
A way to see what frames have been cached.
Useful for debugging
Definition at line 886 of file buffer_core.cpp.
References all_frames_as_string_no_lock(), and frame_mutex_.
|
protected |
A way to see what frames have been cached.
Useful for debugging. Use this call internally.
Useful for debugging. Use this call internally.
regular transforms
Definition at line 897 of file buffer_core.cpp.
References fawkes::tf::TransformStorage::frame_id, frameIDs_reverse, frames_, and get_frame().
Referenced by all_frames_as_string(), get_cache_length(), get_latest_common_time(), and walk_to_top_parent().
std::string fawkes::tf::BufferCore::all_frames_as_YAML | ( | double | current_time | ) | const |
A way to see what frames have been cached in yaml format Useful for debugging tools.
current_time | current time to compute delay |
Definition at line 1135 of file buffer_core.cpp.
References frame_authority_, fawkes::tf::TransformStorage::frame_id, frame_mutex_, frameIDs_reverse, frames_, and get_frame().
std::string fawkes::tf::BufferCore::all_frames_as_YAML | ( | ) | const |
|
protected |
Allocate a new frame cache.
cfid | frame ID for which to create the frame cache |
is_static | true if the transforms for this frame are static, false otherwise |
Definition at line 249 of file buffer_core.cpp.
References cache_time_, and frames_.
Referenced by set_transform().
bool fawkes::tf::BufferCore::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.
target_frame | The frame into which to transform |
source_frame | The frame from which to transform |
time | The time at which to transform |
error_msg | A pointer to a string which will be filled with why the transform failed, if not NULL |
Definition at line 742 of file buffer_core.cpp.
References can_transform_no_lock(), frame_mutex_, lookup_frame_number(), and warn_frame_id().
Referenced by fawkes::tf::Transformer::can_transform(), and can_transform().
bool fawkes::tf::BufferCore::can_transform | ( | const std::string & | target_frame, |
const fawkes::Time & | target_time, | ||
const std::string & | source_frame, | ||
const fawkes::Time & | source_time, | ||
const std::string & | fixed_frame, | ||
std::string * | error_msg = NULL |
||
) | const |
Test if a transform is possible.
target_frame | The frame into which to transform |
target_time | The time into which to transform |
source_frame | The frame from which to transform |
source_time | The time from which to transform |
fixed_frame | The frame in which to treat the transform as constant in time |
error_msg | A pointer to a string which will be filled with why the transform failed, if not NULL |
Definition at line 772 of file buffer_core.cpp.
References can_transform(), and warn_frame_id().
|
protected |
Test if a transform is possible.
Internal check that does lock the frame mutex.
target_id | The frame number into which to transform |
source_id | The frame number from which to transform |
time | The time at which to transform |
error_msg | passed to walk_to_top_parent |
Definition at line 727 of file buffer_core.cpp.
References can_transform_no_lock(), and frame_mutex_.
|
protected |
Test if a transform is possible.
Internal check that does not lock the frame mutex.
target_id | The frame number into which to transform |
source_id | The frame number from which to transform |
time | The time at which to transform |
error_msg | passed to walk_to_top_parent |
Definition at line 696 of file buffer_core.cpp.
References walk_to_top_parent().
Referenced by can_transform(), and can_transform_internal().
void fawkes::tf::BufferCore::clear | ( | void | ) |
Clear all data.
Definition at line 147 of file buffer_core.cpp.
References frame_mutex_, and frames_.
|
protected |
Create error string.
source_frame | compact ID of source frame |
target_frame | compact ID of target frame |
out | upon return contains error string if non-NULL |
Definition at line 868 of file buffer_core.cpp.
References lookup_frame_string().
Referenced by get_latest_common_time(), and walk_to_top_parent().
|
inline |
Get the duration over which this transformer will cache.
Definition at line 155 of file buffer_core.h.
References all_frames_as_string_no_lock(), and cache_time_.
|
protected |
Accessor to get frame cache.
This is an internal function which will get the pointer to the frame associated with the frame id
frame_id | The frameID of the desired Reference Frame |
Definition at line 796 of file buffer_core.cpp.
References frames_.
Referenced by fawkes::tf::Transformer::all_frames_as_dot(), all_frames_as_string_no_lock(), all_frames_as_YAML(), fawkes::tf::Transformer::get_frame_cache(), get_latest_common_time(), lookup_transform(), set_transform(), and walk_to_top_parent().
|
protected |
Get latest common time of two frames.
target_id | target frame number |
source_id | source frame number |
time | upon return contains latest common timestamp |
error_string | upon error contains accumulated error message. |
Definition at line 949 of file buffer_core.cpp.
References all_frames_as_string_no_lock(), create_connectivity_error_string(), get_frame(), MAX_GRAPH_DEPTH, and fawkes::TIME_MAX.
Referenced by walk_to_top_parent().
|
protected |
String to number for frame lookup with dynamic allocation of new frames.
Get compact ID for frame.
frameid_str | frame ID string |
Definition at line 811 of file buffer_core.cpp.
References frameIDs_.
Referenced by can_transform(), fawkes::tf::Transformer::get_frame_cache(), lookup_transform(), and validate_frame_id().
|
protected |
Number to string frame lookup may throw LookupException if number invalid.
Get frame string for compact frame ID.
frame_id_num | compact frame ID |
LookupException | thrown if compact frame ID unknown |
Definition at line 852 of file buffer_core.cpp.
References frameIDs_reverse.
Referenced by create_connectivity_error_string(), and walk_to_top_parent().
|
protected |
String to number for frame lookup with dynamic allocation of new frames.
Get compact ID for frame or create if not existant.
frameid_str | frame ID string |
Definition at line 829 of file buffer_core.cpp.
References frameIDs_, frameIDs_reverse, and frames_.
Referenced by set_transform().
void fawkes::tf::BufferCore::lookup_transform | ( | const std::string & | target_frame, |
const std::string & | source_frame, | ||
const fawkes::Time & | time, | ||
StampedTransform & | transform | ||
) | const |
Lookup transform.
target_frame | target frame ID |
source_frame | source frame ID |
time | time for which to get the transform, set to (0,0) to get latest common time frame |
transform | upon return contains the transform |
ConnectivityException | thrown if no connection between the source and target frame could be found in the tree. |
ExtrapolationException | returning a value would have required extrapolation beyond current limits. |
LookupException | at least one of the two given frames is unknown |
Definition at line 576 of file buffer_core.cpp.
References fawkes::tf::StampedTransform::child_frame_id, fawkes::tf::StampedTransform::frame_id, frame_mutex_, get_frame(), lookup_frame_number(), fawkes::tf::StampedTransform::stamp, validate_frame_id(), and walk_to_top_parent().
Referenced by fawkes::tf::Transformer::lookup_transform(), and lookup_transform().
void fawkes::tf::BufferCore::lookup_transform | ( | const std::string & | target_frame, |
const fawkes::Time & | target_time, | ||
const std::string & | source_frame, | ||
const fawkes::Time & | source_time, | ||
const std::string & | fixed_frame, | ||
StampedTransform & | transform | ||
) | const |
Lookup transform assuming a fixed frame.
This will lookup a transformation from source to target, assuming that there is a fixed frame, by first finding the transform of the source frame in the fixed frame, and then a transformation from the fixed frame to the target frame.
target_frame | target frame ID |
target_time | time for the target frame |
source_frame | source frame ID |
source_time | time in the source frame |
fixed_frame | ID of fixed frame |
transform | upon return contains the transform |
ConnectivityException | thrown if no connection between the source and target frame could be found in the tree. |
ExtrapolationException | returning a value would have required extrapolation beyond current limits. |
LookupException | at least one of the two given frames is unknown |
Definition at line 650 of file buffer_core.cpp.
References fawkes::tf::StampedTransform::child_frame_id, fawkes::tf::StampedTransform::frame_id, lookup_transform(), fawkes::tf::StampedTransform::set_data(), fawkes::tf::StampedTransform::stamp, and validate_frame_id().
bool fawkes::tf::BufferCore::set_transform | ( | const StampedTransform & | transform_in, |
const std::string & | authority, | ||
bool | is_static = false |
||
) |
Add transform information to the tf data structure.
transform_in | The transform to store |
authority | The source of the information for this transform |
is_static | Record this transform as a static transform. It will be good across all time. (This cannot be changed after the first call.) |
Definition at line 171 of file buffer_core.cpp.
References allocate_frame(), fawkes::tf::StampedTransform::child_frame_id, frame_authority_, fawkes::tf::StampedTransform::frame_id, frame_mutex_, get_frame(), fawkes::Time::in_sec(), lookup_or_insert_frame_number(), and fawkes::tf::StampedTransform::stamp.
Referenced by fawkes::tf::TransformListener::bb_interface_data_changed(), and fawkes::tf::MongoDBTransformer::restore().
|
protected |
Check if frame ID is valid and return compact ID.
function_name_arg | informational for error message |
frame_id | frame ID to validate |
InvalidArgumentException | thrown if argument s invalid, i.e. if it is empty or starts with a slash. |
LookupException | thrown if frame does not exist |
Definition at line 105 of file buffer_core.cpp.
References lookup_frame_number().
Referenced by lookup_transform().
|
protected |
Traverse transform tree: walk from frame to top-parent of both.
f | accumulator |
time | timestamp |
target_id | frame number of target |
source_id | frame number of source |
error_string | accumulated error string |
Definition at line 278 of file buffer_core.cpp.
Referenced by can_transform_no_lock(), and lookup_transform().
|
protected |
Traverse transform tree: walk from frame to top-parent of both.
f | accumulator |
time | timestamp |
target_id | frame number of target |
source_id | frame number of source |
error_string | accumulated error string |
frame_chain | If frame_chain is not NULL, store the traversed frame tree in vector frame_chain. |
Definition at line 296 of file buffer_core.cpp.
References all_frames_as_string_no_lock(), create_connectivity_error_string(), get_frame(), get_latest_common_time(), lookup_frame_string(), and MAX_GRAPH_DEPTH.
|
protected |
Warn if an illegal frame_id was passed.
function_name_arg | informative message content |
frame_id | frame ID to check |
Definition at line 79 of file buffer_core.cpp.
Referenced by can_transform().
|
protected |
How long to cache transform history.
Definition at line 187 of file buffer_core.h.
Referenced by allocate_frame(), get_cache_length(), fawkes::tf::Transformer::get_cache_time(), and fawkes::tf::MongoDBTransformer::restore().
|
static |
The default amount of time to cache data in seconds.
Definition at line 115 of file buffer_core.h.
|
protected |
A map to lookup the most recent authority for a given frame.
Definition at line 183 of file buffer_core.h.
Referenced by fawkes::tf::Transformer::all_frames_as_dot(), all_frames_as_YAML(), and set_transform().
|
mutableprotected |
A mutex to protect testing and allocating new frames on the above vector.
Definition at line 174 of file buffer_core.h.
Referenced by fawkes::tf::Transformer::all_frames_as_dot(), all_frames_as_string(), all_frames_as_YAML(), can_transform(), can_transform_internal(), clear(), fawkes::tf::Transformer::frame_exists(), fawkes::tf::Transformer::lock(), lookup_transform(), set_transform(), fawkes::tf::Transformer::try_lock(), and fawkes::tf::Transformer::unlock().
|
protected |
Mapping from frame string IDs to compact IDs.
Definition at line 179 of file buffer_core.h.
Referenced by BufferCore(), fawkes::tf::Transformer::frame_exists(), lookup_frame_number(), and lookup_or_insert_frame_number().
|
protected |
A map from CompactFrameID frame_id_numbers to string for debugging and output.
Definition at line 181 of file buffer_core.h.
Referenced by fawkes::tf::Transformer::all_frames_as_dot(), all_frames_as_string_no_lock(), all_frames_as_YAML(), BufferCore(), fawkes::tf::Transformer::get_frame_id_mappings(), lookup_frame_string(), and lookup_or_insert_frame_number().
|
protected |
The pointers to potential frames that the tree can be made of.
The frames will be dynamically allocated at run time when set the first time.
Definition at line 171 of file buffer_core.h.
Referenced by fawkes::tf::Transformer::all_frames_as_dot(), all_frames_as_string_no_lock(), all_frames_as_YAML(), allocate_frame(), BufferCore(), clear(), get_frame(), fawkes::tf::Transformer::get_frame_caches(), and lookup_or_insert_frame_number().
|
static |
Maximum number of times to recurse before.
Definition at line 116 of file buffer_core.h.
Referenced by get_latest_common_time(), and walk_to_top_parent().