From 948c0be4177bd1d4801a1a742bddf2c9d60a13ba Mon Sep 17 00:00:00 2001 From: HelixCopex Date: Tue, 24 Mar 2026 01:47:33 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=20IMU=20=E8=8A=82=E7=82=B9?= =?UTF-8?q?=E6=97=B6=E9=97=B4=E6=88=B3=EF=BC=8C=E4=BC=98=E5=8C=96=E6=97=A5?= =?UTF-8?q?=E5=BF=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/imu_receiver_node.cpp | 45 +++++++++++++++++++++++++++++------ src/uart_transmitter_node.cpp | 4 ++-- 2 files changed, 40 insertions(+), 9 deletions(-) diff --git a/src/imu_receiver_node.cpp b/src/imu_receiver_node.cpp index ad240bf..49c39d8 100644 --- a/src/imu_receiver_node.cpp +++ b/src/imu_receiver_node.cpp @@ -37,7 +37,7 @@ constexpr uint8_t MSG_AHRS = 0x41; // 航姿参考系统数据 constexpr uint8_t MSG_BODY_ACCELERATION = 0x62; // 机体系加速度数据 // 默认串口设备 -constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyUSB1"; +constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyIMU"; constexpr int DEFAULT_BAUDRATE = 921600; // 串口读取缓冲区大小 (921600bps ≈ 115KB/s, 缓冲区需要足够大) @@ -439,9 +439,28 @@ private: float q3 = *reinterpret_cast(data + 32); float q4 = *reinterpret_cast(data + 36); + // 解析时间戳 (int64_t, us) + int64_t timestamp_us = *reinterpret_cast(data + 40); + + // 将 IMU 时间戳转换为 ROS 时间 + // 使用当前 ROS 时间作为基准，计算相对时间 + rclcpp::Time current_time = this->now(); + rclcpp::Time imu_time = + imu_base_time_ + rclcpp::Duration(0, timestamp_us * 1000); + + // 如果是第一个数据包，初始化基准时间 + if (!imu_time_initialized_) { + imu_base_time_ = current_time - rclcpp::Duration(0, timestamp_us * 1000); + imu_time = current_time; + imu_time_initialized_ = true; + } + + // 保存最新的 IMU 时间戳，供 BodyAcceleration 使用 + latest_imu_timestamp_ = imu_time; + // 发布 IMU 消息 sensor_msgs::msg::Imu imu_msg; - imu_msg.header.stamp = this->now(); + imu_msg.header.stamp = imu_time; imu_msg.header.frame_id = "imu_link"; // 角速度 (转换为 ROS 坐标系: x=roll, y=pitch, z=heading) @@ -465,9 +484,10 @@ private: // 根据 verbose 参数输出日志 bool verbose = this->get_parameter("verbose").as_bool(); if (verbose) { - RCLCPP_INFO(this->get_logger(), - "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad", roll, - pitch, heading); + RCLCPP_INFO( + this->get_logger(), + "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad, Time: %ld us", + roll, pitch, heading, timestamp_us); } else { RCLCPP_DEBUG(this->get_logger(), "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad", roll, @@ -488,6 +508,12 @@ private: // 解析重力加速度 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; @@ -504,11 +530,11 @@ private: bool verbose = this->get_parameter("verbose").as_bool(); if (verbose) { RCLCPP_INFO(this->get_logger(), - "BodyAcc - X: %.3f, Y: %.3f, Z: %.3f m/s², G: %.3f", acc_x, + "BodyAcc - X: %.3f, Y: %.3f, Z: %.3f m/s2, G: %.3f", acc_x, acc_y, acc_z, g_force); } else { RCLCPP_DEBUG(this->get_logger(), - "BodyAcc - X: %.3f, Y: %.3f, Z: %.3f m/s², G: %.3f", acc_x, + "BodyAcc - X: %.3f, Y: %.3f, Z: %.3f m/s2, G: %.3f", acc_x, acc_y, acc_z, g_force); } } @@ -539,6 +565,11 @@ private: std::atomic ahrs_count_; std::atomic body_acc_count_; + // IMU 时间戳同步 + bool imu_time_initialized_ = false; + rclcpp::Time imu_base_time_; + rclcpp::Time latest_imu_timestamp_; + rclcpp::Publisher::SharedPtr ahrs_pub_; rclcpp::Publisher::SharedPtr body_acc_pub_; rclcpp::Publisher::SharedPtr g_force_pub_; diff --git a/src/uart_transmitter_node.cpp b/src/uart_transmitter_node.cpp index 9063546..9413ffd 100644 --- a/src/uart_transmitter_node.cpp +++ b/src/uart_transmitter_node.cpp @@ -252,7 +252,7 @@ private: snprintf(buf, sizeof(buf), "%02X ", frame[i]); frame_hex += buf; } - RCLCPP_INFO(this->get_logger(), "[发包](%d字节): %s", FRAME_LENGTH, + RCLCPP_INFO(this->get_logger(), "[TX](%dB): %s", FRAME_LENGTH, frame_hex.c_str()); // 发送数据 @@ -321,7 +321,7 @@ private: // 输出详细日志 RCLCPP_INFO(this->get_logger(), - "[收包]: %02X %02X | x:%d y:%d yaw:%d " + "[RX]: %02X %02X | x:%d y:%d yaw:%d " "pitch:%d feed:%d | L:%d R:%d | CRC:%02X | %02X", frame_buffer[0], frame_buffer[1], x_move, y_move, yaw, pitch, feed, left_switch, right_switch,