12#ifndef ATTITUDE_CONVERTER_H
13#define ATTITUDE_CONVERTER_H
19#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
56 double roll(
void)
const;
63 double pitch(
void)
const;
70 double yaw(
void)
const;
73 double roll_, pitch_, yaw_;
121 operator tf2::Vector3()
const;
128 operator Eigen::Vector3d()
const;
135 operator geometry_msgs::msg::Vector3()
const;
138 tf2::Vector3 vector3_;
154 const char* what()
const throw() {
155 return "AttitudeConverter: can not calculate the heading, the rotated x-axis is parallel to the world's z-axis";
162 const char* what()
const throw() {
163 return "AttitudeConverter: math error";
170 const char* what()
const throw() {
171 return "AttitudeConverter: invalid attitude, the input probably constains NaNs";
178 const char* what()
const throw() {
179 return "AttitudeConverter: invalid Euler angle format";
186 const char* what()
const throw() {
187 return "AttitudeConverter: cannot set the desired heading, the thrust vector's Z component is 0";
203 AttitudeConverter(
const double& roll,
const double& pitch,
const double& yaw,
const RPY_convention_t& format = RPY_EXTRINSIC);
241 double angle = angle_axis.angle();
242 tf2::Vector3 axis(angle_axis.axis()[0], angle_axis.axis()[1], angle_axis.axis()[2]);
244 tf2_quaternion_.setRotation(axis, angle);
270 operator tf2::Quaternion()
const;
277 operator geometry_msgs::msg::Quaternion()
const;
294 operator Eigen::AngleAxis<T>()
const {
296 double angle = tf2_quaternion_.getAngle();
297 Eigen::Vector3d axis(tf2_quaternion_.getAxis()[0], tf2_quaternion_.getAxis()[1], tf2_quaternion_.getAxis()[2]);
299 Eigen::AngleAxis<T> angle_axis(angle, axis);
313 operator Eigen::Quaternion<T>()
const {
315 return Eigen::Quaternion<T>(tf2_quaternion_.w(), tf2_quaternion_.x(), tf2_quaternion_.y(), tf2_quaternion_.z());
318 operator Eigen::Matrix3d()
const;
325 operator std::tuple<double&, double&, double&>();
332 operator tf2::Matrix3x3()
const;
339 operator tf2::Transform()
const;
450 template <std::
size_t I>
451 constexpr auto get();
457 tf2::Quaternion tf2_quaternion_;
462 void calculateRPY(
void);
467 void validateOrientation(
void);
472 double roll_, pitch_, yaw_;
473 bool got_rpy_ =
false;
477template <std::
size_t I>
478constexpr auto AttitudeConverter::get() {
483 static_assert(I <= 2);
486 if constexpr (I == 0) {
487 return static_cast<double>(roll_);
488 }
else if constexpr (I == 1) {
489 return static_cast<double>(pitch_);
490 }
else if constexpr (I == 2) {
491 return static_cast<double>(yaw_);
498struct std::tuple_size<
mrs_lib::AttitudeConverter>
500 static constexpr int value = 3;
504struct std::tuple_element<0,
mrs_lib::AttitudeConverter>
510struct std::tuple_element<1,
mrs_lib::AttitudeConverter>
516struct std::tuple_element<2,
mrs_lib::AttitudeConverter>
The main convertor class. Instantiate with any type in constructor and get the value in any other typ...
Definition attitude_converter.h:147
AttitudeConverter(const Eigen::AngleAxis< T > angle_axis)
Eigen::AngleAxis constructor.
Definition attitude_converter.h:240
std::tuple< double, double, double > getExtrinsicRPY()
get the Roll, Pitch, Yaw angles in the Extrinsic convention. The same as the default AttitudeConverte...
Definition attitude_converter.cpp:352
Vector3Converter getVectorX(void)
get a unit vector pointing in the X direction
Definition attitude_converter.cpp:325
Vector3Converter getVectorZ(void)
get a unit vector pointing in the Z direction
Definition attitude_converter.cpp:343
double getPitch(void)
get the pitch angle
Definition attitude_converter.cpp:227
double getHeading(void)
get the angle of the rotated x-axis in the original XY plane, a.k.a
Definition attitude_converter.cpp:234
double getRoll(void)
get the roll angle
Definition attitude_converter.cpp:220
Vector3Converter getVectorY(void)
get a unit vector pointing in the Y direction
Definition attitude_converter.cpp:334
AttitudeConverter setYaw(const double &new_yaw)
Updates the extrinsic yaw of the current orientation.
Definition attitude_converter.cpp:374
double getYaw(void)
get the yaw angle
Definition attitude_converter.cpp:213
double getHeadingRate(const Vector3Converter &attitude_rate)
get heading rate base on the orientation and body-based attitude rate
Definition attitude_converter.cpp:292
std::tuple< double, double, double > getIntrinsicRPY()
get the Roll, Pitch, Yaw angles in the Intrinsic convention
Definition attitude_converter.cpp:361
AttitudeConverter setHeading(const double &heading)
Updates the heading of the current orientation by updating the intrinsic yaw.
Definition attitude_converter.cpp:383
double getYawRateIntrinsic(const double &heading_rate)
get the intrinsic yaw rate from a heading rate
Definition attitude_converter.cpp:247
A small class for storing the Euler angles.
Definition attitude_converter.h:40
double roll(void) const
get the roll angle
Definition attitude_converter.cpp:18
double yaw(void) const
get the yaw angle
Definition attitude_converter.cpp:26
double pitch(void) const
get the pitch angle
Definition attitude_converter.cpp:22
Converter of Vector3 representations. Instantiate it with any type of vector3 in constructor and conv...
Definition attitude_converter.h:84
Vector3Converter(const tf2::Vector3 &vector3)
Constructor with tf2::Vector3.
Definition attitude_converter.h:91
Defines useful geometry utilities and functions.
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition attitude_converter.h:24
is thrown when calculating of heading is not possible due to atan2 exception
Definition attitude_converter.h:153
is thrown when the internal attitude becomes invalid
Definition attitude_converter.h:169
is thrown when math breaks
Definition attitude_converter.h:161
is thrown when the heading cannot be set to an existing attitude
Definition attitude_converter.h:185