VSAG/amadeus_26
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

无迹卡尔曼

Browse Source
This commit is contained in:
Huang Haoyu
2026-03-26 05:01:47 +08:00
parent 57b7ce41e1
commit 00157d6410
3 changed files with 359 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -14,6 +14,7 @@ find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(Eigen3 REQUIRED)
# 添加 SDK 库路径
set(TRANSMITTER_SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib/transmitter_sdk)
@@ -41,6 +42,7 @@ ament_target_dependencies(imu_receiver_node
std_msgs
geometry_msgs
sensor_msgs
Eigen3
)
# ==================== 安装目标 ====================

35
docs/IMU/packet/MSG_IMU.md Normal file
View File

@@ -0,0 +1,35 @@
# MSG_IMU | FDISYSTEMS支持中心
1. 机体坐标系及坐标系方向如图所示。在FDI系列产品中，模块表面都会标注准确的机体坐标系方向。具体以实际产品为准。
2. 对于角速度，加速度的标定解释：
- (1) 转台标定：测量并补偿了陀螺仪和加速度计的零偏、尺度误差及三轴不垂直度。
- (2) 温箱标定：在宽温范围（如-40℃~85℃）下，测量了传感器误差随温度的变化规律，并生成了补偿模型。对陀螺和加表的输出数据进行了补偿。
3. 对于加速度中未分离重力加速度解释：
加速度计测量的是载体运动与地球重力的"混合"结果。简单来说，当模块静止时，加速度读数并不为零，而会显示出一个重力值（例如，平放时Z轴约为+9.8 m/s²）。
## 基本信息
| 属性 | 值 |
|------|-----|
| **Packet ID** | 0x40 |
| **Length** | 56 |
| **Read / Write** | Read |
## 数据字段
| Offset | Size | Format | Field | Unit | Description |
|--------|------|--------|-------|------|-------------|
| 0 | 4 | float32_t | Gyroscope_X | rad/s | 机体系X轴角速度 |
| 4 | 4 | float32_t | Gyroscope_Y | rad/s | 机体系Y轴角速度 |
| 8 | 4 | float32_t | Gyroscope_Z | rad/s | 机体系Z轴角速度 |
| 12 | 4 | float32_t | Accelerometer_X | m/s² | 机体系X轴加速度(未分离重力加速度) |
| 16 | 4 | float32_t | Accelerometer_Y | m/s² | 机体系Y轴加速度(未分离重力加速度) |
| 20 | 4 | float32_t | Accelerometer_Z | m/s² | 机体系Z轴加速度(未分离重力加速度) |
| 24 | 4 | float32_t | Magnetometer_X | mG | 机体系X轴磁感应强度 |
| 28 | 4 | float32_t | Magnetometer_Y | mG | 机体系Y轴磁感应强度 |
| 32 | 4 | float32_t | Magnetometer_Z | mG | 机体系Z轴磁感应强度 |
| 36 | 4 | float32_t | IMU_Temperature | deg C | 如果IMU数据由多个传感器组成则该值为这些传感器的平均温度 |
| 40 | 4 | float32_t | Pressure | Pa | 气压值，如果没装气压计，默认为一个标准大气压101325Pa |
| 48 | 8 | int64_t | Timestamp | us | 数据的时间戳，从上电开始启动的微秒数。时钟源为MCU外部晶振。 |

418
src/imu_receiver_node.cpp
View File

@@ -1,21 +1,24 @@
/**
* @file imu_receiver_node.cpp
* @brief IMU 数据接收节点 (FDI DETA10) - 环形缓冲区版本
* @brief IMU 数据接收节点 (FDI DETA10) - 环形缓冲区版本 + UKF
*
* 通过串口读取 FDI DETA10 IMU 模块的数据
* 波特率：921600
* 协议：FDILink
*
* 转发数据包：
* - MSG_AHRS (0x41): 航姿参考系统数据
* - MSG_BODY_VEL (0x60): 机体系速度数据 (IMU卡尔曼滤波后)
* - MSG_BODY_ACCELERATION (0x62): 滤波修正后的机体系加速度（不包括重力）
*
* 特性：使用环形缓冲区避免数据堆积爆发
* 特性：
* - 使用环形缓冲区避免数据堆积爆发
* - 使用 UKF（无迹卡尔曼滤波器）对加速度数据进行滤波
*/
#include <atomic>
#include <cmath>
#include <condition_variable>
#include <cstring>
#include <eigen3/Eigen/Dense>
#include <errno.h>
#include <fcntl.h>
#include <geometry_msgs/msg/quaternion.hpp>
@@ -38,9 +41,8 @@
constexpr uint8_t FRAME_START = 0xFC;
constexpr uint8_t FRAME_END = 0xFD;
// 数据包 ID
constexpr uint8_t MSG_AHRS = 0x41; // 航姿参考系统数据
constexpr uint8_t MSG_BODY_VEL = 0x60; // 机体系速度数据 (卡尔曼滤波后)
// 数据包 ID - 只使用 MSG_BODY_ACCELERATION
constexpr uint8_t MSG_BODY_ACCELERATION = 0x62; // 滤波修正后的机体系加速度
// 默认串口设备
constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyIMU";
@@ -56,6 +58,11 @@ constexpr size_t MAX_PUBLISH_RATE_HZ = 100; // 最大发布频率 100Hz
constexpr auto PUBLISH_PERIOD =
std::chrono::microseconds(1000000 / MAX_PUBLISH_RATE_HZ); // 10ms
// UKF 参数
constexpr int UKF_STATE_DIM = 2; // 状态维度 [位置, 速度]
constexpr int UKF_MEAS_DIM = 1; // 测量维度 [位置]
constexpr int UKF_SIGMA_POINTS = 2 * UKF_STATE_DIM + 1; // 2n+1 个 Sigma 点
// CRC8 查找表
static const uint8_t CRC8Table[] = {
0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31,
@@ -109,16 +116,221 @@ static const uint16_t CRC16Table[256] = {
0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74,
0x2E93, 0x3EB2, 0x0ED1, 0x1EF0};
// IMU 数据结构
struct ImuDataFrame {
bool ahrs_valid = false;
float roll_speed = 0, pitch_speed = 0, heading_speed = 0;
float roll = 0, pitch = 0, heading = 0;
float q1 = 0, q2 = 0, q3 = 0, q4 = 0;
int64_t timestamp_us = 0;
/**
* @brief 无迹卡尔曼滤波器 (UKF) 用于加速度滤波
*
* 状态向量: [位置, 速度]^T
* 测量: 位置
*
* 使用 Sigma 点变换处理非线性
*/
class UKF {
public:
UKF()
: x_(Eigen::Vector2d::Zero()), P_(Eigen::Matrix2d::Identity()),
Q_(Eigen::Matrix2d::Identity() * 0.001),
R_(Eigen::Matrix<double, 1, 1>::Identity() * 0.03), dt_(0.01),
initialized_(false) {
// 初始化权重
computeWeights();
}
bool vel_valid = false;
float vel_x = 0, vel_y = 0, vel_z = 0;
/**
* @brief 设置过程噪声
*/
void setProcessNoise(double q) {
Q_ = Eigen::Matrix2d::Identity() * q;
Q_(1, 1) = q * 10; // 速度噪声更大一些
}
/**
* @brief 设置测量噪声
*/
void setMeasurementNoise(double r) { R_(0, 0) = r; }
/**
* @brief 更新滤波器 - 简单的一维位置滤波
* @param measurement 测量值 (加速度/位置)
* @return 滤波后的值
*/
double update(double measurement) {
if (!initialized_) {
x_(0) = measurement;
x_(1) = 0; // 初始速度为0
initialized_ = true;
return measurement;
}
// === 预测步骤 ===
// 生成 Sigma 点
Eigen::Matrix<double, UKF_STATE_DIM, UKF_SIGMA_POINTS> sigma_points;
generateSigmaPoints(sigma_points);
// 传播 Sigma 点通过状态转移方程
Eigen::Matrix<double, UKF_STATE_DIM, UKF_SIGMA_POINTS> sigma_points_pred;
for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
// 状态转移: 恒定速度模型
// x_k+1 = x_k + v_k * dt
// v_k+1 = v_k
sigma_points_pred(0, i) = sigma_points(0, i) + sigma_points(1, i) * dt_;
sigma_points_pred(1, i) = sigma_points(1, i);
}
// 计算预测均值
x_pred_.setZero();
for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
x_pred_ += weights_m_[i] * sigma_points_pred.col(i);
}
// 计算预测协方差
P_pred_.setZero();
for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
Eigen::Vector2d diff = sigma_points_pred.col(i) - x_pred_;
P_pred_ += weights_c_[i] * diff * diff.transpose();
}
P_pred_ += Q_;
// === 更新步骤 ===
// 传播 Sigma 点到测量空间
Eigen::Matrix<double, UKF_MEAS_DIM, UKF_SIGMA_POINTS> Z_sigma;
for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
// 测量模型: 只测量位置
Z_sigma(0, i) = sigma_points_pred(0, i);
}
// 计算预测测量均值
z_pred_.setZero();
for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
z_pred_ += weights_m_[i] * Z_sigma.col(i);
}
// 计算预测测量协方差
Eigen::Matrix<double, UKF_MEAS_DIM, UKF_MEAS_DIM> S;
S.setZero();
for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
Eigen::Matrix<double, UKF_MEAS_DIM, 1> diff = Z_sigma.col(i) - z_pred_;
S += weights_c_[i] * diff * diff.transpose();
}
S += R_;
// 计算互协方差
Eigen::Matrix<double, UKF_STATE_DIM, UKF_MEAS_DIM> T;
T.setZero();
for (int i = 0; i < UKF_SIGMA_POINTS; ++i) {
Eigen::Vector2d diff_x = sigma_points_pred.col(i) - x_pred_;
Eigen::Matrix<double, UKF_MEAS_DIM, 1> diff_z = Z_sigma.col(i) - z_pred_;
T += weights_c_[i] * diff_x * diff_z.transpose();
}
// 计算卡尔曼增益
Eigen::Matrix<double, UKF_STATE_DIM, UKF_MEAS_DIM> K = T * S.inverse();
// 更新状态
Eigen::Matrix<double, UKF_MEAS_DIM, 1> z;
z(0) = measurement;
x_ = x_pred_ + K * (z - z_pred_);
// 更新协方差
P_ = P_pred_ - K * S * K.transpose();
return x_(0);
}
/**
* @brief 设置采样时间
*/
void setDt(double dt) { dt_ = dt; }
/**
* @brief 获取当前估计值
*/
double getEstimate() const { return x_(0); }
/**
* @brief 获取当前速度估计
*/
double getVelocity() const { return x_(1); }
private:
/**
* @brief 计算 Sigma 点权重
*/
void computeWeights() {
// 使用标准 UKF 参数
// alpha = 1e-3, beta = 2, kappa = 0
double alpha = 1e-3;
double beta = 2.0;
double kappa = 0.0;
lambda_ = alpha * alpha * (UKF_STATE_DIM + kappa) - UKF_STATE_DIM;
weights_m_.resize(UKF_SIGMA_POINTS);
weights_c_.resize(UKF_SIGMA_POINTS);
weights_m_[0] = lambda_ / (UKF_STATE_DIM + lambda_);
weights_c_[0] = weights_m_[0] + (1 - alpha * alpha + beta);
for (int i = 1; i < UKF_SIGMA_POINTS; ++i) {
weights_m_[i] = 0.5 / (UKF_STATE_DIM + lambda_);
weights_c_[i] = weights_m_[i];
}
}
/**
* @brief 生成 Sigma 点
*/
void generateSigmaPoints(
Eigen::Matrix<double, UKF_STATE_DIM, UKF_SIGMA_POINTS> &sigma_points) {
// 计算矩阵平方根
Eigen::Matrix2d sqrt_P = ((UKF_STATE_DIM + lambda_) * P_).llt().matrixL();
sigma_points.col(0) = x_;
for (int i = 0; i < UKF_STATE_DIM; ++i) {
sigma_points.col(i + 1) = x_ + sqrt_P.col(i);
sigma_points.col(i + 1 + UKF_STATE_DIM) = x_ - sqrt_P.col(i);
}
}
// 状态向量 [位置, 速度]
Eigen::Vector2d x_;
Eigen::Vector2d x_pred_;
// 协方差矩阵
Eigen::Matrix2d P_;
Eigen::Matrix2d P_pred_;
// 过程噪声和测量噪声
Eigen::Matrix2d Q_;
Eigen::Matrix<double, UKF_MEAS_DIM, UKF_MEAS_DIM> R_;
// 预测测量
Eigen::Matrix<double, UKF_MEAS_DIM, 1> z_pred_;
// UKF 参数
double lambda_;
std::vector<double> weights_m_;
std::vector<double> weights_c_;
// 采样时间
double dt_;
// 初始化标志
bool initialized_;
};
// IMU 数据结构 - 使用 MSG_BODY_ACCELERATION 数据包
struct ImuDataFrame {
bool valid = false;
// 滤波修正后的机体系加速度 (m/s²) - 不包括重力
float acc_x = 0, acc_y = 0, acc_z = 0;
// 当地重力加速度 (m/s²)
float g_force = 0;
// 时间戳 (从接收到数据的时间)
std::chrono::steady_clock::time_point receive_time;
};
// 线程安全的环形缓冲区
@@ -191,23 +403,48 @@ public:
this->declare_parameter("verbose", true); // 是否输出详细日志
this->declare_parameter("enable_crc", false); // 是否启用CRC校验（默认关闭）
// UKF 参数
this->declare_parameter("ukf_process_noise", 0.001); // 过程噪声
this->declare_parameter("ukf_measurement_noise", 0.03); // 测量噪声
this->declare_parameter("ukf_dt", 0.01); // 采样时间
// 获取参数
serial_port_ = this->get_parameter("serial_port").as_string();
baudrate_ = this->get_parameter("baudrate").as_int();
enable_crc_ = this->get_parameter("enable_crc").as_bool();
// 获取 UKF 参数并设置
double process_noise = this->get_parameter("ukf_process_noise").as_double();
double measurement_noise =
this->get_parameter("ukf_measurement_noise").as_double();
double dt = this->get_parameter("ukf_dt").as_double();
// 设置 UKF 参数
ukf_x_.setProcessNoise(process_noise);
ukf_x_.setMeasurementNoise(measurement_noise);
ukf_x_.setDt(dt);
ukf_y_.setProcessNoise(process_noise);
ukf_y_.setMeasurementNoise(measurement_noise);
ukf_y_.setDt(dt);
ukf_z_.setProcessNoise(process_noise);
ukf_z_.setMeasurementNoise(measurement_noise);
ukf_z_.setDt(dt);
RCLCPP_INFO(this->get_logger(), "---------------------------------");
RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点");
RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点 (UKF 版本)");
RCLCPP_INFO(this->get_logger(), "串口: %s", serial_port_.c_str());
RCLCPP_INFO(this->get_logger(), "波特率: %d", baudrate_);
RCLCPP_INFO(this->get_logger(), "CRC校验: %s",
enable_crc_ ? "启用" : "禁用");
RCLCPP_INFO(this->get_logger(),
"UKF 参数 - 过程噪声: %.4f, 测量噪声: %.4f, dt: %.4f",
process_noise, measurement_noise, dt);
RCLCPP_INFO(this->get_logger(), "---------------------------------");
// 创建发布者
ahrs_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/ahrs", 10);
body_vel_pub_ = this->create_publisher<geometry_msgs::msg::Vector3>(
"imu/body_velocity", 10);
imu_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
accel_pub_ = this->create_publisher<geometry_msgs::msg::Vector3>(
"imu/body_acceleration", 10);
connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>(
"imu/connection_status", 10);
@@ -351,9 +588,6 @@ private:
std::vector<uint8_t> current_frame;
current_frame.reserve(MAX_FRAME_SIZE);
// 当前正在积累的 IMU 数据帧
ImuDataFrame current_data;
while (running_ && rclcpp::ok()) {
ssize_t n = read(serial_fd_, read_buf, sizeof(read_buf));
@@ -396,8 +630,8 @@ private:
rx_buffer.begin() + i + 5 + remaining);
if (current_frame.back() == FRAME_END) {
// 解析帧到 current_data
parseFrameToData(current_frame, current_data);
// 解析帧
parseFrame(current_frame);
}
in_frame = false;
i += total_frame_len;
@@ -414,7 +648,7 @@ private:
} else {
current_frame.push_back(byte);
if (byte == FRAME_END) {
parseFrameToData(current_frame, current_data);
parseFrame(current_frame);
in_frame = false;
i++;
} else if (current_frame.size() >= MAX_FRAME_SIZE) {
@@ -437,8 +671,8 @@ private:
}
}
// 解析帧到数据结构 - 独立处理，不等待配对
void parseFrameToData(const std::vector<uint8_t> &frame, ImuDataFrame &data) {
// 解析帧 - 只处理 MSG_BODY_ACCELERATION
void parseFrame(const std::vector<uint8_t> &frame) {
if (frame.size() < 8)
return;
if (frame.back() != FRAME_END)
@@ -458,43 +692,29 @@ private:
}
const uint8_t *payload = frame.data() + 7;
bool should_push = false;
switch (msg_id) {
case MSG_AHRS:
if (data_len >= 48) {
data.ahrs_valid = true;
data.roll_speed = *reinterpret_cast<const float *>(payload);
data.pitch_speed = *reinterpret_cast<const float *>(payload + 4);
data.heading_speed = *reinterpret_cast<const float *>(payload + 8);
data.roll = *reinterpret_cast<const float *>(payload + 12);
data.pitch = *reinterpret_cast<const float *>(payload + 16);
data.heading = *reinterpret_cast<const float *>(payload + 20);
data.q1 = *reinterpret_cast<const float *>(payload + 24);
data.q2 = *reinterpret_cast<const float *>(payload + 28);
data.q3 = *reinterpret_cast<const float *>(payload + 32);
data.q4 = *reinterpret_cast<const float *>(payload + 36);
data.timestamp_us = *reinterpret_cast<const int64_t *>(payload + 40);
should_push = true;
}
break;
// 只处理 MSG_BODY_ACCELERATION 数据包 (0x62)
if (msg_id == MSG_BODY_ACCELERATION) {
if (data_len >= 16) {
ImuDataFrame data;
data.valid = true;
case MSG_BODY_VEL:
if (data_len >= 12) {
data.vel_valid = true;
data.vel_x = *reinterpret_cast<const float *>(payload);
data.vel_y = *reinterpret_cast<const float *>(payload + 4);
data.vel_z = *reinterpret_cast<const float *>(payload + 8);
should_push = true;
}
break;
}
// 滤波修正后的机体系加速度 (m/s²) - 不包括重力
data.acc_x = *reinterpret_cast<const float *>(payload);
data.acc_y = *reinterpret_cast<const float *>(payload + 4);
data.acc_z = *reinterpret_cast<const float *>(payload + 8);
// 只要有有效数据就推入缓冲区（不等待配对）
if (should_push) {
// 当地重力加速度 (m/s²)
data.g_force = *reinterpret_cast<const float *>(payload + 12);
data.receive_time = std::chrono::steady_clock::now();
// 推入环形缓冲区
ring_buffer_.push(data);
}
}
// 其他数据包类型被忽略
}
// 处理循环 - 固定频率从环形缓冲区读取并发布
void processLoop() {
@@ -523,49 +743,51 @@ private:
}
}
// 发布数据
// 发布数据 - 使用 UKF 对加速度进行滤波
void publishData(const ImuDataFrame &data) {
// 时间戳转换
rclcpp::Time current_time = this->now();
rclcpp::Time imu_time;
if (!imu_time_initialized_) {
imu_base_time_ =
current_time - rclcpp::Duration(0, data.timestamp_us * 1000);
imu_time_initialized_ = true;
}
imu_time = imu_base_time_ + rclcpp::Duration(0, data.timestamp_us * 1000);
latest_imu_timestamp_ = imu_time;
// 使用 UKF 对加速度数据进行滤波
float filtered_acc_x = ukf_x_.update(data.acc_x);
float filtered_acc_y = ukf_y_.update(data.acc_y);
float filtered_acc_z = ukf_z_.update(data.acc_z);
// 发布加速度消息 (滤波后的数据)
geometry_msgs::msg::Vector3 accel_msg;
accel_msg.x = filtered_acc_x;
accel_msg.y = filtered_acc_y;
accel_msg.z = filtered_acc_z;
accel_pub_->publish(accel_msg);
// 发布 IMU 消息
sensor_msgs::msg::Imu imu_msg;
imu_msg.header.stamp = imu_time;
imu_msg.header.stamp = current_time;
imu_msg.header.frame_id = "imu_link";
imu_msg.angular_velocity.x = data.roll_speed;
imu_msg.angular_velocity.y = data.pitch_speed;
imu_msg.angular_velocity.z = data.heading_speed;
imu_msg.orientation.x = data.q2;
imu_msg.orientation.y = data.q3;
imu_msg.orientation.z = data.q4;
imu_msg.orientation.w = data.q1;
imu_msg.linear_acceleration.x = 0;
imu_msg.linear_acceleration.y = 0;
imu_msg.linear_acceleration.z = 0;
ahrs_pub_->publish(imu_msg);
// 发布速度
geometry_msgs::msg::Vector3 vel_msg;
vel_msg.x = data.vel_x;
vel_msg.y = data.vel_y;
vel_msg.z = data.vel_z;
body_vel_pub_->publish(vel_msg);
// 线性加速度 - 使用 UKF 滤波后的值
imu_msg.linear_acceleration.x = filtered_acc_x;
imu_msg.linear_acceleration.y = filtered_acc_y;
imu_msg.linear_acceleration.z = filtered_acc_z;
// 角速度和姿态暂时设为0（MSG_BODY_ACCELERATION 不包含这些数据）
imu_msg.angular_velocity.x = 0;
imu_msg.angular_velocity.y = 0;
imu_msg.angular_velocity.z = 0;
imu_msg.orientation.x = 0;
imu_msg.orientation.y = 0;
imu_msg.orientation.z = 0;
imu_msg.orientation.w = 1; // 单位四元数
imu_pub_->publish(imu_msg);
// 日志
RCLCPP_INFO(this->get_logger(),
"AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f | VEL - X: "
"%.3f, Y: %.3f, Z: %.3f",
data.roll, data.pitch, data.heading, data.vel_x, data.vel_y,
data.vel_z);
RCLCPP_INFO(
this->get_logger(),
"Body Accel(raw): [%.3f, %.3f, %.3f] | Accel(UKF): [%.3f, %.3f, "
"%.3f] m/s² | G-force: %.3f m/s²",
data.acc_x, data.acc_y, data.acc_z, filtered_acc_x, filtered_acc_y,
filtered_acc_z, data.g_force);
}
void publishStatus() {
@@ -592,13 +814,13 @@ private:
std::thread process_thread_;
RingBuffer<ImuDataFrame> ring_buffer_;
// IMU 时间戳同步
bool imu_time_initialized_ = false;
rclcpp::Time imu_base_time_;
rclcpp::Time latest_imu_timestamp_;
// UKF 滤波器（用于加速度滤波）
UKF ukf_x_;
UKF ukf_y_;
UKF ukf_z_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr ahrs_pub_;
rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr body_vel_pub_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr accel_pub_;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
rclcpp::TimerBase::SharedPtr status_timer_;
};