debug

src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp

@@ -4,6 +4,7 @@
 #include <cmath>
 #include <cstdint>
 #include <cstring>
+#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));
 

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;
+        std::string serial_port = "/dev/ttyTHS1";
-        int baud_rate;
+        int baud_rate = 115200;
-        this->declare_parameter("serial_port", "/dev/ttyTHS1");
+        this->declare_parameter("serial_port", serial_port);
-        this->declare_parameter("baud_rate", 115200);
+        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<uint8_t*>(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)