A Class which provides coordinate transforms between any two frames in a system. More...
#include <tf.h>
Public Member Functions | |
boost::signals::connection | addTransformsChangedListener (boost::function< void(void)> callback) |
Add a callback that happens when a new transform has arrived. | |
std::string | allFramesAsDot () const |
A way to see what frames have been cached Useful for debugging. | |
std::string | allFramesAsString () const |
A way to see what frames have been cached Useful for debugging. | |
bool | canTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const |
Test if a transform is possible. | |
bool | canTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, std::string *error_msg=NULL) const |
Test if a transform is possible. | |
void | chainAsVector (const std::string &target_frame, ros::Time target_time, const std::string &source_frame, ros::Time source_time, const std::string &fixed_frame, std::vector< std::string > &output) const |
Debugging function that will print the spanning chain of transforms. Possible exceptions tf::LookupException, tf::ConnectivityException, tf::MaxDepthException. | |
void | clear () |
Clear all data. | |
bool | frameExists (const std::string &frame_id_str) const |
Check if a frame exists in the tree. | |
ros::Duration | getCacheLength () |
Get the duration over which this transformer will cache. | |
void | getFrameStrings (std::vector< std::string > &ids) const |
A way to get a std::vector of available frame ids. | |
int | getLatestCommonTime (const std::string &source_frame, const std::string &target_frame, ros::Time &time, std::string *error_string) const |
Return the latest rostime which is common across the spanning set zero if fails to cross. | |
bool | getParent (const std::string &frame_id, ros::Time time, std::string &parent) const |
Fill the parent of a frame. | |
std::string | getTFPrefix () const |
Get the tf_prefix this is running with. | |
bool | isUsingDedicatedThread () |
void | lookupTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const |
Get the transform between two frames by frame ID. | |
void | lookupTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, StampedTransform &transform) const |
Get the transform between two frames by frame ID assuming fixed frame. | |
void | lookupTwist (const std::string &tracking_frame, const std::string &observation_frame, const std::string &reference_frame, const tf::Point &reference_point, const std::string &reference_point_frame, const ros::Time &time, const ros::Duration &averaging_interval, geometry_msgs::Twist &twist) const |
Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame using the reference point. | |
void | lookupTwist (const std::string &tracking_frame, const std::string &observation_frame, const ros::Time &time, const ros::Duration &averaging_interval, geometry_msgs::Twist &twist) const |
lookup the twist of the tracking frame with respect to the observational frame | |
void | removeTransformsChangedListener (boost::signals::connection c) |
void | setExtrapolationLimit (const ros::Duration &distance) |
Set the distance which tf is allow to extrapolate. | |
bool | setTransform (const StampedTransform &transform, const std::string &authority="default_authority") |
Add transform information to the tf data structure. | |
void | setUsingDedicatedThread (bool value) |
Transformer (bool interpolating=true, ros::Duration cache_time_=ros::Duration(DEFAULT_CACHE_TIME)) | |
void | transformPoint (const std::string &target_frame, const Stamped< tf::Point > &stamped_in, Stamped< tf::Point > &stamped_out) const |
Transform a Stamped Point into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument. | |
void | transformPoint (const std::string &target_frame, const ros::Time &target_time, const Stamped< tf::Point > &stamped_in, const std::string &fixed_frame, Stamped< tf::Point > &stamped_out) const |
Transform a Stamped Point into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument. | |
void | transformPose (const std::string &target_frame, const Stamped< tf::Pose > &stamped_in, Stamped< tf::Pose > &stamped_out) const |
Transform a Stamped Pose into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument. | |
void | transformPose (const std::string &target_frame, const ros::Time &target_time, const Stamped< tf::Pose > &stamped_in, const std::string &fixed_frame, Stamped< tf::Pose > &stamped_out) const |
Transform a Stamped Pose into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument. | |
void | transformQuaternion (const std::string &target_frame, const Stamped< tf::Quaternion > &stamped_in, Stamped< tf::Quaternion > &stamped_out) const |
Transform a Stamped Quaternion into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument. | |
void | transformQuaternion (const std::string &target_frame, const ros::Time &target_time, const Stamped< tf::Quaternion > &stamped_in, const std::string &fixed_frame, Stamped< tf::Quaternion > &stamped_out) const |
Transform a Stamped Quaternion into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument. | |
void | transformVector (const std::string &target_frame, const Stamped< tf::Vector3 > &stamped_in, Stamped< tf::Vector3 > &stamped_out) const |
Transform a Stamped Vector3 into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument. | |
void | transformVector (const std::string &target_frame, const ros::Time &target_time, const Stamped< tf::Vector3 > &stamped_in, const std::string &fixed_frame, Stamped< tf::Vector3 > &stamped_out) const |
Transform a Stamped Vector3 into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument. | |
bool | waitForTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const |
Block until a transform is possible or it times out. | |
bool | waitForTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const |
Block until a transform is possible or it times out. | |
virtual | ~Transformer (void) |
Public Attributes | |
bool | fall_back_to_wall_time_ |
Static Public Attributes | |
static const double | DEFAULT_CACHE_TIME = 10.0 |
10.0 is the default amount of time to cache data in seconds, set in cpp file. | |
static const int64_t | DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 0ULL |
The default amount of time to extrapolate. | |
static const unsigned int | MAX_GRAPH_DEPTH = 100UL |
The maximum number of time to recurse before assuming the tree has a loop. | |
Protected Types | |
typedef boost::unordered_map < std::string, CompactFrameID > | M_StringToCompactFrameID |
The internal storage class for ReferenceTransform. | |
typedef boost::signal< void(void)> | TransformsChangedSignal |
Protected Member Functions | |
TimeCache * | getFrame (unsigned int frame_number) const |
An accessor to get a frame, which will throw an exception if the frame is no there. | |
CompactFrameID | lookupFrameNumber (const std::string &frameid_str) const |
String to number for frame lookup with dynamic allocation of new frames. | |
std::string | lookupFrameString (unsigned int frame_id_num) const |
Number to string frame lookup may throw LookupException if number invalid. | |
CompactFrameID | lookupOrInsertFrameNumber (const std::string &frameid_str) |
String to number for frame lookup with dynamic allocation of new frames. | |
ros::Time | now () const |
virtual bool | ok () const |
Protected Attributes | |
ros::Duration | cache_time |
How long to cache transform history. | |
ros::Duration | cache_time_ |
How long to cache transform history. | |
std::map< CompactFrameID, std::string > | frame_authority_ |
A map to lookup the most recent authority for a given frame. | |
boost::recursive_mutex | frame_mutex_ |
A mutex to protect testing and allocating new frames on the above vector. | |
M_StringToCompactFrameID | frameIDs_ |
std::vector< std::string > | frameIDs_reverse |
A map from CompactFrameID frame_id_numbers to string for debugging and output. | |
std::vector< TimeCache * > | frames_ |
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. | |
bool | interpolating |
whether or not to interpolate or extrapolate | |
ros::Duration | max_extrapolation_distance_ |
whether or not to allow extrapolation | |
std::string | tf_prefix_ |
transform prefix to apply as necessary | |
TransformsChangedSignal | transforms_changed_ |
Signal which is fired whenever new transform data has arrived, from the thread the data arrived in. | |
boost::mutex | transforms_changed_mutex_ |
bool | using_dedicated_thread_ |
Private Member Functions | |
bool | canTransformInternal (CompactFrameID target_id, CompactFrameID source_id, const ros::Time &time, std::string *error_msg) const |
bool | canTransformNoLock (CompactFrameID target_id, CompactFrameID source_id, const ros::Time &time, std::string *error_msg) const |
void | createConnectivityErrorString (CompactFrameID source_frame, CompactFrameID target_frame, std::string *out) const |
int | getLatestCommonTime (CompactFrameID target_frame, CompactFrameID source_frame, ros::Time &time, std::string *error_string) const |
Return the latest rostime which is common across the spanning set zero if fails to cross. | |
template<typename F > | |
int | walkToTopParent (F &f, ros::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string *error_string) const |
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
typedef boost::unordered_map<std::string, CompactFrameID> tf::Transformer::M_StringToCompactFrameID [protected] |
The internal storage class for ReferenceTransform.
An instance of this class is created for each frame in the system. This class natively handles the relationship between frames.
The derived class Pose3DCache provides a buffered history of positions with interpolation.A map from string frame ids to CompactFrameID
typedef boost::signal<void(void)> tf::Transformer::TransformsChangedSignal [protected] |
Transformer::Transformer | ( | bool | interpolating = true , |
ros::Duration | cache_time_ = ros::Duration(DEFAULT_CACHE_TIME) |
||
) |
Transformer::~Transformer | ( | void | ) | [virtual] |
boost::signals::connection Transformer::addTransformsChangedListener | ( | boost::function< void(void)> | callback | ) |
std::string Transformer::allFramesAsDot | ( | ) | const |
std::string Transformer::allFramesAsString | ( | ) | const |
bool Transformer::canTransform | ( | const std::string & | target_frame, |
const std::string & | source_frame, | ||
const ros::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 |
bool Transformer::canTransform | ( | const std::string & | target_frame, |
const ros::Time & | target_time, | ||
const std::string & | source_frame, | ||
const ros::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 |
bool Transformer::canTransformInternal | ( | CompactFrameID | target_id, |
CompactFrameID | source_id, | ||
const ros::Time & | time, | ||
std::string * | error_msg | ||
) | const [private] |
bool Transformer::canTransformNoLock | ( | CompactFrameID | target_id, |
CompactFrameID | source_id, | ||
const ros::Time & | time, | ||
std::string * | error_msg | ||
) | const [private] |
void Transformer::chainAsVector | ( | const std::string & | target_frame, |
ros::Time | target_time, | ||
const std::string & | source_frame, | ||
ros::Time | source_time, | ||
const std::string & | fixed_frame, | ||
std::vector< std::string > & | output | ||
) | const |
Debugging function that will print the spanning chain of transforms. Possible exceptions tf::LookupException, tf::ConnectivityException, tf::MaxDepthException.
Debugging function that will print the spanning chain of transforms. Possible exceptions tf::LookupException, tf::ConnectivityException, tf::MaxDepthException
regular transforms
void Transformer::clear | ( | ) |
void Transformer::createConnectivityErrorString | ( | CompactFrameID | source_frame, |
CompactFrameID | target_frame, | ||
std::string * | out | ||
) | const [private] |
bool Transformer::frameExists | ( | const std::string & | frame_id_str | ) | const |
ros::Duration tf::Transformer::getCacheLength | ( | ) | [inline] |
tf::TimeCache * Transformer::getFrame | ( | unsigned int | frame_number | ) | const [protected] |
An accessor to get a frame, which will throw an exception if the frame is no there.
frame_number | The frameID of the desired Reference Frame |
This is an internal function which will get the pointer to the frame associated with the frame id Possible Exception: tf::LookupException
void Transformer::getFrameStrings | ( | std::vector< std::string > & | ids | ) | const |
int Transformer::getLatestCommonTime | ( | const std::string & | source_frame, |
const std::string & | target_frame, | ||
ros::Time & | time, | ||
std::string * | error_string | ||
) | const |
int Transformer::getLatestCommonTime | ( | CompactFrameID | target_frame, |
CompactFrameID | source_frame, | ||
ros::Time & | time, | ||
std::string * | error_string | ||
) | const [private] |
bool Transformer::getParent | ( | const std::string & | frame_id, |
ros::Time | time, | ||
std::string & | parent | ||
) | const |
std::string tf::Transformer::getTFPrefix | ( | ) | const [inline] |
bool tf::Transformer::isUsingDedicatedThread | ( | ) | [inline] |
CompactFrameID tf::Transformer::lookupFrameNumber | ( | const std::string & | frameid_str | ) | const [inline, protected] |
std::string tf::Transformer::lookupFrameString | ( | unsigned int | frame_id_num | ) | const [inline, protected] |
Number to string frame lookup may throw LookupException if number invalid.
CompactFrameID tf::Transformer::lookupOrInsertFrameNumber | ( | const std::string & | frameid_str | ) | [inline, protected] |
void Transformer::lookupTransform | ( | const std::string & | target_frame, |
const std::string & | source_frame, | ||
const ros::Time & | time, | ||
StampedTransform & | transform | ||
) | const |
Get the transform between two frames by frame ID.
target_frame | The frame to which data should be transformed |
source_frame | The frame where the data originated |
time | The time at which the value of the transform is desired. (0 will get the latest) |
transform | The transform reference to fill. |
Possible exceptions tf::LookupException, tf::ConnectivityException, tf::MaxDepthException, tf::ExtrapolationException
void Transformer::lookupTransform | ( | const std::string & | target_frame, |
const ros::Time & | target_time, | ||
const std::string & | source_frame, | ||
const ros::Time & | source_time, | ||
const std::string & | fixed_frame, | ||
StampedTransform & | transform | ||
) | const |
Get the transform between two frames by frame ID assuming fixed frame.
target_frame | The frame to which data should be transformed |
target_time | The time to which the data should be transformed. (0 will get the latest) |
source_frame | The frame where the data originated |
source_time | The time at which the source_frame should be evaluated. (0 will get the latest) |
fixed_frame | The frame in which to assume the transform is constant in time. |
transform | The transform reference to fill. |
Possible exceptions tf::LookupException, tf::ConnectivityException, tf::MaxDepthException, tf::ExtrapolationException
void Transformer::lookupTwist | ( | const std::string & | tracking_frame, |
const std::string & | observation_frame, | ||
const std::string & | reference_frame, | ||
const tf::Point & | reference_point, | ||
const std::string & | reference_point_frame, | ||
const ros::Time & | time, | ||
const ros::Duration & | averaging_interval, | ||
geometry_msgs::Twist & | twist | ||
) | const |
Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame using the reference point.
tracking_frame | The frame to track |
observation_frame | The frame from which to measure the twist |
reference_frame | The reference frame in which to express the twist |
reference_point | The reference point with which to express the twist |
reference_point_frame | The frame_id in which the reference point is expressed |
time | The time at which to get the velocity |
duration | The period over which to average |
twist | The twist output |
This will compute the average velocity on the interval (time - duration/2, time+duration/2). If that is too close to the most recent reading, in which case it will shift the interval up to duration/2 to prevent extrapolation. Possible exceptions tf::LookupException, tf::ConnectivityException, tf::MaxDepthException, tf::ExtrapolationException
New in geometry 1.1
check time on reference point too
void Transformer::lookupTwist | ( | const std::string & | tracking_frame, |
const std::string & | observation_frame, | ||
const ros::Time & | time, | ||
const ros::Duration & | averaging_interval, | ||
geometry_msgs::Twist & | twist | ||
) | const |
lookup the twist of the tracking frame with respect to the observational frame
This is a simplified version of lookupTwist with it assumed that the reference point is the origin of the tracking frame, and the reference frame is the observation frame.
New in geometry 1.1
ros::Time tf::Transformer::now | ( | ) | const [inline, protected] |
bool Transformer::ok | ( | ) | const [protected, virtual] |
Reimplemented in tf::TransformListener.
void Transformer::removeTransformsChangedListener | ( | boost::signals::connection | c | ) |
void Transformer::setExtrapolationLimit | ( | const ros::Duration & | distance | ) |
bool Transformer::setTransform | ( | const StampedTransform & | transform, |
const std::string & | authority = "default_authority" |
||
) |
void tf::Transformer::setUsingDedicatedThread | ( | bool | value | ) | [inline] |
void Transformer::transformPoint | ( | const std::string & | target_frame, |
const Stamped< tf::Point > & | stamped_in, | ||
Stamped< tf::Point > & | stamped_out | ||
) | const |
Transform a Stamped Point into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
void Transformer::transformPoint | ( | const std::string & | target_frame, |
const ros::Time & | target_time, | ||
const Stamped< tf::Point > & | stamped_in, | ||
const std::string & | fixed_frame, | ||
Stamped< tf::Point > & | stamped_out | ||
) | const |
Transform a Stamped Point into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
void Transformer::transformPose | ( | const std::string & | target_frame, |
const Stamped< tf::Pose > & | stamped_in, | ||
Stamped< tf::Pose > & | stamped_out | ||
) | const |
Transform a Stamped Pose into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
void Transformer::transformPose | ( | const std::string & | target_frame, |
const ros::Time & | target_time, | ||
const Stamped< tf::Pose > & | stamped_in, | ||
const std::string & | fixed_frame, | ||
Stamped< tf::Pose > & | stamped_out | ||
) | const |
Transform a Stamped Pose into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
void Transformer::transformQuaternion | ( | const std::string & | target_frame, |
const Stamped< tf::Quaternion > & | stamped_in, | ||
Stamped< tf::Quaternion > & | stamped_out | ||
) | const |
Transform a Stamped Quaternion into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
void Transformer::transformQuaternion | ( | const std::string & | target_frame, |
const ros::Time & | target_time, | ||
const Stamped< tf::Quaternion > & | stamped_in, | ||
const std::string & | fixed_frame, | ||
Stamped< tf::Quaternion > & | stamped_out | ||
) | const |
Transform a Stamped Quaternion into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
void Transformer::transformVector | ( | const std::string & | target_frame, |
const Stamped< tf::Vector3 > & | stamped_in, | ||
Stamped< tf::Vector3 > & | stamped_out | ||
) | const |
void tf::Transformer::transformVector | ( | const std::string & | target_frame, |
const ros::Time & | target_time, | ||
const Stamped< tf::Vector3 > & | stamped_in, | ||
const std::string & | fixed_frame, | ||
Stamped< tf::Vector3 > & | stamped_out | ||
) | const |
Transform a Stamped Vector3 into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument.
bool Transformer::waitForTransform | ( | const std::string & | target_frame, |
const std::string & | source_frame, | ||
const ros::Time & | time, | ||
const ros::Duration & | timeout, | ||
const ros::Duration & | polling_sleep_duration = ros::Duration(0.01) , |
||
std::string * | error_msg = NULL |
||
) | const |
Block until a transform is possible or it times out.
target_frame | The frame into which to transform |
source_frame | The frame from which to transform |
time | The time at which to transform |
timeout | How long to block before failing |
polling_sleep_duration | How often to retest if failed |
error_msg | A pointer to a string which will be filled with why the transform failed, if not NULL |
bool Transformer::waitForTransform | ( | const std::string & | target_frame, |
const ros::Time & | target_time, | ||
const std::string & | source_frame, | ||
const ros::Time & | source_time, | ||
const std::string & | fixed_frame, | ||
const ros::Duration & | timeout, | ||
const ros::Duration & | polling_sleep_duration = ros::Duration(0.01) , |
||
std::string * | error_msg = NULL |
||
) | const |
Block until a transform is possible or it times out.
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 |
timeout | How long to block before failing |
polling_sleep_duration | How often to retest if failed |
error_msg | A pointer to a string which will be filled with why the transform failed, if not NULL |
int Transformer::walkToTopParent | ( | F & | f, |
ros::Time | time, | ||
CompactFrameID | target_id, | ||
CompactFrameID | source_id, | ||
std::string * | error_string | ||
) | const [private] |
ros::Duration tf::Transformer::cache_time [protected] |
ros::Duration tf::Transformer::cache_time_ [protected] |
const double tf::Transformer::DEFAULT_CACHE_TIME = 10.0 [static] |
const int64_t tf::Transformer::DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 0ULL [static] |
std::map<CompactFrameID, std::string> tf::Transformer::frame_authority_ [protected] |
boost::recursive_mutex tf::Transformer::frame_mutex_ [mutable, protected] |
M_StringToCompactFrameID tf::Transformer::frameIDs_ [protected] |
std::vector<std::string> tf::Transformer::frameIDs_reverse [protected] |
std::vector<TimeCache*> tf::Transformer::frames_ [protected] |
bool tf::Transformer::interpolating [protected] |
const unsigned int tf::Transformer::MAX_GRAPH_DEPTH = 100UL [static] |
std::string tf::Transformer::tf_prefix_ [protected] |
boost::mutex tf::Transformer::transforms_changed_mutex_ [protected] |
bool tf::Transformer::using_dedicated_thread_ [protected] |