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

添加 IMU 节点时间戳，优化日志

Browse Source
This commit is contained in:
Huang Haoyu
2026-03-24 01:47:33 +08:00
parent 01d236d1a0
commit 948c0be417
2 changed files with 40 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
src/imu_receiver_node.cpp
View File

@@ -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_;

4
src/uart_transmitter_node.cpp
View File

@@ -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,