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-25 06:21:43 +08:00
parent 2cec9bb7e1
commit 57b7ce41e1
2 changed files with 200 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files

350
src/imu_receiver_node.cpp
View File

@@ -1,6 +1,6 @@
/**
* @file imu_receiver_node.cpp
* @brief IMU 数据接收节点 (FDI DETA10) - 高实时性版本
* @brief IMU 数据接收节点 (FDI DETA10) - 环形缓冲区版本
*
* 通过串口读取 FDI DETA10 IMU 模块的数据
* 波特率：921600
@@ -8,16 +8,21 @@
*
* 转发数据包：
* - MSG_AHRS (0x41): 航姿参考系统数据
* - MSG_BODY_ACCELERATION (0x62): 机体系加速度数据
* - MSG_BODY_VEL (0x60): 机体系速度数据 (IMU卡尔曼滤波后)
*
* 特性：使用环形缓冲区避免数据堆积爆发
*/
#include <atomic>
#include <condition_variable>
#include <cstring>
#include <errno.h>
#include <fcntl.h>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <mutex>
#include <queue>
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
@@ -34,8 +39,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_ACCELERATION = 0x62; // 机体系加速度数据
constexpr uint8_t MSG_AHRS = 0x41; // 航姿参考系统数据
constexpr uint8_t MSG_BODY_VEL = 0x60; // 机体系速度数据 (卡尔曼滤波后)
// 默认串口设备
constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyIMU";
@@ -45,6 +50,12 @@ constexpr int DEFAULT_BAUDRATE = 921600;
constexpr size_t SERIAL_READ_BUF_SIZE = 4096;
constexpr size_t MAX_FRAME_SIZE = 256;
// 环形缓冲区配置
constexpr size_t RING_BUFFER_SIZE = 16; // 环形缓冲区最大帧数
constexpr size_t MAX_PUBLISH_RATE_HZ = 100; // 最大发布频率 100Hz
constexpr auto PUBLISH_PERIOD =
std::chrono::microseconds(1000000 / MAX_PUBLISH_RATE_HZ); // 10ms
// CRC8 查找表
static const uint8_t CRC8Table[] = {
0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31,
@@ -98,14 +109,87 @@ 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;
bool vel_valid = false;
float vel_x = 0, vel_y = 0, vel_z = 0;
};
// 线程安全的环形缓冲区
template <typename T> class RingBuffer {
public:
explicit RingBuffer(size_t size) : max_size_(size) {}
// 推入数据，如果满则丢弃最旧的数据
void push(const T &item) {
std::lock_guard<std::mutex> lock(mutex_);
if (buffer_.size() >= max_size_) {
buffer_.pop(); // 丢弃最旧的数据
}
buffer_.push(item);
cond_var_.notify_one();
}
// 非阻塞弹出
bool try_pop(T &item) {
std::lock_guard<std::mutex> lock(mutex_);
if (buffer_.empty()) {
return false;
}
item = buffer_.front();
buffer_.pop();
return true;
}
// 获取最新数据（不清除）
bool peek_latest(T &item) {
std::lock_guard<std::mutex> lock(mutex_);
if (buffer_.empty()) {
return false;
}
item = buffer_.back(); // 返回最新的
return true;
}
// 清空缓冲区
void clear() {
std::lock_guard<std::mutex> lock(mutex_);
while (!buffer_.empty())
buffer_.pop();
}
size_t size() {
std::lock_guard<std::mutex> lock(mutex_);
return buffer_.size();
}
bool empty() {
std::lock_guard<std::mutex> lock(mutex_);
return buffer_.empty();
}
private:
std::queue<T> buffer_;
size_t max_size_;
std::mutex mutex_;
std::condition_variable cond_var_;
};
class ImuReceiverNode : public rclcpp::Node {
public:
ImuReceiverNode() : Node("imu_receiver_node") {
ImuReceiverNode()
: Node("imu_receiver_node"), ring_buffer_(RING_BUFFER_SIZE) {
// 声明参数
this->declare_parameter("serial_port", DEFAULT_SERIAL_PORT);
this->declare_parameter("baudrate", DEFAULT_BAUDRATE);
this->declare_parameter("verbose", true); // 是否输出详细日志
this->declare_parameter("enable_crc", true); // 是否启用CRC校验（默认关闭）
this->declare_parameter("enable_crc", false); // 是否启用CRC校验（默认关闭）
// 获取参数
serial_port_ = this->get_parameter("serial_port").as_string();
@@ -122,10 +206,8 @@ public:
// 创建发布者
ahrs_pub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/ahrs", 10);
body_acc_pub_ = this->create_publisher<geometry_msgs::msg::Vector3>(
"imu/body_acceleration", 10);
g_force_pub_ =
this->create_publisher<std_msgs::msg::Float32>("imu/g_force", 10);
body_vel_pub_ = this->create_publisher<geometry_msgs::msg::Vector3>(
"imu/body_velocity", 10);
connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>(
"imu/connection_status", 10);
@@ -139,12 +221,17 @@ public:
// 创建接收线程
receive_thread_ = std::thread(&ImuReceiverNode::receiveLoop, this);
// 创建处理线程（固定频率处理数据）
process_thread_ = std::thread(&ImuReceiverNode::processLoop, this);
// 创建状态发布定时器
status_timer_ = this->create_wall_timer(
std::chrono::seconds(1),
std::bind(&ImuReceiverNode::publishStatus, this));
RCLCPP_INFO(this->get_logger(), "IMU 节点初始化完成");
RCLCPP_INFO(this->get_logger(), "环形缓冲区大小: %zu, 最大发布频率: %zu Hz",
RING_BUFFER_SIZE, MAX_PUBLISH_RATE_HZ);
}
~ImuReceiverNode() {
@@ -152,6 +239,9 @@ public:
if (receive_thread_.joinable()) {
receive_thread_.join();
}
if (process_thread_.joinable()) {
process_thread_.join();
}
if (serial_fd_ >= 0) {
close(serial_fd_);
}
@@ -252,45 +342,36 @@ private:
return crc16;
}
// 接收循环
// 接收循环 - 只负责解析数据并存入环形缓冲区
void receiveLoop() {
// 双缓冲区：一个用于接收，一个用于处理
std::vector<uint8_t> rx_buffer;
rx_buffer.reserve(SERIAL_READ_BUF_SIZE * 2);
// 临时读取缓冲区
uint8_t read_buf[SERIAL_READ_BUF_SIZE];
// 帧解析状态
bool in_frame = false;
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));
if (n > 0) {
// 将新数据追加到缓冲区
rx_buffer.insert(rx_buffer.end(), read_buf, read_buf + n);
// 处理缓冲区中的数据
size_t i = 0;
while (i < rx_buffer.size()) {
uint8_t byte = rx_buffer[i];
if (!in_frame) {
// 查找帧头
if (byte == FRAME_START) {
// 检查是否有足够的字节来读取帧头信息
if (i + 5 <= rx_buffer.size()) {
// 读取帧头信息
uint8_t msg_id = rx_buffer[i + 1];
uint8_t data_len = rx_buffer[i + 2];
uint8_t seq = rx_buffer[i + 3];
uint8_t header_crc = rx_buffer[i + 4];
// 可选的CRC校验
bool header_valid = true;
if (enable_crc_) {
uint8_t header[4] = {FRAME_START, msg_id, data_len, seq};
@@ -298,7 +379,6 @@ private:
}
if (header_valid) {
// 开始收集帧数据
in_frame = true;
current_frame.clear();
current_frame.push_back(FRAME_START);
@@ -307,32 +387,24 @@ private:
current_frame.push_back(seq);
current_frame.push_back(header_crc);
// 计算还需要读取多少字节
size_t total_frame_len =
5 + 2 + data_len + 1; // 头+CRC16+数据+尾
size_t remaining = total_frame_len - 5; // 已经读取了5字节
size_t total_frame_len = 5 + 2 + data_len + 1;
size_t remaining = total_frame_len - 5;
// 检查缓冲区中是否有足够的数据
if (i + 5 + remaining <= rx_buffer.size()) {
// 数据足够，直接复制剩余部分
current_frame.insert(current_frame.end(),
rx_buffer.begin() + i + 5,
rx_buffer.begin() + i + 5 + remaining);
// 检查帧尾
if (current_frame.back() == FRAME_END) {
// 解析帧
processFrame(current_frame);
// 解析帧到 current_data
parseFrameToData(current_frame, current_data);
}
// 无论是否有帧尾，都结束当前帧处理
in_frame = false;
i += total_frame_len;
continue;
} else {
// 数据不够，需要等待更多数据
// 删除已处理的部分，保留当前帧的后续数据
rx_buffer.erase(rx_buffer.begin(), rx_buffer.begin() + i);
i = 5; // 保留当前帧头
i = 5;
break;
}
}
@@ -340,17 +412,12 @@ private:
}
i++;
} else {
// 已经在帧中，继续收集数据
current_frame.push_back(byte);
// 检查是否到达帧尾
if (byte == FRAME_END) {
// 解析帧
processFrame(current_frame);
parseFrameToData(current_frame, current_data);
in_frame = false;
i++;
} else if (current_frame.size() >= MAX_FRAME_SIZE) {
// 帧太长，丢弃
in_frame = false;
i++;
} else {
@@ -359,40 +426,30 @@ private:
}
}
// 如果不在帧中且缓冲区太大，清理旧数据
if (!in_frame && rx_buffer.size() > SERIAL_READ_BUF_SIZE) {
// 保留最后一部分数据，可能包含不完整的帧头
if (rx_buffer.size() > 100) {
rx_buffer.erase(rx_buffer.begin(), rx_buffer.end() - 100);
}
}
} else if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK) {
// 读取错误
RCLCPP_ERROR(this->get_logger(), "串口读取错误: %s", strerror(errno));
}
// n == 0 或 EAGAIN: 没有数据可读，继续循环
}
}
// 处理完整的一帧数据
void processFrame(const std::vector<uint8_t> &frame) {
// 最小帧长度检查：头(5) + CRC16(2) + 尾(1) = 8
// 解析帧到数据结构 - 独立处理，不等待配对
void parseFrameToData(const std::vector<uint8_t> &frame, ImuDataFrame &data) {
if (frame.size() < 8)
return;
// 检查帧尾
if (frame.back() != FRAME_END)
return;
uint8_t msg_id = frame[1];
uint8_t data_len = frame[2];
// 验证数据长度
size_t expected_len = 5 + 2 + data_len + 1;
if (frame.size() != expected_len)
return;
// 可选的数据CRC校验
if (enable_crc_) {
uint16_t data_crc = (frame[5] << 8) | frame[6];
uint16_t calc_crc = calculateCRC16(frame.data() + 7, data_len);
@@ -400,129 +457,115 @@ private:
return;
}
// 解析数据包
parsePacket(msg_id, frame.data() + 7, data_len);
}
const uint8_t *payload = frame.data() + 7;
bool should_push = false;
void parsePacket(uint8_t msg_id, const uint8_t *data, uint8_t len) {
switch (msg_id) {
case MSG_AHRS:
ahrs_count_++;
parseAHRS(data, len);
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;
case MSG_BODY_ACCELERATION:
body_acc_count_++;
parseBodyAcceleration(data, len);
break;
default:
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;
}
// 只要有有效数据就推入缓冲区（不等待配对）
if (should_push) {
ring_buffer_.push(data);
}
}
// 解析 AHRS 数据 (0x41)
void parseAHRS(const uint8_t *data, uint8_t len) {
if (len < 48)
return;
// 处理循环 - 固定频率从环形缓冲区读取并发布
void processLoop() {
ImuDataFrame data;
auto next_publish_time = std::chrono::steady_clock::now();
// 解析角速度 (rad/s)
float roll_speed = *reinterpret_cast<const float *>(data);
float pitch_speed = *reinterpret_cast<const float *>(data + 4);
float heading_speed = *reinterpret_cast<const float *>(data + 8);
while (running_ && rclcpp::ok()) {
// 等待到下一次发布时间
std::this_thread::sleep_until(next_publish_time);
next_publish_time += PUBLISH_PERIOD;
// 解析欧拉角 (rad)
float roll = *reinterpret_cast<const float *>(data + 12);
float pitch = *reinterpret_cast<const float *>(data + 16);
float heading = *reinterpret_cast<const float *>(data + 20);
// 获取最新的数据（如果有）
// 策略：只取最新的一个数据，丢弃中间堆积的数据
bool has_data = false;
ImuDataFrame latest_data;
// 解析四元数
float q1 = *reinterpret_cast<const float *>(data + 24);
float q2 = *reinterpret_cast<const float *>(data + 28);
float q3 = *reinterpret_cast<const float *>(data + 32);
float q4 = *reinterpret_cast<const float *>(data + 36);
// 清空缓冲区，只保留最新的
while (ring_buffer_.try_pop(data)) {
latest_data = data;
has_data = true;
}
// 解析时间戳 (int64_t, us)
int64_t timestamp_us = *reinterpret_cast<const int64_t *>(data + 40);
if (has_data) {
publishData(latest_data);
}
}
}
// 将 IMU 时间戳转换为 ROS 时间
// 使用当前 ROS 时间作为基准，计算相对时间
// 发布数据
void publishData(const ImuDataFrame &data) {
// 时间戳转换
rclcpp::Time current_time = this->now();
rclcpp::Time imu_time =
imu_base_time_ + rclcpp::Duration(0, timestamp_us * 1000);
rclcpp::Time imu_time;
// 如果是第一个数据包，初始化基准时间
if (!imu_time_initialized_) {
imu_base_time_ = current_time - rclcpp::Duration(0, timestamp_us * 1000);
imu_time = current_time;
imu_base_time_ =
current_time - rclcpp::Duration(0, data.timestamp_us * 1000);
imu_time_initialized_ = true;
}
// 保存最新的 IMU 时间戳，供 BodyAcceleration 使用
imu_time = imu_base_time_ + rclcpp::Duration(0, data.timestamp_us * 1000);
latest_imu_timestamp_ = imu_time;
// 发布 IMU 消息
sensor_msgs::msg::Imu imu_msg;
imu_msg.header.stamp = imu_time;
imu_msg.header.frame_id = "imu_link";
// 角速度 (转换为 ROS 坐标系: x=roll, y=pitch, z=heading)
imu_msg.angular_velocity.x = roll_speed;
imu_msg.angular_velocity.y = pitch_speed;
imu_msg.angular_velocity.z = heading_speed;
// 四元数 (FDI: Q1=w, Q2=x, Q3=y, Q4=z -> ROS: x,y,z,w)
imu_msg.orientation.x = q2;
imu_msg.orientation.y = q3;
imu_msg.orientation.z = q4;
imu_msg.orientation.w = q1;
// 线性加速度暂时设为0（从 MSG_BODY_ACCELERATION 获取）
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);
// 输出 AHRS 日志
RCLCPP_INFO(
this->get_logger(),
"AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad, Time: %ld us", roll,
pitch, heading, timestamp_us);
}
// 发布速度
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);
// 解析机体系加速度数据 (0x62)
void parseBodyAcceleration(const uint8_t *data, uint8_t len) {
if (len < 16)
return;
// 解析机体系加速度 (m/s^2)
float acc_x = *reinterpret_cast<const float *>(data);
float acc_y = *reinterpret_cast<const float *>(data + 4);
float acc_z = *reinterpret_cast<const float *>(data + 8);
// 解析重力加速度
float g_force = *reinterpret_cast<const float *>(data + 12);
// 使用与 AHRS 相同的时间戳（如果可用），否则使用当前时间
rclcpp::Time msg_time = latest_imu_timestamp_;
if (msg_time.nanoseconds() == 0) {
msg_time = this->now();
}
// 发布机体系加速度
geometry_msgs::msg::Vector3 acc_msg;
acc_msg.x = acc_x;
acc_msg.y = acc_y;
acc_msg.z = acc_z;
body_acc_pub_->publish(acc_msg);
// 发布重力加速度
std_msgs::msg::Float32 g_msg;
g_msg.data = g_force;
g_force_pub_->publish(g_msg);
// 输出日志
RCLCPP_INFO(this->get_logger(), "ACC - X: %.3f, Y: %.3f, Z: %.3f m/s^2",
acc_x, acc_y, acc_z);
// 日志
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);
}
void publishStatus() {
@@ -538,16 +581,16 @@ private:
// 成员变量
std::string serial_port_;
int baudrate_;
bool enable_crc_ = true; // CRC校验开关
bool enable_crc_ = false; // CRC校验开关
int serial_fd_ = -1;
bool is_connected_ = false;
std::atomic<bool> running_{true};
std::thread receive_thread_;
// 统计信息
std::atomic<size_t> ahrs_count_;
std::atomic<size_t> body_acc_count_;
// 线程
std::thread process_thread_;
RingBuffer<ImuDataFrame> ring_buffer_;
// IMU 时间戳同步
bool imu_time_initialized_ = false;
@@ -555,8 +598,7 @@ private:
rclcpp::Time latest_imu_timestamp_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr ahrs_pub_;
rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr body_acc_pub_;
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr g_force_pub_;
rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr body_vel_pub_;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
rclcpp::TimerBase::SharedPtr status_timer_;
};

8
src/uart_transmitter_node.cpp
View File

@@ -41,11 +41,11 @@ public:
this->declare_parameter("x_move", 0); // 平动左右 [-660, 660] 负为向左
this->declare_parameter("y_move", 0); // 平动前后 [-660, 660] 负为向后
this->declare_parameter("yaw", 0); // 云台偏航 [-660, 660] 负为向左
this->declare_parameter("pitch", 100); // 云台俯仰 [-660, 660] 负为向下
this->declare_parameter("pitch", 0); // 云台俯仰 [-660, 660] 负为向下
this->declare_parameter("feed",
660); // 拨弹轮 [-660, 660]， 660 开火，0 无动作
0); // 拨弹轮 [-660, 660]， 660 开火，0 无动作
this->declare_parameter("left_switch",
2); // 左拨杆 [1, 3] 2 摩擦轮打开 3 关闭
3); // 左拨杆 [1, 3] 2 摩擦轮打开 3 关闭
this->declare_parameter("right_switch",
3); // 右拨杆 [1, 3] 2 小陀螺 3 关闭
@@ -181,7 +181,7 @@ private:
case 921600:
return B921600;
default:
RCLCPP_WARN(this->get_logger(), "不支持的波特率 %d，使用 115200",
RCLCPP_WARN(this->get_logger(), "不支持的波特率 %d, 使用 115200",
baudrate);
return B115200;
}