mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
|
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 () |
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.
mrs_lib::AttitudeConverter::AttitudeConverter | ( | const double & | roll, |
const double & | pitch, | ||
const double & | yaw, | ||
const RPY_convention_t & | format = RPY_EXTRINSIC |
||
) |
Euler angles constructor.
roll | |
pitch | |
yaw | |
format | optional, Euler angle convention, {"extrinsic", "intrinsic"}, defaults to "extrinsic" |
mrs_lib::AttitudeConverter::AttitudeConverter | ( | const tf::Quaternion | quaternion | ) |
tf::Quaternion constructor
quaternion | tf::Quaternion quaternion |
mrs_lib::AttitudeConverter::AttitudeConverter | ( | const geometry_msgs::Quaternion | quaternion | ) |
geometry_msgs::Quaternion constructor
quaternion | geometry_msgs::Quaternion quaternion |
mrs_lib::AttitudeConverter::AttitudeConverter | ( | const mrs_lib::EulerAttitude & | euler_attitude | ) |
mrs_lib::EulerAttitude constructor
euler_attitude | mrs_lib::EulerAttitude |
mrs_lib::AttitudeConverter::AttitudeConverter | ( | const Eigen::Quaterniond | quaternion | ) |
Eigen::Quaterniond constructor.
quaternion | Eigen::Quaterniond quaternion |
mrs_lib::AttitudeConverter::AttitudeConverter | ( | const Eigen::Matrix3d | matrix | ) |
Eigen::Matrix3d constructor.
matrix | Eigen::Matrix3d rotational matrix |
|
inline |
Eigen::AngleAxis constructor.
T | angle-axis base type |
angle_axis | Eigen::AngleAxis |
mrs_lib::AttitudeConverter::AttitudeConverter | ( | const tf2::Quaternion | quaternion | ) |
tf2::Quaternion constructor
quaternion | tf2::Quaternion |
mrs_lib::AttitudeConverter::AttitudeConverter | ( | const tf2::Matrix3x3 | matrix | ) |
tf2::Matrix3x3 constructor
quaternion | tf2::Matrix3x3 |
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.
double mrs_lib::AttitudeConverter::getHeading | ( | void | ) |
get the angle of the rotated x-axis in the original XY plane, a.k.a
double mrs_lib::AttitudeConverter::getHeadingRate | ( | const Vector3Converter & | attitude_rate | ) |
get heading rate base on the orientation and body-based attitude rate
attitude_rate | in the body frame |
std::tuple< double, double, double > mrs_lib::AttitudeConverter::getIntrinsicRPY | ( | ) |
get the Roll, Pitch, Yaw angles in the Intrinsic convention
double mrs_lib::AttitudeConverter::getPitch | ( | void | ) |
get the pitch angle
double mrs_lib::AttitudeConverter::getRoll | ( | void | ) |
get the roll angle
Vector3Converter mrs_lib::AttitudeConverter::getVectorX | ( | void | ) |
get a unit vector pointing in the X direction
Vector3Converter mrs_lib::AttitudeConverter::getVectorY | ( | void | ) |
get a unit vector pointing in the Y direction
Vector3Converter mrs_lib::AttitudeConverter::getVectorZ | ( | void | ) |
get a unit vector pointing in the Z direction
double mrs_lib::AttitudeConverter::getYaw | ( | void | ) |
get the yaw angle
double mrs_lib::AttitudeConverter::getYawRateIntrinsic | ( | const double & | heading_rate | ) |
get the intrinsic yaw rate from a heading rate
heading_rate |
|
inline |
typecast to Eigen::AngleAxis
T | angle-axis base type |
|
inline |
typecast to EulerAttitude Eigen::Quaternion
T | quaternion base type |
mrs_lib::AttitudeConverter::operator EulerAttitude | ( | ) | const |
typecast to EulerAttitude
mrs_lib::AttitudeConverter::operator geometry_msgs::Quaternion | ( | ) | const |
typecast to geometry_msgs::Quaternion
mrs_lib::AttitudeConverter::operator std::tuple< double &, double &, double & > | ( | ) |
typecase to tuple of Euler angles in extrinsic RPY
mrs_lib::AttitudeConverter::operator tf2::Matrix3x3 | ( | ) | const |
typecase to tf2::Matrix3x3
mrs_lib::AttitudeConverter::operator tf2::Quaternion | ( | ) | const |
typecast to tf2::Quaternion
mrs_lib::AttitudeConverter::operator tf2::Transform | ( | ) | const |
typecase to tf2::Transform
mrs_lib::AttitudeConverter::operator tf::Quaternion | ( | ) | const |
typecast to tf::Quaternion
AttitudeConverter mrs_lib::AttitudeConverter::setHeading | ( | const double & | heading | ) |
Updates the heading of the current orientation by updating the intrinsic yaw.
new | heading |
AttitudeConverter mrs_lib::AttitudeConverter::setYaw | ( | const double & | new_yaw | ) |
Updates the extrinsic yaw of the current orientation.
new | yaw |