Types.h

Go to the documentation of this file.

#ifndef CONTROLS_UTILS_TYPES_H
#define CONTROLS_UTILS_TYPES_H
 
#include <cmath>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
 
// ROS message types
#include <bb_controls_msgs/msg/thruster_forces.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/wrench_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <optional>
 
// Fixed-size matrix types
using Eigen::Matrix2d;
using Eigen::Matrix3d;
using Matrix4d = Eigen::Matrix<double, 4, 4>;
using Matrix6d = Eigen::Matrix<double, 6, 6>;
 
// Dynamic-size matrix type
using Eigen::MatrixXd;
 
// Fixed-size vector types
using Eigen::Vector2d;
using Eigen::Vector3d;
using Vector4d  = Eigen::Matrix<double, 4, 1>;
using Vector6d  = Eigen::Matrix<double, 6, 1>;
using Vector18d = Eigen::Matrix<double, 18, 1>;
 
// Dynamic-size vector type
using Eigen::VectorXd;
 
// Quaternion type
using Eigen::Quaterniond;
 
// Array types
using Eigen::ArrayXd;
using Array6d = Eigen::Array<double, 6, 1>;
 
constexpr double RAD2DEG = 180.0 / M_PI;
constexpr double DEG2RAD = M_PI / 180.0;
constexpr double M_2PI   = 2.0 * M_PI;
 
struct Pose {
    Vector3d position = Vector3d::Zero();
 
    Quaterniond orientation = Quaterniond::Identity();
};
 
struct State {
    Pose pose{};
 
    Vector6d twist = Vector6d::Zero();
 
    Vector6d acceleration = Vector6d::Zero();
};
 
struct DebugMessage {
    std::optional<double> t;
 
    std::optional<geometry_msgs::msg::PoseArray> traj_viz;
 
    std::optional<Vector6d> controller_force;
 
    std::optional<Vector6d> allocator_force;
 
    std::optional<bb_controls_msgs::msg::ThrusterForces> thruster_forces;
};
 
#endif  // CONTROLS_UTILS_TUPES_H