Fawkes API
Fawkes Development Version
|
Coordinate transforms between any two frames in a system. More...
#include <>>
Public Member Functions | |
Transformer (float cache_time_sec=BufferCore::DEFAULT_CACHE_TIME) | |
Constructor. More... | |
virtual | ~Transformer (void) |
Destructor. More... | |
void | set_enabled (bool enabled) |
Set enabled status of transformer. More... | |
bool | is_enabled () const |
Check if transformer is enabled. More... | |
float | get_cache_time () const |
Get cache time. More... | |
void | lock () |
Lock transformer. More... | |
bool | try_lock () |
Try to acquire lock. More... | |
void | unlock () |
Unlock. More... | |
bool | frame_exists (const std::string &frame_id_str) const |
Check if frame exists. More... | |
TimeCacheInterfacePtr | get_frame_cache (const std::string &frame_id) const |
Get cache for specific frame. More... | |
std::vector< TimeCacheInterfacePtr > | get_frame_caches () const |
Get all currently existing caches. More... | |
std::vector< std::string > | get_frame_id_mappings () const |
Get mappings from frame ID to names. 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... | |
void | lookup_transform (const std::string &target_frame, const std::string &source_frame, StampedTransform &transform) const |
Lookup transform at latest common time. 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... | |
void | transform_quaternion (const std::string &target_frame, const Stamped< Quaternion > &stamped_in, Stamped< Quaternion > &stamped_out) const |
Transform a stamped Quaternion into the target frame. More... | |
void | transform_vector (const std::string &target_frame, const Stamped< Vector3 > &stamped_in, Stamped< Vector3 > &stamped_out) const |
Transform a stamped vector into the target frame. More... | |
void | transform_point (const std::string &target_frame, const Stamped< Point > &stamped_in, Stamped< Point > &stamped_out) const |
Transform a stamped point into the target frame. More... | |
void | transform_pose (const std::string &target_frame, const Stamped< Pose > &stamped_in, Stamped< Pose > &stamped_out) const |
Transform a stamped pose into the target frame. More... | |
bool | transform_origin (const std::string &source_frame, const std::string &target_frame, Stamped< Pose > &stamped_out, const fawkes::Time time=fawkes::Time(0, 0)) const |
Transform ident pose from one frame to another. More... | |
void | transform_quaternion (const std::string &target_frame, const fawkes::Time &target_time, const Stamped< Quaternion > &stamped_in, const std::string &fixed_frame, Stamped< Quaternion > &stamped_out) const |
Transform a stamped Quaternion into the target frame assuming a fixed frame. More... | |
void | transform_vector (const std::string &target_frame, const fawkes::Time &target_time, const Stamped< Vector3 > &stamped_in, const std::string &fixed_frame, Stamped< Vector3 > &stamped_out) const |
Transform a stamped vector into the target frame assuming a fixed frame. More... | |
void | transform_point (const std::string &target_frame, const fawkes::Time &target_time, const Stamped< Point > &stamped_in, const std::string &fixed_frame, Stamped< Point > &stamped_out) const |
Transform a stamped point into the target frame assuming a fixed frame. More... | |
void | transform_pose (const std::string &target_frame, const fawkes::Time &target_time, const Stamped< Pose > &stamped_in, const std::string &fixed_frame, Stamped< Pose > &stamped_out) const |
Transform a stamped pose into the target frame assuming a fixed frame. More... | |
std::string | all_frames_as_dot (bool print_time, fawkes::Time *time=0) const |
Get DOT graph of all frames. More... | |
![]() | |
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... | |
Additional Inherited Members | |
![]() | |
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... | |
![]() | |
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... | |
![]() | |
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... | |
![]() | |
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... | |
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.
TF 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. TF is designed to take care of all the intermediate steps for you.
Internal Representation TF 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 fawkes::tf::LookupException
Definition at line 68 of file transformer.h.
fawkes::tf::Transformer::Transformer | ( | float | cache_time = BufferCore::DEFAULT_CACHE_TIME | ) |
Constructor.
cache_time | time in seconds to cache incoming transforms |
Definition at line 103 of file transformer.cpp.
|
virtual |
Destructor.
Definition at line 111 of file transformer.cpp.
std::string fawkes::tf::Transformer::all_frames_as_dot | ( | bool | print_time, |
fawkes::Time * | time = 0 |
||
) | const |
Get DOT graph of all frames.
print_time | true to add the time of the transform as graph label |
time | if not NULL will be assigned the time of the graph generation |
Definition at line 666 of file transformer.cpp.
References fawkes::tf::BufferCore::frame_authority_, fawkes::tf::TransformStorage::frame_id, fawkes::tf::BufferCore::frame_mutex_, fawkes::tf::BufferCore::frameIDs_reverse, fawkes::tf::BufferCore::frames_, fawkes::tf::BufferCore::get_frame(), fawkes::Time::in_sec(), lock(), and fawkes::Time::str().
bool fawkes::tf::Transformer::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 226 of file transformer.cpp.
References fawkes::tf::BufferCore::can_transform().
Referenced by ClipsTFThread::clips_context_destroyed().
bool fawkes::tf::Transformer::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 251 of file transformer.cpp.
References fawkes::tf::BufferCore::can_transform().
bool fawkes::tf::Transformer::frame_exists | ( | const std::string & | frame_id_str | ) | const |
Check if frame exists.
frame_id_str | frame ID |
Definition at line 179 of file transformer.cpp.
References fawkes::tf::BufferCore::frame_mutex_, fawkes::tf::BufferCore::frameIDs_, and lock().
Referenced by ClipsTFThread::clips_context_destroyed(), TfExampleThread::loop(), KatanaActThread::loop(), TabletopObjectsThread::loop(), and KatanaActThread::once().
float fawkes::tf::Transformer::get_cache_time | ( | ) | const |
Get cache time.
Definition at line 138 of file transformer.cpp.
References fawkes::tf::BufferCore::cache_time_.
Referenced by MongoLogTransformsThread::init(), ColliThread::init(), MapLaserGenThread::loop(), LaserClusterThread::loop(), ColliThread::loop(), and LaserLinesThread::loop().
TimeCacheInterfacePtr fawkes::tf::Transformer::get_frame_cache | ( | const std::string & | frame_id | ) | const |
Get cache for specific frame.
frame_id | ID of frame |
Definition at line 192 of file transformer.cpp.
References fawkes::tf::BufferCore::get_frame(), and fawkes::tf::BufferCore::lookup_frame_number().
Referenced by TfExampleThread::loop(), and ColliThread::loop().
std::vector< TimeCacheInterfacePtr > fawkes::tf::Transformer::get_frame_caches | ( | ) | const |
Get all currently existing caches.
Definition at line 201 of file transformer.cpp.
References fawkes::tf::BufferCore::frames_.
Referenced by MongoLogTransformsThread::loop(), PointCloudDBMergePipeline< pcl::PointXYZ >::merge(), and PointCloudDBRetrievePipeline< pcl::PointXYZ >::retrieve().
std::vector< std::string > fawkes::tf::Transformer::get_frame_id_mappings | ( | ) | const |
Get mappings from frame ID to names.
Definition at line 211 of file transformer.cpp.
References fawkes::tf::BufferCore::frameIDs_reverse.
Referenced by MongoLogTransformsThread::loop().
bool fawkes::tf::Transformer::is_enabled | ( | ) | const |
Check if transformer is enabled.
Definition at line 129 of file transformer.cpp.
void fawkes::tf::Transformer::lock | ( | ) |
Lock transformer.
No new transforms can be added and no lookups can be performed while the lock is held.
Definition at line 148 of file transformer.cpp.
References fawkes::tf::BufferCore::frame_mutex_.
Referenced by all_frames_as_dot(), frame_exists(), and MongoLogTransformsThread::loop().
void fawkes::tf::Transformer::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 282 of file transformer.cpp.
References fawkes::tf::BufferCore::lookup_transform().
Referenced by LaserProjectionDataFilter::filter(), LaserMapFilterDataFilter::filter(), lookup_transform(), TfExampleThread::loop(), TabletopObjectsThread::loop(), PointCloudDBMergePipeline< pcl::PointXYZ >::merge(), fawkes::LaserOccupancyGrid::reset_old(), PointCloudDBRetrievePipeline< pcl::PointXYZ >::retrieve(), transform_point(), transform_pose(), transform_quaternion(), transform_vector(), and fawkes::LaserOccupancyGrid::update_occ_grid().
void fawkes::tf::Transformer::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 316 of file transformer.cpp.
References fawkes::tf::BufferCore::lookup_transform().
void fawkes::tf::Transformer::lookup_transform | ( | const std::string & | target_frame, |
const std::string & | source_frame, | ||
StampedTransform & | transform | ||
) | const |
Lookup transform at latest common time.
target_frame | target frame ID |
source_frame | source frame ID |
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 345 of file transformer.cpp.
References lookup_transform().
void fawkes::tf::Transformer::set_enabled | ( | bool | enabled | ) |
Set enabled status of transformer.
enabled | true to enable, false to disable |
Definition at line 120 of file transformer.cpp.
Referenced by fawkes::tf::TransformListener::TransformListener().
bool fawkes::tf::Transformer::transform_origin | ( | const std::string & | source_frame, |
const std::string & | target_frame, | ||
Stamped< Pose > & | stamped_out, | ||
const fawkes::Time | time = fawkes::Time(0,0) |
||
) | const |
Transform ident pose from one frame to another.
This utility method can be used to transform the ident pose, i.e. the origin of one frame, into another. Note that this method does not throw an exception on error, rather the success of the transformation is indicated by a return value.
For example, if the source frame is the base frame (e.g. /base_link), and the target frame is the global frame (e.g. /map), then the result would be the robot's global position.
source_frame | frame whose origin to transform |
target_frame | frame in which you want to know the position of the origin of the source frame |
stamped_out | upon successful completion (check return value) stamped pose of origin of source frame in target frame |
time | time for when to do the transformation, by default take the latest matching transformation. |
Definition at line 492 of file transformer.cpp.
References transform_pose().
Referenced by NavGraphThread::loop().
void fawkes::tf::Transformer::transform_point | ( | const std::string & | target_frame, |
const Stamped< Point > & | stamped_in, | ||
Stamped< Point > & | stamped_out | ||
) | const |
Transform a stamped point into the target frame.
This transforms the point given relative to the frame set in the stamped point into the target frame.
target_frame | frame into which to transform |
stamped_in | stamped point, defines source frame and time |
stamped_out | stamped output point in target_frame |
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 430 of file transformer.cpp.
References fawkes::tf::Stamped< T >::frame_id, lookup_transform(), fawkes::tf::Stamped< T >::set_data(), fawkes::tf::StampedTransform::stamp, and fawkes::tf::Stamped< T >::stamp.
Referenced by NavGraphClustersThread::blocked_edges_centroids(), ClipsTFThread::clips_context_destroyed(), TrackedLineInfo::distance(), firevision::PositionToPixel::get_pixel_position_unchecked(), ColliThread::init(), TabletopVisualizationThread::loop(), ColliThread::loop(), KatanaActThread::loop(), TabletopObjectsThread::loop(), KatanaActThread::once(), NavGraphClustersThread::robot_pose(), and TrackedLineInfo::update().
void fawkes::tf::Transformer::transform_point | ( | const std::string & | target_frame, |
const fawkes::Time & | target_time, | ||
const Stamped< Point > & | stamped_in, | ||
const std::string & | fixed_frame, | ||
Stamped< Point > & | stamped_out | ||
) | const |
Transform a stamped point into the target frame assuming a fixed frame.
This transforms the point relative to the frame set in the stamped quaternion into the target frame. This will transform the point 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 into the target frame.
target_frame | frame into which to transform |
target_time | desired time in the target frame |
stamped_in | stamped point, defines source frame and time |
fixed_frame | ID of fixed frame |
stamped_out | stamped output point in target_frame |
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 607 of file transformer.cpp.
References fawkes::tf::Stamped< T >::frame_id, lookup_transform(), fawkes::tf::Stamped< T >::set_data(), fawkes::tf::StampedTransform::stamp, and fawkes::tf::Stamped< T >::stamp.
void fawkes::tf::Transformer::transform_pose | ( | const std::string & | target_frame, |
const Stamped< Pose > & | stamped_in, | ||
Stamped< Pose > & | stamped_out | ||
) | const |
Transform a stamped pose into the target frame.
This transforms the pose given relative to the frame set in the stamped vector into the target frame.
target_frame | frame into which to transform |
stamped_in | stamped pose, defines source frame and time |
stamped_out | stamped output pose in target_frame |
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 458 of file transformer.cpp.
References fawkes::tf::Stamped< T >::frame_id, lookup_transform(), fawkes::tf::Stamped< T >::set_data(), fawkes::tf::StampedTransform::stamp, and fawkes::tf::Stamped< T >::stamp.
Referenced by ClipsTFThread::clips_context_destroyed(), MapLaserGenThread::finalize(), RosMoveBaseThread::loop(), LaserClusterThread::loop(), NavGraphThread::loop(), TabletopObjectsThread::loop(), RosMoveBaseThread::once(), and transform_origin().
void fawkes::tf::Transformer::transform_pose | ( | const std::string & | target_frame, |
const fawkes::Time & | target_time, | ||
const Stamped< Pose > & | stamped_in, | ||
const std::string & | fixed_frame, | ||
Stamped< Pose > & | stamped_out | ||
) | const |
Transform a stamped pose into the target frame assuming a fixed frame.
This transforms the pose relative to the frame set in the stamped quaternion into the target frame. This will transform the pose 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 into the target frame.
target_frame | frame into which to transform |
target_time | desired time in the target frame |
stamped_in | stamped pose, defines source frame and time |
fixed_frame | ID of fixed frame |
stamped_out | stamped output pose in target_frame |
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 643 of file transformer.cpp.
References fawkes::tf::Stamped< T >::frame_id, lookup_transform(), fawkes::tf::Stamped< T >::set_data(), fawkes::tf::StampedTransform::stamp, and fawkes::tf::Stamped< T >::stamp.
void fawkes::tf::Transformer::transform_quaternion | ( | const std::string & | target_frame, |
const Stamped< Quaternion > & | stamped_in, | ||
Stamped< Quaternion > & | stamped_out | ||
) | const |
Transform a stamped Quaternion into the target frame.
This transforms the quaternion relative to the frame set in the stamped quaternion into the target frame.
target_frame | frame into which to transform |
stamped_in | stamped quaternion, defines source frame and time |
stamped_out | stamped output quaternion in target_frame |
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 |
InvalidArgument | thrown if the Quaternion is invalid, most likely an uninitialized Quaternion (0,0,0,0). |
Definition at line 369 of file transformer.cpp.
References fawkes::tf::Stamped< T >::frame_id, lookup_transform(), fawkes::tf::Stamped< T >::set_data(), fawkes::tf::StampedTransform::stamp, and fawkes::tf::Stamped< T >::stamp.
Referenced by ClipsTFThread::clips_context_destroyed().
void fawkes::tf::Transformer::transform_quaternion | ( | const std::string & | target_frame, |
const fawkes::Time & | target_time, | ||
const Stamped< Quaternion > & | stamped_in, | ||
const std::string & | fixed_frame, | ||
Stamped< Quaternion > & | stamped_out | ||
) | const |
Transform a stamped Quaternion into the target frame assuming a fixed frame.
This transforms the quaternion relative to the frame set in the stamped quaternion into the target frame. This will transform the quaternion 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 into the target frame.
target_frame | frame into which to transform |
target_time | desired time in the target frame |
stamped_in | stamped quaternion, defines source frame and time |
fixed_frame | ID of fixed frame |
stamped_out | stamped output quaternion in target_frame |
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 |
InvalidArgument | thrown if the Quaternion is invalid, most likely an uninitialized Quaternion (0,0,0,0). |
Definition at line 528 of file transformer.cpp.
References fawkes::tf::Stamped< T >::frame_id, lookup_transform(), fawkes::tf::Stamped< T >::set_data(), fawkes::tf::StampedTransform::stamp, and fawkes::tf::Stamped< T >::stamp.
void fawkes::tf::Transformer::transform_vector | ( | const std::string & | target_frame, |
const Stamped< Vector3 > & | stamped_in, | ||
Stamped< Vector3 > & | stamped_out | ||
) | const |
Transform a stamped vector into the target frame.
This transforms the vector given relative to the frame set in the stamped vector into the target frame.
target_frame | frame into which to transform |
stamped_in | stamped vector, defines source frame and time |
stamped_out | stamped output vector in target_frame |
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 398 of file transformer.cpp.
References fawkes::tf::Stamped< T >::frame_id, lookup_transform(), fawkes::tf::Stamped< T >::set_data(), fawkes::tf::StampedTransform::stamp, and fawkes::tf::Stamped< T >::stamp.
Referenced by ClipsTFThread::clips_context_destroyed(), and TabletopObjectsThread::loop().
void fawkes::tf::Transformer::transform_vector | ( | const std::string & | target_frame, |
const fawkes::Time & | target_time, | ||
const Stamped< Vector3 > & | stamped_in, | ||
const std::string & | fixed_frame, | ||
Stamped< Vector3 > & | stamped_out | ||
) | const |
Transform a stamped vector into the target frame assuming a fixed frame.
This transforms the vector relative to the frame set in the stamped quaternion into the target frame. This will transform the vector 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 into the target frame.
target_frame | frame into which to transform |
target_time | desired time in the target frame |
stamped_in | stamped vector, defines source frame and time |
fixed_frame | ID of fixed frame |
stamped_out | stamped output vector in target_frame |
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 565 of file transformer.cpp.
References fawkes::tf::Stamped< T >::frame_id, lookup_transform(), fawkes::tf::Stamped< T >::set_data(), fawkes::tf::StampedTransform::stamp, and fawkes::tf::Stamped< T >::stamp.
bool fawkes::tf::Transformer::try_lock | ( | ) |
Try to acquire lock.
Definition at line 158 of file transformer.cpp.
References fawkes::tf::BufferCore::frame_mutex_.
void fawkes::tf::Transformer::unlock | ( | ) |
Unlock.
Release currently held lock.
Definition at line 168 of file transformer.cpp.
References fawkes::tf::BufferCore::frame_mutex_.
Referenced by MongoLogTransformsThread::loop().