From ed22f2575d3c56b2fdf81ddf23ef2d47f2cb450a Mon Sep 17 00:00:00 2001 From: Alvin-lyq Date: Wed, 29 Apr 2026 10:17:15 +0800 Subject: [PATCH] debug --- .../rtk_asensing/asensing_protocol.hpp | 24 ++++++++++++ .../rtk_asensing/src/rtk_asensing_node.cpp | 38 +++++++++++++++++-- 2 files changed, 58 insertions(+), 4 deletions(-) diff --git a/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp b/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp index 5ccea95..29c2a19 100644 --- a/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp +++ b/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp @@ -4,6 +4,7 @@ #include #include #include +#include "logger/logger.h" // Asensing协议4数据帧定义 // 帧头: 0xBD 0xDB @@ -63,6 +64,11 @@ class AsensingProtocol // 返回值: 1=成功解析一帧, 0=数据不足, -1=校验失败 int feedData(const uint8_t* data, int len) { + static int total_sync_attempts = 0; + static int total_checksum1_fail = 0; + static int total_checksum2_fail = 0; + static int total_frame_type_fail = 0; + for (int i = 0; i < len; i++) { if (!is_synced_) @@ -75,6 +81,8 @@ class AsensingProtocol else if (frame_index_ == 1 && data[i] == 0xDB) { frame_buf_[frame_index_++] = data[i]; + total_sync_attempts++; + LOG_DEBUG("[DEBUG-PROTO] 找到帧头 BD DB, 累计尝试: %d", total_sync_attempts); } else if (frame_index_ == 1 && data[i] != 0xDB) { @@ -100,6 +108,11 @@ class AsensingProtocol // 已同步帧头，继续接收数据 frame_buf_[frame_index_++] = data[i]; + if (frame_index_ == 3) + { + LOG_DEBUG("[DEBUG-PROTO] 帧类型: 0x%02X", frame_buf_[2]); + } + // 检查是否接收完整帧 (帧头2字节 + 类型1字节 + 数据58字节 + GPS周4字节 + 校验1字节 = 66字节) // 实际数据到偏移57是check1(1字节)，然后58-61是GPS周，62是check2 // 所以完整帧长 = 63字节 @@ -108,6 +121,8 @@ class AsensingProtocol // 检查帧类型 if (frame_buf_[2] != 0x0B) { + total_frame_type_fail++; + LOG_DEBUG("[DEBUG-PROTO] 帧类型不匹配: 0x%02X (期望 0x0B), 累计失败: %d", frame_buf_[2], total_frame_type_fail); // 帧类型不匹配，重新同步 is_synced_ = false; frame_index_ = 0; @@ -123,6 +138,9 @@ class AsensingProtocol if (xor_check1 != frame_buf_[57]) { + total_checksum1_fail++; + LOG_DEBUG("[DEBUG-PROTO] 校验位1失败: 计算=0x%02X, 接收=0x%02X, 累计失败: %d", + xor_check1, frame_buf_[57], total_checksum1_fail); // 校验失败，重新同步 is_synced_ = false; frame_index_ = 0; @@ -138,12 +156,18 @@ class AsensingProtocol if (xor_check2 != frame_buf_[62]) { + total_checksum2_fail++; + LOG_DEBUG("[DEBUG-PROTO] 校验位2失败: 计算=0x%02X, 接收=0x%02X, 累计失败: %d", + xor_check2, frame_buf_[62], total_checksum2_fail); // 校验失败，重新同步 is_synced_ = false; frame_index_ = 0; continue; } + LOG_DEBUG("[DEBUG-PROTO] 解析成功! 帧头: %02X %02X, 类型: %02X", + frame_buf_[0], frame_buf_[1], frame_buf_[2]); + // 校验通过，复制数据到结构体 memcpy(&raw_data_, frame_buf_, sizeof(AsensingInsData)); diff --git a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp index 295ac55..66c08e7 100644 --- a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp +++ b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp @@ -16,10 +16,10 @@ class RtkAsensingNode : public rclcpp::Node // 初始化串口读取 boost_serial = new Boost_serial(); - std::string serial_port; - int baud_rate; - this->declare_parameter("serial_port", "/dev/ttyTHS1"); - this->declare_parameter("baud_rate", 115200); + std::string serial_port = "/dev/ttyTHS1"; + int baud_rate = 115200; + this->declare_parameter("serial_port", serial_port); + this->declare_parameter("baud_rate", baud_rate); this->get_parameter("serial_port", serial_port); this->get_parameter("baud_rate", baud_rate); boost_serial->init(serial_port.c_str(), baud_rate); @@ -50,6 +50,10 @@ class RtkAsensingNode : public rclcpp::Node private: void timer_callback() { + static int read_count = 0; + static int parse_count = 0; + static int publish_count = 0; + try { // 读取串口数据 @@ -61,13 +65,39 @@ class RtkAsensingNode : public rclcpp::Node return; } + read_count++; + + // 打印读取的原始数据（仅前20字节） + if (read_count % 50 == 1) // 每50次打印一次，避免刷屏 + { + LOG_DEBUG("[DEBUG] 串口读取 %d 字节:", num); + int print_len = (num < 20) ? num : 20; + std::string hex_str; + for (int i = 0; i < print_len; i++) + { + char hex[4]; + snprintf(hex, sizeof(hex), "%02X ", (uint8_t)serial_buf[i]); + hex_str += hex; + } + LOG_DEBUG("[DEBUG] 前%d字节 HEX: %s", print_len, hex_str.c_str()); + } + // 将数据送入协议解析器 int result = protocol_parser->feedData(reinterpret_cast(serial_buf), num); if (result == 1) { + parse_count++; + LOG_DEBUG("[DEBUG] 解析成功! 累计解析帧数: %d", parse_count); + // 成功解析一帧数据，发布ROS消息 publishData(); + publish_count++; + + if (publish_count % 50 == 1) + { + LOG_DEBUG("[DEBUG] 累计发布次数: %d", publish_count); + } } } catch (const std::exception& e)