Fawkes API  Fawkes Development Version
fawkes::tf::BufferCore Class Reference

A Class which provides coordinate transforms between any two frames in a system. More...

#include <buffer_core.h>

Inheritance diagram for fawkes::tf::BufferCore:

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...
 

Detailed Description

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.

Member Typedef Documentation

◆ M_StringToCompactFrameID

typedef std::unordered_map<std::string, CompactFrameID> fawkes::tf::BufferCore::M_StringToCompactFrameID
protected

A map from string frame ids to CompactFrameID.

Definition at line 177 of file buffer_core.h.

◆ V_TimeCacheInterface

typedef std::vector<TimeCacheInterfacePtr> fawkes::tf::BufferCore::V_TimeCacheInterface
protected

Vector data type for frame caches.

Definition at line 167 of file buffer_core.h.

Constructor & Destructor Documentation

◆ BufferCore()

fawkes::tf::BufferCore::BufferCore ( float  cache_time = DEFAULT_CACHE_TIME)

assuming the tree has a loop

Constructor.

Parameters
cache_timeHow long to keep a history of transforms in nanoseconds

Definition at line 133 of file buffer_core.cpp.

References frameIDs_, frameIDs_reverse, and frames_.

Member Function Documentation

◆ all_frames_as_string()

std::string fawkes::tf::BufferCore::all_frames_as_string ( ) const

A way to see what frames have been cached.

Useful for debugging

Returns
all current frames as a single string

Definition at line 886 of file buffer_core.cpp.

References all_frames_as_string_no_lock(), and frame_mutex_.

◆ all_frames_as_string_no_lock()

std::string fawkes::tf::BufferCore::all_frames_as_string_no_lock ( ) const
protected

A way to see what frames have been cached.

Useful for debugging. Use this call internally.

Useful for debugging. Use this call internally.

Returns
all current frames as a single string

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().

◆ all_frames_as_YAML() [1/2]

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.

Parameters
current_timecurrent time to compute delay
Returns
YAML string

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().

◆ all_frames_as_YAML() [2/2]

std::string fawkes::tf::BufferCore::all_frames_as_YAML ( ) const

Get latest frames as YAML.

Returns
YAML string

Definition at line 1196 of file buffer_core.cpp.

◆ allocate_frame()

TimeCacheInterfacePtr fawkes::tf::BufferCore::allocate_frame ( CompactFrameID  cfid,
bool  is_static 
)
protected

Allocate a new frame cache.

Parameters
cfidframe ID for which to create the frame cache
is_statictrue if the transforms for this frame are static, false otherwise
Returns
pointer to new cache

Definition at line 249 of file buffer_core.cpp.

References cache_time_, and frames_.

Referenced by set_transform().

◆ can_transform() [1/2]

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.

Parameters
target_frameThe frame into which to transform
source_frameThe frame from which to transform
timeThe time at which to transform
error_msgA pointer to a string which will be filled with why the transform failed, if not NULL
Returns
True if the transform is possible, false otherwise

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().

◆ can_transform() [2/2]

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.

Parameters
target_frameThe frame into which to transform
target_timeThe time into which to transform
source_frameThe frame from which to transform
source_timeThe time from which to transform
fixed_frameThe frame in which to treat the transform as constant in time
error_msgA pointer to a string which will be filled with why the transform failed, if not NULL
Returns
true if the transform is possible, false otherwise

Definition at line 772 of file buffer_core.cpp.

References can_transform(), and warn_frame_id().

◆ can_transform_internal()

bool fawkes::tf::BufferCore::can_transform_internal ( CompactFrameID  target_id,
CompactFrameID  source_id,
const fawkes::Time time,
std::string *  error_msg 
) const
protected

Test if a transform is possible.

Internal check that does lock the frame mutex.

Parameters
target_idThe frame number into which to transform
source_idThe frame number from which to transform
timeThe time at which to transform
error_msgpassed to walk_to_top_parent
Returns
true if the transformation can be calculated, false otherwise

Definition at line 727 of file buffer_core.cpp.

References can_transform_no_lock(), and frame_mutex_.

◆ can_transform_no_lock()

bool fawkes::tf::BufferCore::can_transform_no_lock ( CompactFrameID  target_id,
CompactFrameID  source_id,
const fawkes::Time time,
std::string *  error_msg 
) const
protected

Test if a transform is possible.

Internal check that does not lock the frame mutex.

Parameters
target_idThe frame number into which to transform
source_idThe frame number from which to transform
timeThe time at which to transform
error_msgpassed to walk_to_top_parent
Returns
true if the transformation can be calculated, false otherwise

Definition at line 696 of file buffer_core.cpp.

References walk_to_top_parent().

Referenced by can_transform(), and can_transform_internal().

◆ clear()

void fawkes::tf::BufferCore::clear ( void  )

Clear all data.

Definition at line 147 of file buffer_core.cpp.

References frame_mutex_, and frames_.

◆ create_connectivity_error_string()

void fawkes::tf::BufferCore::create_connectivity_error_string ( CompactFrameID  source_frame,
CompactFrameID  target_frame,
std::string *  out 
) const
protected

Create error string.

Parameters
source_framecompact ID of source frame
target_framecompact ID of target frame
outupon 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().

◆ get_cache_length()

float fawkes::tf::BufferCore::get_cache_length ( )
inline

Get the duration over which this transformer will cache.

Returns
cache length in seconds

Definition at line 155 of file buffer_core.h.

References all_frames_as_string_no_lock(), and cache_time_.

◆ get_frame()

TimeCacheInterfacePtr fawkes::tf::BufferCore::get_frame ( CompactFrameID  frame_id) const
protected

Accessor to get frame cache.

This is an internal function which will get the pointer to the frame associated with the frame id

Parameters
frame_idThe frameID of the desired Reference Frame
Returns
shared pointer to time cache

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().

◆ get_latest_common_time()

int fawkes::tf::BufferCore::get_latest_common_time ( CompactFrameID  target_id,
CompactFrameID  source_id,
fawkes::Time time,
std::string *  error_string 
) const
protected

Get latest common time of two frames.

Parameters
target_idtarget frame number
source_idsource frame number
timeupon return contains latest common timestamp
error_stringupon error contains accumulated error message.
Returns
value from ErrorValues

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().

◆ lookup_frame_number()

CompactFrameID fawkes::tf::BufferCore::lookup_frame_number ( const std::string &  frameid_str) const
protected

String to number for frame lookup with dynamic allocation of new frames.

Get compact ID for frame.

Parameters
frameid_strframe ID string
Returns
compact frame ID, zero if frame unknown

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().

◆ lookup_frame_string()

const std::string & fawkes::tf::BufferCore::lookup_frame_string ( CompactFrameID  frame_id_num) const
protected

Number to string frame lookup may throw LookupException if number invalid.

Get frame string for compact frame ID.

Parameters
frame_id_numcompact frame ID
Returns
string for frame ID
Exceptions
LookupExceptionthrown 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().

◆ lookup_or_insert_frame_number()

CompactFrameID fawkes::tf::BufferCore::lookup_or_insert_frame_number ( const std::string &  frameid_str)
protected

String to number for frame lookup with dynamic allocation of new frames.

Get compact ID for frame or create if not existant.

Parameters
frameid_strframe ID string
Returns
compact frame ID

Definition at line 829 of file buffer_core.cpp.

References frameIDs_, frameIDs_reverse, and frames_.

Referenced by set_transform().

◆ lookup_transform() [1/2]

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.

Parameters
target_frametarget frame ID
source_framesource frame ID
timetime for which to get the transform, set to (0,0) to get latest common time frame
transformupon return contains the transform
Exceptions
ConnectivityExceptionthrown if no connection between the source and target frame could be found in the tree.
ExtrapolationExceptionreturning a value would have required extrapolation beyond current limits.
LookupExceptionat 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().

◆ lookup_transform() [2/2]

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.

Parameters
target_frametarget frame ID
target_timetime for the target frame
source_framesource frame ID
source_timetime in the source frame
fixed_frameID of fixed frame
transformupon return contains the transform
Exceptions
ConnectivityExceptionthrown if no connection between the source and target frame could be found in the tree.
ExtrapolationExceptionreturning a value would have required extrapolation beyond current limits.
LookupExceptionat 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().

◆ set_transform()

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.

Parameters
transform_inThe transform to store
authorityThe source of the information for this transform
is_staticRecord this transform as a static transform. It will be good across all time. (This cannot be changed after the first call.)
Returns
true unless an error occured

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().

◆ validate_frame_id()

CompactFrameID fawkes::tf::BufferCore::validate_frame_id ( const char *  function_name_arg,
const std::string &  frame_id 
) const
protected

Check if frame ID is valid and return compact ID.

Parameters
function_name_arginformational for error message
frame_idframe ID to validate
Returns
compact frame ID, if frame is valid
Exceptions
InvalidArgumentExceptionthrown if argument s invalid, i.e. if it is empty or starts with a slash.
LookupExceptionthrown if frame does not exist

Definition at line 105 of file buffer_core.cpp.

References lookup_frame_number().

Referenced by lookup_transform().

◆ walk_to_top_parent() [1/2]

template<typename F >
int fawkes::tf::BufferCore::walk_to_top_parent ( F &  f,
fawkes::Time  time,
CompactFrameID  target_id,
CompactFrameID  source_id,
std::string *  error_string 
) const
protected

Traverse transform tree: walk from frame to top-parent of both.

Parameters
faccumulator
timetimestamp
target_idframe number of target
source_idframe number of source
error_stringaccumulated error string
Returns
error flag from ErrorValues

Definition at line 278 of file buffer_core.cpp.

Referenced by can_transform_no_lock(), and lookup_transform().

◆ walk_to_top_parent() [2/2]

template<typename F >
int fawkes::tf::BufferCore::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
protected

Traverse transform tree: walk from frame to top-parent of both.

Parameters
faccumulator
timetimestamp
target_idframe number of target
source_idframe number of source
error_stringaccumulated error string
frame_chainIf frame_chain is not NULL, store the traversed frame tree in vector frame_chain.
Returns
error flag from ErrorValues

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.

◆ warn_frame_id()

bool fawkes::tf::BufferCore::warn_frame_id ( const char *  function_name_arg,
const std::string &  frame_id 
) const
protected

Warn if an illegal frame_id was passed.

Parameters
function_name_arginformative message content
frame_idframe ID to check
Returns
true if we have something to warn about, false otherwise

Definition at line 79 of file buffer_core.cpp.

Referenced by can_transform().

Member Data Documentation

◆ cache_time_

float fawkes::tf::BufferCore::cache_time_
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().

◆ DEFAULT_CACHE_TIME

const int fawkes::tf::BufferCore::DEFAULT_CACHE_TIME = 10
static

The default amount of time to cache data in seconds.

Definition at line 115 of file buffer_core.h.

◆ frame_authority_

std::map<CompactFrameID, std::string> fawkes::tf::BufferCore::frame_authority_
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().

◆ frame_mutex_

◆ frameIDs_

M_StringToCompactFrameID fawkes::tf::BufferCore::frameIDs_
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().

◆ frameIDs_reverse

std::vector<std::string> fawkes::tf::BufferCore::frameIDs_reverse
protected

◆ frames_

V_TimeCacheInterface fawkes::tf::BufferCore::frames_
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().

◆ MAX_GRAPH_DEPTH

const uint32_t fawkes::tf::BufferCore::MAX_GRAPH_DEPTH = 1000UL
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().


The documentation for this class was generated from the following files: