tf2 rolling
tf2 maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired point in time.
Loading...
Searching...
No Matches
Namespaces | Classes | Typedefs | Enumerations | Functions | Variables
tf2 Namespace Reference

Namespaces

namespace  impl
 

Classes

class  BackwardExtrapolationException
 An exception class to notify that the requested value would have required extrapolation in the past. More...
 
class  BufferCore
 A Class which provides coordinate transforms between any two frames in a system. More...
 
class  BufferCoreInterface
 Interface for providing coordinate transforms between any two frames in a system. More...
 
class  ConnectivityException
 An exception class to notify of no connection. More...
 
class  ExtrapolationException
 An exception class to notify that the requested value would have required extrapolation beyond current limits. More...
 
class  ForwardExtrapolationException
 An exception class to notify that the requested value would have required extrapolation in the future. More...
 
class  InvalidArgumentException
 An exception class to notify that one of the arguments is invalid. More...
 
class  LookupException
 An exception class to notify of bad frame number. More...
 
class  Matrix3x3
 The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Quaternion, Transform and Vector3. Make sure to only include a pure orthogonal matrix without scaling. More...
 
struct  Matrix3x3DoubleData
 for serialization More...
 
struct  Matrix3x3FloatData
 for serialization More...
 
class  NoDataForExtrapolationException
 An exception class to notify that the requested value would have required extrapolation, but only zero or one data is available, so not enough for extrapolation. More...
 
class  QuadWord
 The QuadWord class is base class for Vector3 and Quaternion. Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. More...
 
class  Quaternion
 The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x3, Vector3 and Transform. More...
 
class  Stamped
 The data type which will be cross compatable with geometry_msgs This is the tf2 datatype equivilant of a MessageStamped. More...
 
class  StaticCache
 
class  tf2Vector4
 
class  TimeCache
 A class to keep a sorted linked list in time (newest first, oldest last). This builds and maintains a list of timestamped data. And provides lookup functions to get data out as a function of time. More...
 
class  TimeCacheInterface
 
class  TimeoutException
 An exception class to notify that a timeout has occured. More...
 
class  Transform
 The Transform class supports rigid transforms with only translation and rotation and no scaling/shear. It can be used in combination with Vector3, Quaternion and Matrix3x3 linear algebra classes. More...
 
struct  TransformDoubleData
 
class  TransformException
 A base class for all tf2 exceptions This inherits from ros::exception which inherits from std::runtime_exception. More...
 
struct  TransformFloatData
 for serialization More...
 
class  TransformStorage
 Storage for transforms and their parent. More...
 
class  Vector3
 tf2::Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte alignment when tf2::Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user Ideally, this class should be replaced by a platform optimized TF2SIMD version that keeps the data in registers More...
 
struct  Vector3DoubleData
 
struct  Vector3FloatData
 
class  WithCovarianceStamped
 The data type which will be cross compatable with geometry_msgs This is the tf2 datatype equivalent of a MessageWithCovarianceStamped. More...
 

Typedefs

typedef std::pair< TimePoint, CompactFrameIDP_TimeAndFrameID
 
typedef uint64_t TransformableRequestHandle
 
using TimeCacheInterfacePtr = std::shared_ptr< TimeCacheInterface >
 
using Duration = std::chrono::nanoseconds
 
using TimePoint = std::chrono::time_point< std::chrono::system_clock, Duration >
 
using IDuration = std::chrono::duration< int, std::nano >
 
typedef uint32_t CompactFrameID
 

Enumerations

enum  TransformableResult { TransformAvailable , TransformFailure }
 
enum class  TF2Error : std::uint8_t {
  TF2_NO_ERROR = 0 , TF2_LOOKUP_ERROR = 1 , TF2_CONNECTIVITY_ERROR = 2 , TF2_EXTRAPOLATION_ERROR = 3 ,
  TF2_INVALID_ARGUMENT_ERROR = 4 , TF2_TIMEOUT_ERROR = 5 , TF2_TRANSFORM_ERROR = 6 , TF2_BACKWARD_EXTRAPOLATION_ERROR = 7 ,
  TF2_FORWARD_EXTRAPOLATION_ERROR = 8 , TF2_NO_DATA_FOR_EXTRAPOLATION_ERROR = 9
}
 

Functions

template<class T >
void doTransform (const T &data_in, T &data_out, const geometry_msgs::msg::TransformStamped &transform)
 The templated function expected to be able to do a transform.
 
template<class T >
tf2::TimePoint getTimestamp (const T &t)
 Get the timestamp from data.
 
template<class T >
std::string getFrameId (const T &t)
 Get the frame_id from data.
 
template<class T >
std::array< std::array< double, 6 >, 6 > getCovarianceMatrix (const T &t)
 Get the covariance matrix from data.
 
template<class P >
tf2::TimePoint getTimestamp (const tf2::Stamped< P > &t)
 Get the frame_id from data.
 
template<class P >
std::string getFrameId (const tf2::Stamped< P > &t)
 Get the frame_id from data.
 
template<class P >
std::array< std::array< double, 6 >, 6 > getCovarianceMatrix (const tf2::WithCovarianceStamped< P > &t)
 Get the covariance matrix from data.
 
template<typename A , typename B >
toMsg (const A &a)
 Function that converts from one type to a ROS message type. It has to be implemented by each data type in tf2_* (except ROS messages) as it is used in the "convert" function.
 
template<typename A , typename B >
void fromMsg (const A &a, B &b)
 Function that converts from a ROS message type to another type. It has to be implemented by each data type in tf2_* (except ROS messages) as it is used in the "convert" function.
 
template<class A , class B >
void convert (const A &a, B &b)
 Function that converts any type to any type (messages or not). Matching toMsg and from Msg conversion functions need to exist. If they don't exist or do not apply (for example, if your two classes are ROS messages), just write a specialization of the function.
 
template<class A >
void convert (const A &a1, A &a2)
 
std::array< std::array< double, 6 >, 6 > covarianceRowMajorToNested (const std::array< double, 36 > &row_major)
 Function that converts from a row-major representation of a 6x6 covariance matrix to a nested array representation.
 
std::array< double, 36 > covarianceNestedToRowMajor (const std::array< std::array< double, 6 >, 6 > &nested_array)
 Function that converts from a nested array representation of a 6x6 covariance matrix to a row-major representation.
 
void fromMsg (const geometry_msgs::msg::Quaternion &in, tf2::Quaternion &out)
 
Vector3 operator* (const Matrix3x3 &m, const Vector3 &v)
 
Vector3 operator* (const Vector3 &v, const Matrix3x3 &m)
 
Matrix3x3 operator* (const Matrix3x3 &m1, const Matrix3x3 &m2)
 
bool operator== (const Matrix3x3 &m1, const Matrix3x3 &m2)
 Equality operator between two matrices It will test all elements are equal.

 
Quaternion operator- (const Quaternion &q)
 Return the negative of a quaternion.
 
Quaternion operator* (const Quaternion &q1, const Quaternion &q2)
 Return the product of two quaternions.
 
Quaternion operator* (const Quaternion &q, const Vector3 &w)
 
Quaternion operator* (const Vector3 &w, const Quaternion &q)
 
tf2Scalar dot (const Quaternion &q1, const Quaternion &q2)
 Calculate the dot product between two quaternions.
 
tf2Scalar length (const Quaternion &q)
 Return the length of a quaternion.
 
tf2Scalar angle (const Quaternion &q1, const Quaternion &q2)
 Return the half angle between two quaternions.
 
tf2Scalar angleShortestPath (const Quaternion &q1, const Quaternion &q2)
 Return the shortest angle between two quaternions.
 
Quaternion inverse (const Quaternion &q)
 Return the inverse of a quaternion.
 
Quaternion slerp (const Quaternion &q1, const Quaternion &q2, const tf2Scalar &t)
 Return the result of spherical linear interpolation betwen two quaternions.
 
Vector3 quatRotate (const Quaternion &rotation, const Vector3 &v)
 
Quaternion shortestArcQuat (const Vector3 &v0, const Vector3 &v1)
 
Quaternion shortestArcQuatNormalize2 (Vector3 &v0, Vector3 &v1)
 
bool operator== (const Transform &t1, const Transform &t2)
 Test if two transforms have all elements equal.
 
Vector3 operator+ (const Vector3 &v1, const Vector3 &v2)
 Return the sum of two vectors (Point symantics)
 
Vector3 operator* (const Vector3 &v1, const Vector3 &v2)
 Return the elementwise product of two vectors.
 
Vector3 operator- (const Vector3 &v1, const Vector3 &v2)
 Return the difference between two vectors.
 
Vector3 operator- (const Vector3 &v)
 Return the negative of the vector.
 
Vector3 operator* (const Vector3 &v, const tf2Scalar &s)
 Return the vector scaled by s.
 
Vector3 operator* (const tf2Scalar &s, const Vector3 &v)
 Return the vector scaled by s.
 
Vector3 operator/ (const Vector3 &v, const tf2Scalar &s)
 Return the vector inversely scaled by s.
 
Vector3 operator/ (const Vector3 &v1, const Vector3 &v2)
 Return the vector inversely scaled by s.
 
tf2Scalar tf2Dot (const Vector3 &v1, const Vector3 &v2)
 Return the dot product between two vectors.
 
tf2Scalar tf2Distance2 (const Vector3 &v1, const Vector3 &v2)
 Return the distance squared between two vectors.
 
tf2Scalar tf2Distance (const Vector3 &v1, const Vector3 &v2)
 Return the distance between two vectors.
 
tf2Scalar tf2Angle (const Vector3 &v1, const Vector3 &v2)
 Return the angle between two vectors.
 
Vector3 tf2Cross (const Vector3 &v1, const Vector3 &v2)
 Return the cross product of two vectors.
 
tf2Scalar tf2Triple (const Vector3 &v1, const Vector3 &v2, const Vector3 &v3)
 
Vector3 lerp (const Vector3 &v1, const Vector3 &v2, const tf2Scalar &t)
 Return the linear interpolation between two vectors.
 
void tf2SwapScalarEndian (const tf2Scalar &sourceVal, tf2Scalar &destVal)
 tf2SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
 
void tf2SwapVector3Endian (const Vector3 &sourceVec, Vector3 &destVec)
 tf2SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
 
void tf2UnSwapVector3Endian (Vector3 &vector)
 tf2UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
 
void tf2PlaneSpace1 (const Vector3 &n, Vector3 &p, Vector3 &q)
 
TimePoint get_now ()
 
Duration durationFromSec (double t_sec)
 
TimePoint timeFromSec (double t_sec)
 
double durationToSec (const tf2::Duration &input)
 
double timeToSec (const TimePoint &timepoint)
 
std::string displayTimePoint (const TimePoint &stamp)
 
template<typename T >
bool operator== (const Stamped< T > &a, const Stamped< T > &b)
 Comparison Operator for Stamped datatypes.
 
template<typename T >
bool operator== (const WithCovarianceStamped< T > &a, const WithCovarianceStamped< T > &b)
 Comparison operator for WithCovarianceStamped datatypes.
 
template<class A >
void getEulerYPR (const A &a, double &yaw, double &pitch, double &roll)
 
template<class A >
double getYaw (const A &a)
 
template<class A >
A getTransformIdentity ()
 

Variables

constexpr tf2::Duration TIMECACHE_DEFAULT_MAX_STORAGE_TIME = std::chrono::seconds(10)
 default value of 10 seconds storage
 

Typedef Documentation

◆ P_TimeAndFrameID

◆ TransformableRequestHandle

◆ TimeCacheInterfacePtr

◆ Duration

using tf2::Duration = typedef std::chrono::nanoseconds

◆ TimePoint

using tf2::TimePoint = typedef std::chrono::time_point<std::chrono::system_clock, Duration>

◆ IDuration

using tf2::IDuration = typedef std::chrono::duration<int, std::nano>

◆ CompactFrameID

Enumeration Type Documentation

◆ TransformableResult

Enumerator
TransformAvailable 
TransformFailure 

◆ TF2Error

enum class tf2::TF2Error : std::uint8_t
strong
Enumerator
TF2_NO_ERROR 
TF2_LOOKUP_ERROR 
TF2_CONNECTIVITY_ERROR 
TF2_EXTRAPOLATION_ERROR 
TF2_INVALID_ARGUMENT_ERROR 
TF2_TIMEOUT_ERROR 
TF2_TRANSFORM_ERROR 
TF2_BACKWARD_EXTRAPOLATION_ERROR 
TF2_FORWARD_EXTRAPOLATION_ERROR 
TF2_NO_DATA_FOR_EXTRAPOLATION_ERROR 

Function Documentation

◆ doTransform()

template<class T >
void tf2::doTransform ( const T &  data_in,
T &  data_out,
const geometry_msgs::msg::TransformStamped &  transform 
)

The templated function expected to be able to do a transform.

This is the method which tf2 will use to try to apply a transform for any given datatype.

Parameters
[in]data_inThe data to be transformed.
[in,out]data_outA reference to the output data. Note this can point to data in and the method should be mutation safe.
[in]transformThe transform to apply to data_in to fill data_out.

This method needs to be implemented by client library developers

◆ getTimestamp() [1/2]

template<class T >
tf2::TimePoint tf2::getTimestamp ( const T &  t)

Get the timestamp from data.

Parameters
[in]tThe data input.
Returns
The timestamp associated with the data.

◆ getFrameId() [1/2]

template<class T >
std::string tf2::getFrameId ( const T &  t)

Get the frame_id from data.

Parameters
[in]tThe data input.
Returns
The frame_id associated with the data.

◆ getCovarianceMatrix() [1/2]

template<class T >
std::array< std::array< double, 6 >, 6 > tf2::getCovarianceMatrix ( const T &  t)

Get the covariance matrix from data.

Parameters
[in]tThe data input.
Returns
The covariance matrix associated with the data.

◆ getTimestamp() [2/2]

template<class P >
tf2::TimePoint tf2::getTimestamp ( const tf2::Stamped< P > &  t)

Get the frame_id from data.

An implementation for Stamped

datatypes.

Parameters
[in]tThe data input.
Returns
The frame_id associated with the data.

◆ getFrameId() [2/2]

template<class P >
std::string tf2::getFrameId ( const tf2::Stamped< P > &  t)

Get the frame_id from data.

An implementation for Stamped

datatypes.

Parameters
[in]tThe data input.
Returns
The frame_id associated with the data.

◆ getCovarianceMatrix() [2/2]

template<class P >
std::array< std::array< double, 6 >, 6 > tf2::getCovarianceMatrix ( const tf2::WithCovarianceStamped< P > &  t)

Get the covariance matrix from data.

An implementation for WithCovarianceStamped

datatypes.

Parameters
[in]tThe data input.
Returns
The covariance matrix associated with the data.

◆ toMsg()

template<typename A , typename B >
B tf2::toMsg ( const A &  a)

Function that converts from one type to a ROS message type. It has to be implemented by each data type in tf2_* (except ROS messages) as it is used in the "convert" function.

Parameters
aan object of whatever type
Returns
the conversion as a ROS message

◆ fromMsg() [1/2]

template<typename A , typename B >
void tf2::fromMsg ( const A a,
B b 
)

Function that converts from a ROS message type to another type. It has to be implemented by each data type in tf2_* (except ROS messages) as it is used in the "convert" function.

Parameters
aa ROS message to convert from
bthe object to convert to

◆ convert() [1/2]

template<class A , class B >
void tf2::convert ( const A a,
B b 
)

Function that converts any type to any type (messages or not). Matching toMsg and from Msg conversion functions need to exist. If they don't exist or do not apply (for example, if your two classes are ROS messages), just write a specialization of the function.

Parameters
aan object to convert from
bthe object to convert to

◆ convert() [2/2]

template<class A >
void tf2::convert ( const A a1,
A a2 
)

◆ covarianceRowMajorToNested()

std::array< std::array< double, 6 >, 6 > tf2::covarianceRowMajorToNested ( const std::array< double, 36 > &  row_major)
inline

Function that converts from a row-major representation of a 6x6 covariance matrix to a nested array representation.

Parameters
row_majorA row-major array of 36 covariance values.
Returns
A nested array representation of 6x6 covariance values.

◆ covarianceNestedToRowMajor()

std::array< double, 36 > tf2::covarianceNestedToRowMajor ( const std::array< std::array< double, 6 >, 6 > &  nested_array)
inline

Function that converts from a nested array representation of a 6x6 covariance matrix to a row-major representation.

Parameters
nested_arrayA nested array representation of 6x6 covariance values.
Returns
A row-major array of 36 covariance values.

◆ fromMsg() [2/2]

void tf2::fromMsg ( const geometry_msgs::msg::Quaternion &  in,
tf2::Quaternion out 
)

◆ operator*() [1/9]

◆ operator*() [2/9]

◆ operator*() [3/9]

◆ operator==() [1/4]

bool tf2::operator== ( const Matrix3x3 m1,
const Matrix3x3 m2 
)

Equality operator between two matrices It will test all elements are equal.

◆ operator-() [1/3]

Return the negative of a quaternion.

◆ operator*() [4/9]

Return the product of two quaternions.

◆ operator*() [5/9]

◆ operator*() [6/9]

◆ dot()

tf2Scalar tf2::dot ( const Quaternion q1,
const Quaternion q2 
)

Calculate the dot product between two quaternions.

◆ length()

tf2Scalar tf2::length ( const Quaternion q)

Return the length of a quaternion.

◆ angle()

tf2Scalar tf2::angle ( const Quaternion q1,
const Quaternion q2 
)

Return the half angle between two quaternions.

◆ angleShortestPath()

tf2Scalar tf2::angleShortestPath ( const Quaternion q1,
const Quaternion q2 
)

Return the shortest angle between two quaternions.

◆ inverse()

Quaternion tf2::inverse ( const Quaternion q)

Return the inverse of a quaternion.

◆ slerp()

Quaternion tf2::slerp ( const Quaternion q1,
const Quaternion q2,
const tf2Scalar t 
)

Return the result of spherical linear interpolation betwen two quaternions.

Parameters
q1The first quaternion
q2The second quaternion
tThe ration between q1 and q2. t = 0 return q1, t=1 returns q2 Slerp assumes constant velocity between positions.

◆ quatRotate()

Vector3 tf2::quatRotate ( const Quaternion rotation,
const Vector3 v 
)

◆ shortestArcQuat()

Quaternion tf2::shortestArcQuat ( const Vector3 v0,
const Vector3 v1 
)

◆ shortestArcQuatNormalize2()

Quaternion tf2::shortestArcQuatNormalize2 ( Vector3 v0,
Vector3 v1 
)

◆ operator==() [2/4]

bool tf2::operator== ( const Transform t1,
const Transform t2 
)

Test if two transforms have all elements equal.

◆ operator+()

Return the sum of two vectors (Point symantics)

◆ operator*() [7/9]

Return the elementwise product of two vectors.

◆ operator-() [2/3]

Return the difference between two vectors.

◆ operator-() [3/3]

Return the negative of the vector.

◆ operator*() [8/9]

Return the vector scaled by s.

◆ operator*() [9/9]

Return the vector scaled by s.

◆ operator/() [1/2]

Return the vector inversely scaled by s.

◆ operator/() [2/2]

Return the vector inversely scaled by s.

◆ tf2Dot()

tf2Scalar tf2::tf2Dot ( const Vector3 v1,
const Vector3 v2 
)

Return the dot product between two vectors.

◆ tf2Distance2()

tf2Scalar tf2::tf2Distance2 ( const Vector3 v1,
const Vector3 v2 
)

Return the distance squared between two vectors.

◆ tf2Distance()

tf2Scalar tf2::tf2Distance ( const Vector3 v1,
const Vector3 v2 
)

Return the distance between two vectors.

◆ tf2Angle()

tf2Scalar tf2::tf2Angle ( const Vector3 v1,
const Vector3 v2 
)

Return the angle between two vectors.

◆ tf2Cross()

Vector3 tf2::tf2Cross ( const Vector3 v1,
const Vector3 v2 
)

Return the cross product of two vectors.

◆ tf2Triple()

tf2Scalar tf2::tf2Triple ( const Vector3 v1,
const Vector3 v2,
const Vector3 v3 
)

◆ lerp()

Vector3 tf2::lerp ( const Vector3 v1,
const Vector3 v2,
const tf2Scalar t 
)

Return the linear interpolation between two vectors.

Parameters
v1One vector
v2The other vector
tThe ration of this to v (t = 0 => return v1, t=1 => return v2)

◆ tf2SwapScalarEndian()

void tf2::tf2SwapScalarEndian ( const tf2Scalar sourceVal,
tf2Scalar destVal 
)

tf2SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization

◆ tf2SwapVector3Endian()

void tf2::tf2SwapVector3Endian ( const Vector3 sourceVec,
Vector3 destVec 
)

tf2SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization

◆ tf2UnSwapVector3Endian()

void tf2::tf2UnSwapVector3Endian ( Vector3 vector)

tf2UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization

◆ tf2PlaneSpace1()

void tf2::tf2PlaneSpace1 ( const Vector3 n,
Vector3 p,
Vector3 q 
)

◆ get_now()

TimePoint tf2::get_now ( )

◆ durationFromSec()

Duration tf2::durationFromSec ( double  t_sec)

◆ timeFromSec()

TimePoint tf2::timeFromSec ( double  t_sec)

◆ durationToSec()

double tf2::durationToSec ( const tf2::Duration input)

◆ timeToSec()

double tf2::timeToSec ( const TimePoint timepoint)

◆ displayTimePoint()

std::string tf2::displayTimePoint ( const TimePoint stamp)

◆ operator==() [3/4]

template<typename T >
bool tf2::operator== ( const Stamped< T > &  a,
const Stamped< T > &  b 
)

Comparison Operator for Stamped datatypes.

◆ operator==() [4/4]

template<typename T >
bool tf2::operator== ( const WithCovarianceStamped< T > &  a,
const WithCovarianceStamped< T > &  b 
)

Comparison operator for WithCovarianceStamped datatypes.

◆ getEulerYPR()

template<class A >
void tf2::getEulerYPR ( const A a,
double yaw,
double pitch,
double roll 
)

Return the yaw, pitch, roll of anything that can be converted to a tf2::Quaternion The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h

Parameters
athe object to get data from (it represents a rotation/quaternion)
yawyaw
pitchpitch
rollroll

◆ getYaw()

template<class A >
double tf2::getYaw ( const A a)

Return the yaw of anything that can be converted to a tf2::Quaternion The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h This function is a specialization of getEulerYPR and is useful for its wide-spread use in navigation

Parameters
athe object to get data from (it represents a rotation/quaternion)
Returns
yaw

◆ getTransformIdentity()

template<class A >
A tf2::getTransformIdentity ( )

Return the identity for any type that can be converted to a tf2::Transform

Returns
an object of class A that is an identity transform

Variable Documentation

◆ TIMECACHE_DEFAULT_MAX_STORAGE_TIME

constexpr tf2::Duration tf2::TIMECACHE_DEFAULT_MAX_STORAGE_TIME = std::chrono::seconds(10)
constexpr

default value of 10 seconds storage