attitude_converter.h

Go to the documentation of this file.

// clang: TomasFormat
#ifndef ATTITUDE_CONVERTER_H
#define ATTITUDE_CONVERTER_H
 
#include <cmath>
#include <Eigen/Dense>
#include <tuple>
 
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
#include <mrs_lib/geometry/misc.h>
 
namespace mrs_lib
{
 
  // type of the object we are grasping
  typedef enum
  {
 
    RPY_INTRINSIC = 1,
    RPY_EXTRINSIC = 2,
 
  } RPY_convention_t;
 
  /* class EulerAttitude //{ */
 
  class EulerAttitude
  {
  public:
    EulerAttitude(const double& roll, const double& pitch, const double& yaw);
 
    double roll(void) const;
 
    double pitch(void) const;
 
    double yaw(void) const;
 
  private:
    double roll_, pitch_, yaw_;
  };
 
  //}
 
  /* class Vector3Converter //{ */
 
  class Vector3Converter
  {
  public:
    Vector3Converter(const tf2::Vector3& vector3) : vector3_(vector3){};
 
    Vector3Converter(const Eigen::Vector3d& vector3);
 
    Vector3Converter(const geometry_msgs::msg::Vector3& vector3);
 
    Vector3Converter(const double& x, const double& y, const double& z);
 
    operator tf2::Vector3() const;
 
    operator Eigen::Vector3d() const;
 
    operator geometry_msgs::msg::Vector3() const;
 
  private:
    tf2::Vector3 vector3_;
  };
 
  //}
 
  class AttitudeConverter
  {
  public:
    /* exceptions //{ */
 
    struct GetHeadingException : public std::exception
    {
      const char* what() const throw()
      {
        return "AttitudeConverter: can not calculate the heading, the rotated x-axis is parallel to the world's z-axis";
      }
    };
 
    struct MathErrorException : public std::exception
    {
      const char* what() const throw()
      {
        return "AttitudeConverter: math error";
      }
    };
 
    struct InvalidAttitudeException : public std::exception
    {
      const char* what() const throw()
      {
        return "AttitudeConverter: invalid attitude, the input probably constains NaNs";
      }
    };
 
    struct EulerFormatException : public std::exception
    {
      const char* what() const throw()
      {
        return "AttitudeConverter: invalid Euler angle format";
      }
    };
 
    struct SetHeadingException : public std::exception
    {
      const char* what() const throw()
      {
        return "AttitudeConverter: cannot set the desired heading, the thrust vector's Z component is 0";
      }
    };
 
    //}
 
    /* constructors //{ */
 
    AttitudeConverter(const double& roll, const double& pitch, const double& yaw, const RPY_convention_t& format = RPY_EXTRINSIC);
 
    AttitudeConverter(const geometry_msgs::msg::Quaternion quaternion);
 
    AttitudeConverter(const mrs_lib::EulerAttitude& euler_attitude);
 
    AttitudeConverter(const Eigen::Quaterniond quaternion);
 
    AttitudeConverter(const Eigen::Matrix3d matrix);
 
    template <class T>
    AttitudeConverter(const Eigen::AngleAxis<T> angle_axis)
    {
      double angle = angle_axis.angle();
      tf2::Vector3 axis(angle_axis.axis()[0], angle_axis.axis()[1], angle_axis.axis()[2]);
 
      tf2_quaternion_.setRotation(axis, angle);
    }
 
    AttitudeConverter(const tf2::Quaternion quaternion);
 
    AttitudeConverter(const tf2::Matrix3x3 matrix);
 
    //}
 
    /* operators //{ */
 
    operator tf2::Quaternion() const;
 
    operator geometry_msgs::msg::Quaternion() const;
 
    operator EulerAttitude() const;
 
    template <class T>
    operator Eigen::AngleAxis<T>() const
    {
 
      double angle = tf2_quaternion_.getAngle();
      Eigen::Vector3d axis(tf2_quaternion_.getAxis()[0], tf2_quaternion_.getAxis()[1], tf2_quaternion_.getAxis()[2]);
 
      Eigen::AngleAxis<T> angle_axis(angle, axis);
 
      return angle_axis;
    }
 
 
    template <class T>
    operator Eigen::Quaternion<T>() const
    {
 
      return Eigen::Quaternion<T>(tf2_quaternion_.w(), tf2_quaternion_.x(), tf2_quaternion_.y(), tf2_quaternion_.z());
    }
 
    operator Eigen::Matrix3d() const;
 
    operator std::tuple<double&, double&, double&>();
 
    operator tf2::Matrix3x3() const;
 
    operator tf2::Transform() const;
 
    //}
 
    /* getters //{ */
 
    double getRoll(void);
 
    double getPitch(void);
 
    double getYaw(void);
 
    double getHeading(void);
 
    double getHeadingRate(const Vector3Converter& attitude_rate);
 
    double getYawRateIntrinsic(const double& heading_rate);
 
    Vector3Converter getVectorX(void);
 
    Vector3Converter getVectorY(void);
 
    Vector3Converter getVectorZ(void);
 
    std::tuple<double, double, double> getIntrinsicRPY();
 
    std::tuple<double, double, double> getExtrinsicRPY();
 
    //}
 
    /* setters //{ */
 
    AttitudeConverter setHeading(const double& heading);
 
    AttitudeConverter setYaw(const double& new_yaw);
 
    //}
 
    template <std::size_t I>
    constexpr auto get();
 
  private:
    tf2::Quaternion tf2_quaternion_;
 
    void calculateRPY(void);
 
    void validateOrientation(void);
 
    double roll_, pitch_, yaw_;
    bool got_rpy_ = false;
  };
 
 
  template <std::size_t I>
  constexpr auto AttitudeConverter::get()
  {
 
    calculateRPY();
 
    // call compilation error if I > 2
    static_assert(I <= 2);
 
    // get the RPY components based on the index in the tuple
    if constexpr (I == 0)
    {
      return static_cast<double>(roll_);
    } else if constexpr (I == 1)
    {
      return static_cast<double>(pitch_);
    } else if constexpr (I == 2)
    {
      return static_cast<double>(yaw_);
    }
  }
 
} // namespace mrs_lib
 
template <>
struct std::tuple_size<mrs_lib::AttitudeConverter>
{
  static constexpr int value = 3;
};
 
template <>
struct std::tuple_element<0, mrs_lib::AttitudeConverter>
{
  using type = double;
};
 
template <>
struct std::tuple_element<1, mrs_lib::AttitudeConverter>
{
  using type = double;
};
 
template <>
struct std::tuple_element<2, mrs_lib::AttitudeConverter>
{
  using type = double;
};
 
#endif