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<const float *>(data + 32);
     float q4 = *reinterpret_cast<const float *>(data + 36);
 
+    // 解析时间戳 (int64_t, us)
+    int64_t timestamp_us = *reinterpret_cast<const int64_t *>(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<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;
@@ -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<size_t> ahrs_count_;
   std::atomic<size_t> body_acc_count_;
 
+  // IMU 时间戳同步
+  bool imu_time_initialized_ = false;
+  rclcpp::Time imu_base_time_;
+  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_;

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,