attitude_converter.h

Go to the documentation of this file.

// clang: TomasFormat
#ifndef ATTITUDE_CONVERTER_H
#define ATTITUDE_CONVERTER_H
 
#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include <tuple>
 
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf/transform_datatypes.h>
#include <tf_conversions/tf_eigen.h>
 
#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::Vector3& vector3);
 
  Vector3Converter(const double& x, const double& y, const double& z);
 
  operator tf2::Vector3() const;
 
  operator Eigen::Vector3d() const;
 
  operator geometry_msgs::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 tf::Quaternion quaternion);
 
  AttitudeConverter(const geometry_msgs::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 tf::Quaternion() const;
 
  operator geometry_msgs::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