#pragma once
#include <array>
#include "robot_std_msgs/Header.h"
#include "geometry_msgs/Quaternion.h"
#include "geometry_msgs/Vector3.h"

namespace robot_sensor_msgs
{

struct Imu
{
    robot_std_msgs::Header header;  // Thời gian và frame gốc

    robot_geometry_msgs::Quaternion orientation;        // Góc quay (đơn vị: quaternion)
    std::array<double, 9> orientation_covariance; // Ma trận hiệp phương sai (row-major)

    robot_geometry_msgs::Vector3 angular_velocity;        // Tốc độ góc (rad/s)
    std::array<double, 9> angular_velocity_covariance; // Ma trận hiệp phương sai

    robot_geometry_msgs::Vector3 linear_acceleration;        // Gia tốc tuyến tính (m/s^2)
    std::array<double, 9> linear_acceleration_covariance; // Ma trận hiệp phương sai

    Imu()
    {
        orientation_covariance.fill(0.0);
        angular_velocity_covariance.fill(0.0);
        linear_acceleration_covariance.fill(0.0);
    }
};

} // namespace robot_sensor_msgs
