Program Listing for File frame_tf.hpp

↰ Return to documentation for file (include/mavros/frame_tf.hpp)

/*
 * Copyright 2016,2017,2021 Vladimir Ermakov.
 * Copyright 2017,2018 Nuno Marques.
 *
 * This file is part of the mavros package and subject to the license terms
 * in the top-level LICENSE file of the mavros repository.
 * https://github.com/mavlink/mavros/tree/master/LICENSE.md
 */
#pragma once

#ifndef MAVROS__FRAME_TF_HPP_
#define MAVROS__FRAME_TF_HPP_

#include <array>
#include <algorithm>
#include <Eigen/Eigen>              // NOLINT
#include <Eigen/Geometry>           // NOLINT
#include <rcpputils/asserts.hpp>    // NOLINT

// for Covariance types
#include "sensor_msgs/msg/imu.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/vector3.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/pose_with_covariance.hpp"

namespace mavros
{
namespace ftf
{
using Covariance3d = sensor_msgs::msg::Imu::_angular_velocity_covariance_type;

using Covariance6d = geometry_msgs::msg::PoseWithCovariance::_covariance_type;

using Covariance9d = std::array<double, 81>;

using EigenMapCovariance3d = Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>;
using EigenMapConstCovariance3d = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>;

using EigenMapCovariance6d = Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>>;
using EigenMapConstCovariance6d = Eigen::Map<const Eigen::Matrix<double, 6, 6, Eigen::RowMajor>>;

using EigenMapCovariance9d = Eigen::Map<Eigen::Matrix<double, 9, 9, Eigen::RowMajor>>;
using EigenMapConstCovariance9d = Eigen::Map<const Eigen::Matrix<double, 9, 9, Eigen::RowMajor>>;

enum class StaticTF
{
  NED_TO_ENU,
  ENU_TO_NED,
  AIRCRAFT_TO_BASELINK,
  BASELINK_TO_AIRCRAFT,
  //  baselink frame in an absolute frame of reference.
  ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK,
  //  aircraft frame in an absolute frame of reference
  ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT,
};

enum class StaticEcefTF
{
  ECEF_TO_ENU,
  ENU_TO_ECEF
};

namespace detail
{
Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond & q, const StaticTF transform);

Eigen::Vector3d transform_frame(const Eigen::Vector3d & vec, const Eigen::Quaterniond & q);

Covariance3d transform_frame(const Covariance3d & cov, const Eigen::Quaterniond & q);

Covariance6d transform_frame(const Covariance6d & cov, const Eigen::Quaterniond & q);

Covariance9d transform_frame(const Covariance9d & cov, const Eigen::Quaterniond & q);

Eigen::Vector3d transform_static_frame(const Eigen::Vector3d & vec, const StaticTF transform);

Covariance3d transform_static_frame(const Covariance3d & cov, const StaticTF transform);

Covariance6d transform_static_frame(const Covariance6d & cov, const StaticTF transform);

Covariance9d transform_static_frame(const Covariance9d & cov, const StaticTF transform);

Eigen::Vector3d transform_static_frame(
  const Eigen::Vector3d & vec,
  const Eigen::Vector3d & map_origin,
  const StaticEcefTF transform);

}       // namespace detail

// -*- frame tf -*-

template<class T>
inline T transform_orientation_ned_enu(const T & in)
{
  return detail::transform_orientation(in, StaticTF::NED_TO_ENU);
}

template<class T>
inline T transform_orientation_enu_ned(const T & in)
{
  return detail::transform_orientation(in, StaticTF::ENU_TO_NED);
}

template<class T>
inline T transform_orientation_aircraft_baselink(const T & in)
{
  return detail::transform_orientation(in, StaticTF::AIRCRAFT_TO_BASELINK);
}

template<class T>
inline T transform_orientation_baselink_aircraft(const T & in)
{
  return detail::transform_orientation(in, StaticTF::BASELINK_TO_AIRCRAFT);
}

template<class T>
inline T transform_orientation_absolute_frame_aircraft_baselink(const T & in)
{
  return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK);
}

template<class T>
inline T transform_orientation_absolute_frame_baselink_aircraft(const T & in)
{
  return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT);
}

template<class T>
inline T transform_frame_ned_enu(const T & in)
{
  return detail::transform_static_frame(in, StaticTF::NED_TO_ENU);
}

template<class T>
inline T transform_frame_enu_ned(const T & in)
{
  return detail::transform_static_frame(in, StaticTF::ENU_TO_NED);
}

template<class T>
inline T transform_frame_aircraft_baselink(const T & in)
{
  return detail::transform_static_frame(in, StaticTF::AIRCRAFT_TO_BASELINK);
}

template<class T>
inline T transform_frame_baselink_aircraft(const T & in)
{
  return detail::transform_static_frame(in, StaticTF::BASELINK_TO_AIRCRAFT);
}

template<class T>
inline T transform_frame_ecef_enu(const T & in, const T & map_origin)
{
  return detail::transform_static_frame(in, map_origin, StaticEcefTF::ECEF_TO_ENU);
}

template<class T>
inline T transform_frame_enu_ecef(const T & in, const T & map_origin)
{
  return detail::transform_static_frame(in, map_origin, StaticEcefTF::ENU_TO_ECEF);
}

template<class T>
inline T transform_frame_aircraft_ned(const T & in, const Eigen::Quaterniond & q)
{
  return detail::transform_frame(in, q);
}

template<class T>
inline T transform_frame_ned_aircraft(const T & in, const Eigen::Quaterniond & q)
{
  return detail::transform_frame(in, q);
}

template<class T>
inline T transform_frame_aircraft_enu(const T & in, const Eigen::Quaterniond & q)
{
  return detail::transform_frame(in, q);
}

template<class T>
inline T transform_frame_enu_aircraft(const T & in, const Eigen::Quaterniond & q)
{
  return detail::transform_frame(in, q);
}

template<class T>
inline T transform_frame_enu_baselink(const T & in, const Eigen::Quaterniond & q)
{
  return detail::transform_frame(in, q);
}

template<class T>
inline T transform_frame_baselink_enu(const T & in, const Eigen::Quaterniond & q)
{
  return detail::transform_frame(in, q);
}

// -*- utils -*-


Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d & rpy);

inline Eigen::Quaterniond quaternion_from_rpy(
  const double roll, const double pitch,
  const double yaw)
{
  return quaternion_from_rpy(Eigen::Vector3d(roll, pitch, yaw));
}

Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond & q);

inline void quaternion_to_rpy(
  const Eigen::Quaterniond & q, double & roll, double & pitch,
  double & yaw)
{
  const auto rpy = quaternion_to_rpy(q);
  roll = rpy.x();
  pitch = rpy.y();
  yaw = rpy.z();
}

double quaternion_get_yaw(const Eigen::Quaterniond & q);

template<typename _Scalar, std::enable_if_t<std::is_floating_point<_Scalar>::value, bool> = true>
inline void quaternion_to_mavlink(const Eigen::Quaternion<_Scalar> & q, std::array<float, 4> & qmsg)
{
  qmsg[0] = q.w();
  qmsg[1] = q.x();
  qmsg[2] = q.y();
  qmsg[3] = q.z();
}

inline Eigen::Quaterniond mavlink_to_quaternion(const std::array<float, 4> & q)
{
  return Eigen::Quaterniond(q[0], q[1], q[2], q[3]);
}

template<class T, std::size_t SIZE>
inline void covariance_to_mavlink(const T & cov, std::array<float, SIZE> & covmsg)
{
  std::copy(cov.cbegin(), cov.cend(), covmsg.begin());
}

template<class T, std::size_t ARR_SIZE>
inline void covariance_urt_to_mavlink(const T & covmap, std::array<float, ARR_SIZE> & covmsg)
{
  auto m = covmap;
  std::size_t COV_SIZE = m.rows() * (m.rows() + 1) / 2;
  rcpputils::assert_true(
    COV_SIZE == ARR_SIZE,
    "frame_tf: covariance matrix URT size is different from Mavlink msg covariance field size");

  auto out = covmsg.begin();

  for (Eigen::Index x = 0; x < m.cols(); x++) {
    for (Eigen::Index y = x; y < m.rows(); y++) {
      *out++ = m(y, x);
    }
  }
}

template<class T, std::size_t ARR_SIZE>
inline void mavlink_urt_to_covariance_matrix(const std::array<float, ARR_SIZE> & covmsg, T & covmat)
{
  std::size_t COV_SIZE = covmat.rows() * (covmat.rows() + 1) / 2;
  rcpputils::assert_true(
    COV_SIZE == ARR_SIZE,
    "frame_tf: covariance matrix URT size is different from Mavlink msg covariance field size");

  auto in = covmsg.begin();

  for (Eigen::Index x = 0; x < covmat.cols(); x++) {
    for (Eigen::Index y = x; y < covmat.rows(); y++) {
      covmat(x, y) = static_cast<double>(*in++);
      covmat(y, x) = covmat(x, y);
    }
  }
}

// [[[cog:
// def make_to_eigen(te, tr, fields):
//     fl = ", ".join(["r." + f for f in fields])
//     cog.outl(f"""//! @brief Helper to convert common ROS geometry_msgs::{tr} to Eigen::{te}""")
//     cog.outl(f"""inline Eigen::{te} to_eigen(const geometry_msgs::msg::{tr} r)""")
//     cog.outl(f"""{{""")
//     cog.outl(f"""  return Eigen::{te}({fl});""")
//     cog.outl(f"""}}""")
//
// make_to_eigen("Vector3d", "Point", "xyz")
// make_to_eigen("Vector3d", "Vector3", "xyz")
// make_to_eigen("Quaterniond", "Quaternion", "wxyz")
// ]]]
inline Eigen::Vector3d to_eigen(const geometry_msgs::msg::Point r)
{
  return Eigen::Vector3d(r.x, r.y, r.z);
}
inline Eigen::Vector3d to_eigen(const geometry_msgs::msg::Vector3 r)
{
  return Eigen::Vector3d(r.x, r.y, r.z);
}
inline Eigen::Quaterniond to_eigen(const geometry_msgs::msg::Quaternion r)
{
  return Eigen::Quaterniond(r.w, r.x, r.y, r.z);
}
// [[[end]]] (checksum: 2f12174368db2fc32ab814cb97b1bbec)

}       // namespace ftf
}       // namespace mavros

#endif  // MAVROS__FRAME_TF_HPP_