mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Loading...
Searching...
No Matches
attitude_converter.h
Go to the documentation of this file.
1// clang: TomasFormat
12#ifndef ATTITUDE_CONVERTER_H
13#define ATTITUDE_CONVERTER_H
14
15#include <cmath>
16#include <Eigen/Dense>
17#include <tuple>
18
19#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
20
22
23namespace mrs_lib
24{
25
26// type of the object we are grasping
27typedef enum
28{
29
30 RPY_INTRINSIC = 1,
31 RPY_EXTRINSIC = 2,
32
33} RPY_convention_t;
34
35/* class EulerAttitude //{ */
36
41public:
49 EulerAttitude(const double& roll, const double& pitch, const double& yaw);
50
56 double roll(void) const;
57
63 double pitch(void) const;
64
70 double yaw(void) const;
71
72private:
73 double roll_, pitch_, yaw_;
74};
75
76//}
77
78/* class Vector3Converter //{ */
79
85public:
91 Vector3Converter(const tf2::Vector3& vector3) : vector3_(vector3){};
92
98 Vector3Converter(const Eigen::Vector3d& vector3);
99
105 Vector3Converter(const geometry_msgs::msg::Vector3& vector3);
106
114 Vector3Converter(const double& x, const double& y, const double& z);
115
121 operator tf2::Vector3() const;
122
128 operator Eigen::Vector3d() const;
129
135 operator geometry_msgs::msg::Vector3() const;
136
137private:
138 tf2::Vector3 vector3_;
139};
140
141//}
142
148public:
149 /* exceptions //{ */
150
152 struct GetHeadingException : public std::exception
153 {
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";
156 }
157 };
158
160 struct MathErrorException : public std::exception
161 {
162 const char* what() const throw() {
163 return "AttitudeConverter: math error";
164 }
165 };
166
168 struct InvalidAttitudeException : public std::exception
169 {
170 const char* what() const throw() {
171 return "AttitudeConverter: invalid attitude, the input probably constains NaNs";
172 }
173 };
174
176 struct EulerFormatException : public std::exception
177 {
178 const char* what() const throw() {
179 return "AttitudeConverter: invalid Euler angle format";
180 }
181 };
182
184 struct SetHeadingException : public std::exception
185 {
186 const char* what() const throw() {
187 return "AttitudeConverter: cannot set the desired heading, the thrust vector's Z component is 0";
188 }
189 };
190
191 //}
192
193 /* constructors //{ */
194
203 AttitudeConverter(const double& roll, const double& pitch, const double& yaw, const RPY_convention_t& format = RPY_EXTRINSIC);
204
210 AttitudeConverter(const geometry_msgs::msg::Quaternion quaternion);
211
217 AttitudeConverter(const mrs_lib::EulerAttitude& euler_attitude);
218
224 AttitudeConverter(const Eigen::Quaterniond quaternion);
225
231 AttitudeConverter(const Eigen::Matrix3d matrix);
232
239 template <class T>
240 AttitudeConverter(const Eigen::AngleAxis<T> angle_axis) {
241 double angle = angle_axis.angle();
242 tf2::Vector3 axis(angle_axis.axis()[0], angle_axis.axis()[1], angle_axis.axis()[2]);
243
244 tf2_quaternion_.setRotation(axis, angle);
245 }
246
252 AttitudeConverter(const tf2::Quaternion quaternion);
253
259 AttitudeConverter(const tf2::Matrix3x3 matrix);
260
261 //}
262
263 /* operators //{ */
264
270 operator tf2::Quaternion() const;
271
277 operator geometry_msgs::msg::Quaternion() const;
278
284 operator EulerAttitude() const;
285
293 template <class T>
294 operator Eigen::AngleAxis<T>() const {
295
296 double angle = tf2_quaternion_.getAngle();
297 Eigen::Vector3d axis(tf2_quaternion_.getAxis()[0], tf2_quaternion_.getAxis()[1], tf2_quaternion_.getAxis()[2]);
298
299 Eigen::AngleAxis<T> angle_axis(angle, axis);
300
301 return angle_axis;
302 }
303
304
312 template <class T>
313 operator Eigen::Quaternion<T>() const {
314
315 return Eigen::Quaternion<T>(tf2_quaternion_.w(), tf2_quaternion_.x(), tf2_quaternion_.y(), tf2_quaternion_.z());
316 }
317
318 operator Eigen::Matrix3d() const;
319
325 operator std::tuple<double&, double&, double&>();
326
332 operator tf2::Matrix3x3() const;
333
339 operator tf2::Transform() const;
340
341 //}
342
343 /* getters //{ */
344
350 double getRoll(void);
351
357 double getPitch(void);
358
364 double getYaw(void);
365
371 double getHeading(void);
372
380 double getHeadingRate(const Vector3Converter& attitude_rate);
381
389 double getYawRateIntrinsic(const double& heading_rate);
390
397
404
411
417 std::tuple<double, double, double> getIntrinsicRPY();
418
424 std::tuple<double, double, double> getExtrinsicRPY();
425
426 //}
427
428 /* setters //{ */
429
437 AttitudeConverter setHeading(const double& heading);
438
446 AttitudeConverter setYaw(const double& new_yaw);
447
448 //}
449
450 template <std::size_t I>
451 constexpr auto get();
452
453private:
457 tf2::Quaternion tf2_quaternion_;
458
462 void calculateRPY(void);
463
467 void validateOrientation(void);
468
472 double roll_, pitch_, yaw_;
473 bool got_rpy_ = false;
474};
475
476
477template <std::size_t I>
478constexpr auto AttitudeConverter::get() {
479
480 calculateRPY();
481
482 // call compilation error if I > 2
483 static_assert(I <= 2);
484
485 // get the RPY components based on the index in the tuple
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_);
492 }
493}
494
495} // namespace mrs_lib
496
497template <>
498struct std::tuple_size<mrs_lib::AttitudeConverter>
499{
500 static constexpr int value = 3;
501};
502
503template <>
504struct std::tuple_element<0, mrs_lib::AttitudeConverter>
505{
506 using type = double;
507};
508
509template <>
510struct std::tuple_element<1, mrs_lib::AttitudeConverter>
511{
512 using type = double;
513};
514
515template <>
516struct std::tuple_element<2, mrs_lib::AttitudeConverter>
517{
518 using type = double;
519};
520
521#endif
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 the Euler angle format is set wrongly
Definition attitude_converter.h:177
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