From 57b7ce41e16f8c2df7723dd3986319bf3bee9864 Mon Sep 17 00:00:00 2001 From: HelixCopex Date: Wed, 25 Mar 2026 06:21:43 +0800 Subject: [PATCH] =?UTF-8?q?=E7=8E=AF=E5=BD=A2=E7=BC=93=E5=86=B2=E5=8C=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/imu_receiver_node.cpp | 350 +++++++++++++++++++--------------- src/uart_transmitter_node.cpp | 8 +- 2 files changed, 200 insertions(+), 158 deletions(-) diff --git a/src/imu_receiver_node.cpp b/src/imu_receiver_node.cpp index 3dd85b2..ca1bce4 100644 --- a/src/imu_receiver_node.cpp +++ b/src/imu_receiver_node.cpp @@ -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 +#include #include #include #include #include #include #include +#include +#include #include #include #include @@ -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 class RingBuffer { +public: + explicit RingBuffer(size_t size) : max_size_(size) {} + + // 推入数据，如果满则丢弃最旧的数据 + void push(const T &item) { + std::lock_guard lock(mutex_); + if (buffer_.size() >= max_size_) { + buffer_.pop(); // 丢弃最旧的数据 + } + buffer_.push(item); + cond_var_.notify_one(); + } + + // 非阻塞弹出 + bool try_pop(T &item) { + std::lock_guard lock(mutex_); + if (buffer_.empty()) { + return false; + } + item = buffer_.front(); + buffer_.pop(); + return true; + } + + // 获取最新数据（不清除） + bool peek_latest(T &item) { + std::lock_guard lock(mutex_); + if (buffer_.empty()) { + return false; + } + item = buffer_.back(); // 返回最新的 + return true; + } + + // 清空缓冲区 + void clear() { + std::lock_guard lock(mutex_); + while (!buffer_.empty()) + buffer_.pop(); + } + + size_t size() { + std::lock_guard lock(mutex_); + return buffer_.size(); + } + + bool empty() { + std::lock_guard lock(mutex_); + return buffer_.empty(); + } + +private: + std::queue 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("imu/ahrs", 10); - body_acc_pub_ = this->create_publisher( - "imu/body_acceleration", 10); - g_force_pub_ = - this->create_publisher("imu/g_force", 10); + body_vel_pub_ = this->create_publisher( + "imu/body_velocity", 10); connection_status_pub_ = this->create_publisher( "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 rx_buffer; rx_buffer.reserve(SERIAL_READ_BUF_SIZE * 2); - - // 临时读取缓冲区 uint8_t read_buf[SERIAL_READ_BUF_SIZE]; - - // 帧解析状态 bool in_frame = false; std::vector 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 &frame) { - // 最小帧长度检查：头(5) + CRC16(2) + 尾(1) = 8 + // 解析帧到数据结构 - 独立处理，不等待配对 + void parseFrameToData(const std::vector &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(payload); + data.pitch_speed = *reinterpret_cast(payload + 4); + data.heading_speed = *reinterpret_cast(payload + 8); + data.roll = *reinterpret_cast(payload + 12); + data.pitch = *reinterpret_cast(payload + 16); + data.heading = *reinterpret_cast(payload + 20); + data.q1 = *reinterpret_cast(payload + 24); + data.q2 = *reinterpret_cast(payload + 28); + data.q3 = *reinterpret_cast(payload + 32); + data.q4 = *reinterpret_cast(payload + 36); + data.timestamp_us = *reinterpret_cast(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(payload); + data.vel_y = *reinterpret_cast(payload + 4); + data.vel_z = *reinterpret_cast(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(data); - float pitch_speed = *reinterpret_cast(data + 4); - float heading_speed = *reinterpret_cast(data + 8); + while (running_ && rclcpp::ok()) { + // 等待到下一次发布时间 + std::this_thread::sleep_until(next_publish_time); + next_publish_time += PUBLISH_PERIOD; - // 解析欧拉角 (rad) - float roll = *reinterpret_cast(data + 12); - float pitch = *reinterpret_cast(data + 16); - float heading = *reinterpret_cast(data + 20); + // 获取最新的数据（如果有） + // 策略：只取最新的一个数据，丢弃中间堆积的数据 + bool has_data = false; + ImuDataFrame latest_data; - // 解析四元数 - float q1 = *reinterpret_cast(data + 24); - float q2 = *reinterpret_cast(data + 28); - float q3 = *reinterpret_cast(data + 32); - float q4 = *reinterpret_cast(data + 36); + // 清空缓冲区，只保留最新的 + while (ring_buffer_.try_pop(data)) { + latest_data = data; + has_data = true; + } - // 解析时间戳 (int64_t, us) - int64_t timestamp_us = *reinterpret_cast(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(data); - float acc_y = *reinterpret_cast(data + 4); - float acc_z = *reinterpret_cast(data + 8); - - // 解析重力加速度 - float g_force = *reinterpret_cast(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 running_{true}; std::thread receive_thread_; - // 统计信息 - std::atomic ahrs_count_; - std::atomic body_acc_count_; + // 线程 + std::thread process_thread_; + RingBuffer ring_buffer_; // IMU 时间戳同步 bool imu_time_initialized_ = false; @@ -555,8 +598,7 @@ private: rclcpp::Time latest_imu_timestamp_; rclcpp::Publisher::SharedPtr ahrs_pub_; - rclcpp::Publisher::SharedPtr body_acc_pub_; - rclcpp::Publisher::SharedPtr g_force_pub_; + rclcpp::Publisher::SharedPtr body_vel_pub_; rclcpp::Publisher::SharedPtr connection_status_pub_; rclcpp::TimerBase::SharedPtr status_timer_; }; diff --git a/src/uart_transmitter_node.cpp b/src/uart_transmitter_node.cpp index 886f883..c2e8c7e 100644 --- a/src/uart_transmitter_node.cpp +++ b/src/uart_transmitter_node.cpp @@ -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; }