mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
mrs_lib::AttitudeConverter Class Reference

The main convertor class. Instantiate with any type in constructor and get the value in any other type by assigning the instance to your variable, as: tf::Quaternion tf1_quaternion = AttitudeConverter(roll, pitch, yaw); All the default Euler angles are in the extrinsic RPY notation. More...

#include <attitude_converter.h>

Classes

struct  EulerFormatException
 is thrown when the Euler angle format is set wrongly More...
 
struct  GetHeadingException
 is thrown when calculating of heading is not possible due to atan2 exception More...
 
struct  InvalidAttitudeException
 is thrown when the internal attitude becomes invalid More...
 
struct  MathErrorException
 is thrown when math breaks More...
 
struct  SetHeadingException
 is thrown when the heading cannot be set to an existing attitude More...
 

Public Member Functions

 AttitudeConverter (const double &roll, const double &pitch, const double &yaw, const RPY_convention_t &format=RPY_EXTRINSIC)
 Euler angles constructor. More...
 
 AttitudeConverter (const tf::Quaternion quaternion)
 tf::Quaternion constructor More...
 
 AttitudeConverter (const geometry_msgs::Quaternion quaternion)
 geometry_msgs::Quaternion constructor More...
 
 AttitudeConverter (const mrs_lib::EulerAttitude &euler_attitude)
 mrs_lib::EulerAttitude constructor More...
 
 AttitudeConverter (const Eigen::Quaterniond quaternion)
 Eigen::Quaterniond constructor. More...
 
 AttitudeConverter (const Eigen::Matrix3d matrix)
 Eigen::Matrix3d constructor. More...
 
template<class T >
 AttitudeConverter (const Eigen::AngleAxis< T > angle_axis)
 Eigen::AngleAxis constructor. More...
 
 AttitudeConverter (const tf2::Quaternion quaternion)
 tf2::Quaternion constructor More...
 
 AttitudeConverter (const tf2::Matrix3x3 matrix)
 tf2::Matrix3x3 constructor More...
 
 operator tf2::Quaternion () const
 typecast to tf2::Quaternion More...
 
 operator tf::Quaternion () const
 typecast to tf::Quaternion More...
 
 operator geometry_msgs::Quaternion () const
 typecast to geometry_msgs::Quaternion More...
 
 operator EulerAttitude () const
 typecast to EulerAttitude More...
 
template<class T >
 operator Eigen::AngleAxis< T > () const
 typecast to Eigen::AngleAxis More...
 
template<class T >
 operator Eigen::Quaternion< T > () const
 typecast to EulerAttitude Eigen::Quaternion More...
 
 operator Eigen::Matrix3d () const
 
 operator std::tuple< double &, double &, double & > ()
 typecase to tuple of Euler angles in extrinsic RPY More...
 
 operator tf2::Matrix3x3 () const
 typecase to tf2::Matrix3x3 More...
 
 operator tf2::Transform () const
 typecase to tf2::Transform More...
 
double getRoll (void)
 get the roll angle More...
 
double getPitch (void)
 get the pitch angle More...
 
double getYaw (void)
 get the yaw angle More...
 
double getHeading (void)
 get the angle of the rotated x-axis in the original XY plane, a.k.a More...
 
double getHeadingRate (const Vector3Converter &attitude_rate)
 get heading rate base on the orientation and body-based attitude rate More...
 
double getYawRateIntrinsic (const double &heading_rate)
 get the intrinsic yaw rate from a heading rate More...
 
Vector3Converter getVectorX (void)
 get a unit vector pointing in the X direction More...
 
Vector3Converter getVectorY (void)
 get a unit vector pointing in the Y direction More...
 
Vector3Converter getVectorZ (void)
 get a unit vector pointing in the Z direction More...
 
std::tuple< double, double, double > getIntrinsicRPY ()
 get the Roll, Pitch, Yaw angles in the Intrinsic convention More...
 
std::tuple< double, double, double > getExtrinsicRPY ()
 get the Roll, Pitch, Yaw angles in the Extrinsic convention. The same as the default AttitudeConverter assignment. More...
 
AttitudeConverter setHeading (const double &heading)
 Updates the heading of the current orientation by updating the intrinsic yaw. More...
 
AttitudeConverter setYaw (const double &new_yaw)
 Updates the extrinsic yaw of the current orientation. More...
 
template<std::size_t I>
constexpr auto get ()
 

Detailed Description

The main convertor class. Instantiate with any type in constructor and get the value in any other type by assigning the instance to your variable, as: tf::Quaternion tf1_quaternion = AttitudeConverter(roll, pitch, yaw); All the default Euler angles are in the extrinsic RPY notation.

Constructor & Destructor Documentation

◆ AttitudeConverter() [1/9]

mrs_lib::AttitudeConverter::AttitudeConverter ( const double &  roll,
const double &  pitch,
const double &  yaw,
const RPY_convention_t &  format = RPY_EXTRINSIC 
)

Euler angles constructor.

Parameters
roll
pitch
yaw
formatoptional, Euler angle convention, {"extrinsic", "intrinsic"}, defaults to "extrinsic"

◆ AttitudeConverter() [2/9]

mrs_lib::AttitudeConverter::AttitudeConverter ( const tf::Quaternion  quaternion)

tf::Quaternion constructor

Parameters
quaterniontf::Quaternion quaternion

◆ AttitudeConverter() [3/9]

mrs_lib::AttitudeConverter::AttitudeConverter ( const geometry_msgs::Quaternion  quaternion)

geometry_msgs::Quaternion constructor

Parameters
quaterniongeometry_msgs::Quaternion quaternion

◆ AttitudeConverter() [4/9]

mrs_lib::AttitudeConverter::AttitudeConverter ( const mrs_lib::EulerAttitude euler_attitude)

mrs_lib::EulerAttitude constructor

Parameters
euler_attitudemrs_lib::EulerAttitude

◆ AttitudeConverter() [5/9]

mrs_lib::AttitudeConverter::AttitudeConverter ( const Eigen::Quaterniond  quaternion)

Eigen::Quaterniond constructor.

Parameters
quaternionEigen::Quaterniond quaternion

◆ AttitudeConverter() [6/9]

mrs_lib::AttitudeConverter::AttitudeConverter ( const Eigen::Matrix3d  matrix)

Eigen::Matrix3d constructor.

Parameters
matrixEigen::Matrix3d rotational matrix

◆ AttitudeConverter() [7/9]

template<class T >
mrs_lib::AttitudeConverter::AttitudeConverter ( const Eigen::AngleAxis< T >  angle_axis)
inline

Eigen::AngleAxis constructor.

Template Parameters
Tangle-axis base type
Parameters
angle_axisEigen::AngleAxis

◆ AttitudeConverter() [8/9]

mrs_lib::AttitudeConverter::AttitudeConverter ( const tf2::Quaternion  quaternion)

tf2::Quaternion constructor

Parameters
quaterniontf2::Quaternion

◆ AttitudeConverter() [9/9]

mrs_lib::AttitudeConverter::AttitudeConverter ( const tf2::Matrix3x3  matrix)

tf2::Matrix3x3 constructor

Parameters
quaterniontf2::Matrix3x3

Member Function Documentation

◆ getExtrinsicRPY()

std::tuple< double, double, double > mrs_lib::AttitudeConverter::getExtrinsicRPY ( )

get the Roll, Pitch, Yaw angles in the Extrinsic convention. The same as the default AttitudeConverter assignment.

Returns
RPY

◆ getHeading()

double mrs_lib::AttitudeConverter::getHeading ( void  )

get the angle of the rotated x-axis in the original XY plane, a.k.a

Returns
heading

◆ getHeadingRate()

double mrs_lib::AttitudeConverter::getHeadingRate ( const Vector3Converter attitude_rate)

get heading rate base on the orientation and body-based attitude rate

Parameters
attitude_ratein the body frame
Returns
heading rate in the world

◆ getIntrinsicRPY()

std::tuple< double, double, double > mrs_lib::AttitudeConverter::getIntrinsicRPY ( )

get the Roll, Pitch, Yaw angles in the Intrinsic convention

Returns
RPY

◆ getPitch()

double mrs_lib::AttitudeConverter::getPitch ( void  )

get the pitch angle

Returns
pitch

◆ getRoll()

double mrs_lib::AttitudeConverter::getRoll ( void  )

get the roll angle

Returns
roll

◆ getVectorX()

Vector3Converter mrs_lib::AttitudeConverter::getVectorX ( void  )

get a unit vector pointing in the X direction

Returns
the vector

◆ getVectorY()

Vector3Converter mrs_lib::AttitudeConverter::getVectorY ( void  )

get a unit vector pointing in the Y direction

Returns
the vector

◆ getVectorZ()

Vector3Converter mrs_lib::AttitudeConverter::getVectorZ ( void  )

get a unit vector pointing in the Z direction

Returns
the vector

◆ getYaw()

double mrs_lib::AttitudeConverter::getYaw ( void  )

get the yaw angle

Returns
yaw

◆ getYawRateIntrinsic()

double mrs_lib::AttitudeConverter::getYawRateIntrinsic ( const double &  heading_rate)

get the intrinsic yaw rate from a heading rate

Parameters
heading_rate
Returns
intrinsic yaw rate

◆ operator Eigen::AngleAxis< T >()

template<class T >
mrs_lib::AttitudeConverter::operator Eigen::AngleAxis< T > ( ) const
inline

typecast to Eigen::AngleAxis

Template Parameters
Tangle-axis base type
Returns
orientation in EulerAttitude

◆ operator Eigen::Quaternion< T >()

template<class T >
mrs_lib::AttitudeConverter::operator Eigen::Quaternion< T > ( ) const
inline

typecast to EulerAttitude Eigen::Quaternion

Template Parameters
Tquaternion base type
Returns
orientation in Eigen::Quaternion

◆ operator EulerAttitude()

mrs_lib::AttitudeConverter::operator EulerAttitude ( ) const

typecast to EulerAttitude

Returns
orientation in EulerAttitude

◆ operator geometry_msgs::Quaternion()

mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion ( ) const

typecast to geometry_msgs::Quaternion

Returns
orientation in geometry_msgs::Quaternion

◆ operator std::tuple< double &, double &, double & >()

mrs_lib::AttitudeConverter::operator std::tuple< double &, double &, double & > ( )

typecase to tuple of Euler angles in extrinsic RPY

Returns
std::tuple of extrinsic RPY

◆ operator tf2::Matrix3x3()

mrs_lib::AttitudeConverter::operator tf2::Matrix3x3 ( ) const

typecase to tf2::Matrix3x3

Returns
tf2::Matrix3x3 rotational matrix

◆ operator tf2::Quaternion()

mrs_lib::AttitudeConverter::operator tf2::Quaternion ( ) const

typecast to tf2::Quaternion

Returns
orientation in tf2::Quaternion

◆ operator tf2::Transform()

mrs_lib::AttitudeConverter::operator tf2::Transform ( ) const

typecase to tf2::Transform

Returns
tf2::Transform

◆ operator tf::Quaternion()

mrs_lib::AttitudeConverter::operator tf::Quaternion ( ) const

typecast to tf::Quaternion

Returns
orientation in tf::Quaternion

◆ setHeading()

AttitudeConverter mrs_lib::AttitudeConverter::setHeading ( const double &  heading)

Updates the heading of the current orientation by updating the intrinsic yaw.

Parameters
newheading
Returns
the orientation

◆ setYaw()

AttitudeConverter mrs_lib::AttitudeConverter::setYaw ( const double &  new_yaw)

Updates the extrinsic yaw of the current orientation.

Parameters
newyaw
Returns
the orientation

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