Line data    Source code

       1             : #ifndef CONTROLS_UTILS_TYPES_H
       2             : #define CONTROLS_UTILS_TYPES_H
       3             : 
       4             : #include <cmath>
       5             : #include <eigen3/Eigen/Core>
       6             : #include <eigen3/Eigen/Geometry>
       7             : 
       8             : // ROS message types
       9             : #include <bb_controls_msgs/msg/thruster_forces.hpp>
      10             : #include <geometry_msgs/msg/pose_array.hpp>
      11             : #include <geometry_msgs/msg/twist_stamped.hpp>
      12             : #include <geometry_msgs/msg/wrench_stamped.hpp>
      13             : #include <nav_msgs/msg/odometry.hpp>
      14             : #include <optional>
      15             : 
      16             : // Fixed-size matrix types
      17             : using Eigen::Matrix2d;
      18             : using Eigen::Matrix3d;
      19           0 : using Matrix4d = Eigen::Matrix<double, 4, 4>;
      20           0 : using Matrix6d = Eigen::Matrix<double, 6, 6>;
      21             : 
      22             : // Dynamic-size matrix type
      23             : using Eigen::MatrixXd;
      24             : 
      25             : // Fixed-size vector types
      26             : using Eigen::Vector2d;
      27             : using Eigen::Vector3d;
      28           0 : using Vector4d  = Eigen::Matrix<double, 4, 1>;
      29           0 : using Vector6d  = Eigen::Matrix<double, 6, 1>;
      30           0 : using Vector18d = Eigen::Matrix<double, 18, 1>;
      31             : 
      32             : // Dynamic-size vector type
      33             : using Eigen::VectorXd;
      34             : 
      35             : // Quaternion type
      36             : using Eigen::Quaterniond;
      37             : 
      38             : // Array types
      39             : using Eigen::ArrayXd;
      40           0 : using Array6d = Eigen::Array<double, 6, 1>;
      41             : 
      42             : constexpr double RAD2DEG = 180.0 / M_PI;
      43             : constexpr double DEG2RAD = M_PI / 180.0;
      44             : constexpr double M_2PI   = 2.0 * M_PI;
      45             : 
      46             : /**
      47             :  * @brief 6DOF rigid-body pose as a 3D position and unit quaternion orientation
      48             :  */
      49           1 : struct Pose {
      50             :     /** @brief 3D position in the world frame (x, y, z) in metres */
      51             :     Vector3d position = Vector3d::Zero();
      52             : 
      53             :     /** @brief Orientation as a unit quaternion (w, x, y, z) */
      54             :     Quaterniond orientation = Quaterniond::Identity();
      55             : };
      56             : 
      57             : /**
      58             :  * @brief Full kinematic state of the vehicle
      59             :  *
      60             :  * Pose is expressed in the world frame. Twist and acceleration are expressed
      61             :  * in the body frame, following the nav_msgs/Odometry convention.
      62             :  */
      63           1 : struct State {
      64             :     /** @brief 6DOF pose (position + orientation) in the world frame */
      65             :     Pose pose{};
      66             : 
      67             :     /** @brief Body-frame velocity [u, v, w, p, q, r] in m/s and rad/s */
      68             :     Vector6d twist = Vector6d::Zero();
      69             : 
      70             :     /** @brief Body-frame acceleration [ax, ay, az, αx, αy, αz] in m/s² and rad/s² */
      71             :     Vector6d acceleration = Vector6d::Zero();
      72             : };
      73             : 
      74             : /**
      75             :  * @brief Aggregates optional debug data produced each control-loop tick
      76             :  *
      77             :  * Fields are optional so the debug publisher can skip absent data without
      78             :  * special-casing each field.
      79             :  */
      80           1 : struct DebugMessage {
      81             :     /** @brief ROS timestamp of this message in seconds */
      82             :     std::optional<double> t;
      83             : 
      84             :     /** @brief Sampled trajectory poses for RViz visualisation */
      85             :     std::optional<geometry_msgs::msg::PoseArray> traj_viz;
      86             : 
      87             :     /** @brief 6DOF body-frame force output by the controller (N / Nm) */
      88             :     std::optional<Vector6d> controller_force;
      89             : 
      90             :     /** @brief 6DOF body force actually achieved after thrust allocation */
      91             :     std::optional<Vector6d> allocator_force;
      92             : 
      93             :     /** @brief Per-thruster force magnitudes in Newtons */
      94             :     std::optional<bb_controls_msgs::msg::ThrusterForces> thruster_forces;
      95             : };
      96             : 
      97             : #endif  // CONTROLS_UTILS_TUPES_H