Asensing RTK

src/perception/rtk_asensing/4 数据输出协议.md

@@ -0,0 +1,70 @@
+## 4 数据输出协议
+
+### 4.1 Asensing 协议输出
+
+#### 4.1.1 BD DB 0B 组合导航 INS 数据
+
+- 帧头：0xbd 0xdb
+- 帧类型：0x0B（组合导航模式，58字节，包含姿态、角速率、加速度、温度、经纬度、速度等）
+- 配置指令：`AGBDDB0B,[输出串口],[发送参数],[输出频率]`
+- 返回指令：`AGBDDB0B,[输出串口],[发送参数],[输出频率],OK`
+
+**参数说明**
+
+| 参数定义 | 取值范围 | 说明                            |
+| -------- | -------- | ------------------------------- |
+| 输出串口 | 0        | 从 UART0 输出，当前仅支持 UART0 |
+| 输出串口 | 3        | 从 UART3 输出（可选）           |
+| 发送参数 | 0        | 关闭数据输出                    |
+| 发送参数 | 1        | 打开数据输出                    |
+| 输出频率 | 20       | 20Hz 播发（50ms 1次）           |
+| 输出频率 | 50       | 50Hz 播发（20ms 1次）           |
+| 输出频率 | 100      | 100Hz 播发（10ms 1次）          |
+| 输出频率 | 200      | 200Hz 播发（5ms 1次）           |
+
+**输出格式**
+
+| 偏移  | 定义     | 格式 | 长度 | 系数      | 单位  | 字节序    | 说明                                                       |
+| ----- | -------- | ---- | ---- | --------- | ----- | --------- | ---------------------------------------------------------- |
+| 0     | 0xBD     | -    | 1    | -         | -     | -         | 帧头第一位                                                 |
+| 1     | 0xDB     | -    | 1    | -         | -     | -         | 帧头第二位                                                 |
+| 2     | 0x0B     | -    | 1    | -         | -     | -         | 帧类型（组合导航模式）                                     |
+| 3     | 横滚角   | I16  | 2    | 360/32768 | deg   | LSB_first | 取值范围[-90,90]                                           |
+| 5     | 俯仰角   | I16  | 2    | 360/32768 | deg   | LSB_first | 取值范围[-180,180]                                         |
+| 7     | 方位角   | I16  | 2    | 360/32768 | deg   | LSB_first | [-180,180]，正北顺时针到正南为正0-180°                     |
+| 9     | 陀螺x轴  | I16  | 2    | 300/32768 | deg/s | LSB_first | 取值范围[-250,250]                                         |
+| 11    | 陀螺y轴  | I16  | 2    | 300/32768 | deg/s | LSB_first | 取值范围[-250,250]                                         |
+| 13    | 陀螺z轴  | I16  | 2    | 300/32768 | deg/s | LSB_first | 取值范围[-250,250]                                         |
+| 15    | 加表x轴  | I16  | 2    | 12/32768  | g     | LSB_first | 取值范围[-4,4]，与量程有关（如4g）                         |
+| 17    | 加表y轴  | I16  | 2    | 12/32768  | g     | LSB_first | 取值范围[-4,4]，与量程有关（如4g）                         |
+| 19    | 加表z轴  | I16  | 2    | 12/32768  | g     | LSB_first | 取值范围[-4,4]，与量程有关（如4g）                         |
+| 21    | 纬度     | I32  | 4    | 1.00E-07  | deg   | LSB_first | [-90,90]，正为N                                            |
+| 25    | 经度     | I32  | 4    | 1.00E-07  | deg   | LSB_first | [-180,180]，正为E                                          |
+| 29    | 高度     | I32  | 4    | 1.00E-03  | m     | LSB_first | 海拔高度                                                   |
+| 33    | 北向速度 | I16  | 2    | 1e2/32768 | m/s   | LSB_first | 北向速度分量                                               |
+| 35    | 东向速度 | I16  | 2    | 1e2/32768 | m/s   | LSB_first | 东向速度分量                                               |
+| 37    | 地向速度 | I16  | 2    | 1e2/32768 | m/s   | LSB_first | 地向速度分量                                               |
+| 39    | 状态     | U8   | 1    | -         | -     | -         | bit0=位置，bit1=速度，bit2=姿态，bit3=航向角；1=完成初对准 |
+| 40-45 | 保留     | -    | 6    | -         | -     | -         | 预留字段                                                   |
+| 46    | Data1    | I16  | 2    | -         | -     | LSB_first | 轮循数据，参考轮询数据表                                   |
+| 48    | Data2    | I16  | 2    | -         | -     | LSB_first | 轮循数据，参考轮询数据表                                   |
+| 50    | Data3    | I16  | 2    | -         | -     | LSB_first | 轮循数据，参考轮询数据表                                   |
+| 52    | 时间     | U32  | 4    | 2.50E-04  | 秒    | LSB_first | GPS时间（周内秒），毫秒精度                                |
+| 56    | Type     | U8   | 1    | -         | -     | -         | 轮循数据类型，参考轮询数据表                               |
+| 57    | 校验位   | U8   | 1    | -         | -     | -         | 异或校验，包含0~56的数据                                   |
+| 58    | GPS周    | U32  | 4    | 1         | -     | LSB_first | GPS周数                                                    |
+| 62    | 校验位   | U8   | 1    | -         | -     | -         | 异或校验，包含0~61的数据                                   |
+
+**轮询数据表**
+
+| 偏移 | 名称         | 单位 | Data1          | Data2          | Data3         | 计算公式及备注                 |
+| ---- | ------------ | ---- | -------------- | -------------- | ------------- | ------------------------------ |
+| 0    | 定位信息精度 | m    | latstd（纬度） | lonstd（经度） | hstd（海拔）  | 精度=e^(data/100)，e取2.718281 |
+| 1    | 定速信息精度 | m/s  | vnstd（北向）  | vestd（东向）  | vdstd（地向） | 精度=e^(data/100)，e取2.718281 |
+| 2    | 姿态信息精度 | deg  | Rollstd        | Pitchstd       | Yawstd        | 精度=e^(data/100)，e取2.718281 |
+| 22   | 设备内部温度 | ℃    | 温度           | -              | -             | 温度=数据×(200/32768)          |
+| 32   | GNSS状态     | -    | 定位状态       | 收星数         | 定向状态      | 位置状态参考附录一             |
+| 33   | 轮速状态     | -    | -              | 轮速有无标志   | -             | Data2≠0=有轮速，连续=0=无轮速  |
+| 49   | IMU状态      | -    | 故障标志位     | -              | -             | Data1=1=故障，0=正常           |
+
+#### 

src/perception/rtk_asensing/CMakeLists.txt

@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 3.5)
+project(rtk_asensing)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sweeper_interfaces REQUIRED)
+find_package(logger REQUIRED)
+
+include_directories(include/rtk_asensing)
+
+add_executable(rtk_asensing_node src/rtk_asensing_node.cpp src/serial_read.cpp)
+target_link_libraries(rtk_asensing_node ${Boost_LIBRARIES})
+ament_target_dependencies(
+  rtk_asensing_node
+  rclcpp
+  std_msgs
+  sweeper_interfaces
+  logger)
+
+install(TARGETS rtk_asensing_node DESTINATION lib/${PROJECT_NAME})
+
+# 安装配置文件和启动文件
+install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/perception/rtk_asensing/config/rtk_asensing_params.yaml

@@ -0,0 +1,6 @@
+rtk_asensing:
+  ros__parameters:
+    # 串口设备名称
+    serial_port: "/dev/ttyTHS1"
+    # 波特率
+    baud_rate: 460800

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

@@ -0,0 +1,301 @@
+#ifndef __ASENSING_PROTOCOL_H__
+#define __ASENSING_PROTOCOL_H__
+
+#include <cmath>
+#include <cstdint>
+#include <cstring>
+
+// Asensing协议4数据帧定义
+// 帧头: 0xBD 0xDB
+// 帧类型: 0x0B (组合导航模式，58字节数据)
+
+#pragma pack(push, 1)  // 1字节对齐，避免结构体填充
+
+// Asensing协议4组合导航数据帧
+struct AsensingInsData
+{
+    uint8_t frame_header[2];  // 偏移0-1: 帧头 0xBD 0xDB
+    uint8_t frame_type;       // 偏移2: 帧类型 0x0B
+    int16_t roll;             // 偏移3-4: 横滚角, 系数 360/32768, 单位 deg
+    int16_t pitch;            // 偏移5-6: 俯仰角, 系数 360/32768, 单位 deg
+    int16_t yaw;              // 偏移7-8: 方位角, 系数 360/32768, 单位 deg
+    int16_t gyro_x;           // 偏移9-10: 陀螺x轴, 系数 300/32768, 单位 deg/s
+    int16_t gyro_y;           // 偏移11-12: 陀螺y轴, 系数 300/32768, 单位 deg/s
+    int16_t gyro_z;           // 偏移13-14: 陀螺z轴, 系数 300/32768, 单位 deg/s
+    int16_t accel_x;          // 偏移15-16: 加表x轴, 系数 12/32768, 单位 g
+    int16_t accel_y;          // 偏移17-18: 加表y轴, 系数 12/32768, 单位 g
+    int16_t accel_z;          // 偏移19-20: 加表z轴, 系数 12/32768, 单位 g
+    int32_t latitude;         // 偏移21-24: 纬度, 系数 1e-7, 单位 deg
+    int32_t longitude;        // 偏移25-28: 经度, 系数 1e-7, 单位 deg
+    int32_t altitude;         // 偏移29-32: 高度, 系数 1e-3, 单位 m
+    int16_t vel_north;        // 偏移33-34: 北向速度, 系数 1e2/32768, 单位 m/s
+    int16_t vel_east;         // 偏移35-36: 东向速度, 系数 1e2/32768, 单位 m/s
+    int16_t vel_down;         // 偏移37-38: 地向速度, 系数 1e2/32768, 单位 m/s
+    uint8_t status;           // 偏移39: 状态位
+    uint8_t reserved[6];      // 偏移40-45: 保留字段
+    int16_t data1;            // 偏移46-47: 轮循数据1
+    int16_t data2;            // 偏移48-49: 轮循数据2
+    int16_t data3;            // 偏移50-51: 轮循数据3
+    uint32_t gps_time;        // 偏移52-55: GPS时间（周内秒）, 系数 2.5e-4, 单位 秒
+    uint8_t pollute_type;     // 偏移56: 轮循数据类型
+    uint8_t check1;           // 偏移57: 校验位1 (异或校验，包含0-56的数据)
+    uint32_t gps_week;        // 偏移58-61: GPS周数
+    uint8_t check2;           // 偏移62: 校验位2 (异或校验，包含0-61的数据)
+};
+
+#pragma pack(pop)
+
+// Asensing协议数据解析类
+class AsensingProtocol
+{
+   public:
+    AsensingProtocol() : is_synced_(false), frame_index_(0) {}
+
+    // 重置解析状态
+    void reset()
+    {
+        is_synced_ = false;
+        frame_index_ = 0;
+        memset(&raw_data_, 0, sizeof(raw_data_));
+    }
+
+    // 尝试从串口数据中解析出一帧完整的数据
+    // 返回值: 1=成功解析一帧, 0=数据不足, -1=校验失败
+    int feedData(const uint8_t* data, int len)
+    {
+        for (int i = 0; i < len; i++)
+        {
+            if (!is_synced_)
+            {
+                // 寻找帧头 0xBD 0xDB
+                if (frame_index_ == 0 && data[i] == 0xBD)
+                {
+                    frame_buf_[frame_index_++] = data[i];
+                }
+                else if (frame_index_ == 1 && data[i] == 0xDB)
+                {
+                    frame_buf_[frame_index_++] = data[i];
+                }
+                else if (frame_index_ == 1 && data[i] != 0xDB)
+                {
+                    frame_index_ = 0;
+                    if (data[i] == 0xBD)
+                    {
+                        frame_buf_[frame_index_++] = data[i];
+                    }
+                }
+                else
+                {
+                    frame_index_ = 0;
+                }
+
+                // 帧头匹配成功，继续接收后续数据
+                if (frame_index_ == 2)
+                {
+                    is_synced_ = true;
+                }
+            }
+            else
+            {
+                // 已同步帧头，继续接收数据
+                frame_buf_[frame_index_++] = data[i];
+
+                // 检查是否接收完整帧 (帧头2字节 + 类型1字节 + 数据58字节 + GPS周4字节 + 校验1字节 = 66字节)
+                // 实际数据到偏移57是check1(1字节)，然后58-61是GPS周，62是check2
+                // 所以完整帧长 = 63字节
+                if (frame_index_ >= 63)
+                {
+                    // 检查帧类型
+                    if (frame_buf_[2] != 0x0B)
+                    {
+                        // 帧类型不匹配，重新同步
+                        is_synced_ = false;
+                        frame_index_ = 0;
+                        continue;
+                    }
+
+                    // 验证校验位1 (异或校验，包含0-56的数据)
+                    uint8_t xor_check1 = 0;
+                    for (int j = 0; j < 57; j++)
+                    {
+                        xor_check1 ^= frame_buf_[j];
+                    }
+
+                    if (xor_check1 != frame_buf_[57])
+                    {
+                        // 校验失败，重新同步
+                        is_synced_ = false;
+                        frame_index_ = 0;
+                        continue;
+                    }
+
+                    // 验证校验位2 (异或校验，包含0-61的数据)
+                    uint8_t xor_check2 = 0;
+                    for (int j = 0; j < 62; j++)
+                    {
+                        xor_check2 ^= frame_buf_[j];
+                    }
+
+                    if (xor_check2 != frame_buf_[62])
+                    {
+                        // 校验失败，重新同步
+                        is_synced_ = false;
+                        frame_index_ = 0;
+                        continue;
+                    }
+
+                    // 校验通过，复制数据到结构体
+                    memcpy(&raw_data_, frame_buf_, sizeof(AsensingInsData));
+
+                    // 重置解析状态
+                    is_synced_ = false;
+                    frame_index_ = 0;
+
+                    return 1;  // 成功解析一帧
+                }
+            }
+        }
+
+        return 0;  // 数据不足
+    }
+
+    // 获取解析后的数据
+    const AsensingInsData& getData() const { return raw_data_; }
+
+    // 获取解析后的物理量（已转换单位）
+
+    // 获取横滚角 (deg)
+    double getRoll() const { return static_cast<double>(raw_data_.roll) * 360.0 / 32768.0; }
+
+    // 获取俯仰角 (deg)
+    double getPitch() const { return static_cast<double>(raw_data_.pitch) * 360.0 / 32768.0; }
+
+    // 获取方位角 (deg)
+    double getYaw() const { return static_cast<double>(raw_data_.yaw) * 360.0 / 32768.0; }
+
+    // 获取陀螺x轴 (deg/s)
+    double getGyroX() const { return static_cast<double>(raw_data_.gyro_x) * 300.0 / 32768.0; }
+
+    // 获取陀螺y轴 (deg/s)
+    double getGyroY() const { return static_cast<double>(raw_data_.gyro_y) * 300.0 / 32768.0; }
+
+    // 获取陀螺z轴 (deg/s)
+    double getGyroZ() const { return static_cast<double>(raw_data_.gyro_z) * 300.0 / 32768.0; }
+
+    // 获取加速度x轴 (g)
+    double getAccelX() const { return static_cast<double>(raw_data_.accel_x) * 12.0 / 32768.0; }
+
+    // 获取加速度y轴 (g)
+    double getAccelY() const { return static_cast<double>(raw_data_.accel_y) * 12.0 / 32768.0; }
+
+    // 获取加速度z轴 (g)
+    double getAccelZ() const { return static_cast<double>(raw_data_.accel_z) * 12.0 / 32768.0; }
+
+    // 获取纬度 (deg)
+    double getLatitude() const { return static_cast<double>(raw_data_.latitude) * 1e-7; }
+
+    // 获取经度 (deg)
+    double getLongitude() const { return static_cast<double>(raw_data_.longitude) * 1e-7; }
+
+    // 获取高度 (m)
+    double getAltitude() const { return static_cast<double>(raw_data_.altitude) * 1e-3; }
+
+    // 获取北向速度 (m/s)
+    double getVelNorth() const { return static_cast<double>(raw_data_.vel_north) * 100.0 / 32768.0; }
+
+    // 获取东向速度 (m/s)
+    double getVelEast() const { return static_cast<double>(raw_data_.vel_east) * 100.0 / 32768.0; }
+
+    // 获取地向速度 (m/s)
+    double getVelDown() const { return static_cast<double>(raw_data_.vel_down) * 100.0 / 32768.0; }
+
+    // 获取地面速度 (m/s)
+    double getGroundSpeed() const
+    {
+        double vn = getVelNorth();
+        double ve = getVelEast();
+        return std::sqrt(vn * vn + ve * ve);
+    }
+
+    // 获取合速度 (m/s)
+    double getTotalSpeed() const
+    {
+        double vn = getVelNorth();
+        double ve = getVelEast();
+        double vd = getVelDown();
+        return std::sqrt(vn * vn + ve * ve + vd * vd);
+    }
+
+    // 获取GPS时间（周内秒）(s)
+    double getGpsTime() const { return static_cast<double>(raw_data_.gps_time) * 2.5e-4; }
+
+    // 获取GPS周数
+    uint32_t getGpsWeek() const { return raw_data_.gps_week; }
+
+    // 获取定位状态
+    // bit0=位置, bit1=速度, bit2=姿态, bit3=航向角; 1=完成初对准
+    uint8_t getStatus() const { return raw_data_.status; }
+
+    // 获取位置定位状态
+    bool isPositionValid() const { return (raw_data_.status & 0x01) != 0; }
+
+    // 获取速度定位状态
+    bool isVelocityValid() const { return (raw_data_.status & 0x02) != 0; }
+
+    // 获取姿态定位状态
+    bool isAttitudeValid() const { return (raw_data_.status & 0x04) != 0; }
+
+    // 获取航向角定位状态
+    bool isHeadingValid() const { return (raw_data_.status & 0x08) != 0; }
+
+    // 获取轮循数据类型
+    uint8_t getPollutionType() const { return raw_data_.pollute_type; }
+
+    // 根据轮循数据类型获取温度 (℃)
+    // 只有当pollution_type == 22时有效
+    double getTemperature() const
+    {
+        if (raw_data_.pollute_type == 22)
+        {
+            return static_cast<double>(raw_data_.data1) * 200.0 / 32768.0;
+        }
+        return 0.0;
+    }
+
+    // 根据轮循数据类型获取GNSS状态信息
+    // pollution_type == 32时: Data1=定位状态, Data2=收星数, Data3=定向状态
+    void getGnssStatus(uint8_t& pos_status, uint8_t& sat_count, uint8_t& heading_status) const
+    {
+        if (raw_data_.pollute_type == 32)
+        {
+            pos_status = static_cast<uint8_t>(raw_data_.data1 & 0xFF);
+            sat_count = static_cast<uint8_t>((raw_data_.data2 >> 8) & 0xFF);
+            heading_status = static_cast<uint8_t>(raw_data_.data3 & 0xFF);
+        }
+        else
+        {
+            pos_status = 0;
+            sat_count = 0;
+            heading_status = 0;
+        }
+    }
+
+    // 获取IMU状态
+    // pollution_type == 49时: Data1=故障标志位 (1=故障, 0=正常)
+    bool isImuNormal() const
+    {
+        if (raw_data_.pollute_type == 49)
+        {
+            return (raw_data_.data1 & 0xFF) == 0;
+        }
+        return true;  // 默认正常
+    }
+
+   private:
+    AsensingInsData raw_data_;  // 解析后的数据
+    uint8_t frame_buf_[63];     // 帧缓冲区
+    bool is_synced_;            // 帧同步状态
+    int frame_index_;           // 当前帧索引
+};
+
+#endif

src/perception/rtk_asensing/include/rtk_asensing/serial_read.hpp

@@ -0,0 +1,23 @@
+#ifndef __UART_READ_H__
+#define __UART_READ_H__
+
+#include <boost/asio.hpp>
+#include <iostream>
+
+using namespace std;
+using namespace boost::asio;
+
+class Boost_serial
+{
+   public:
+    Boost_serial();
+    ~Boost_serial();
+    void init(const string port_name, int baud_rate = 115200);
+    int serial_read(char buf[], int size);
+
+   private:
+    io_service service;
+    serial_port* sp;
+};
+
+#endif

src/perception/rtk_asensing/launch/rtk_asensing.launch.py

@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    # 获取配置文件路径
+    config_dir = os.path.join(
+        get_package_share_directory("rtk_asensing"),
+        "config",
+        "rtk_asensing_params.yaml",
+    )
+
+    return LaunchDescription(
+        [
+            Node(
+                package="rtk_asensing",
+                executable="rtk_asensing_node",
+                name="rtk_asensing_node",
+                parameters=[config_dir],  # 从YAML文件加载参数
+                output="screen",
+            ),
+        ]
+    )

src/perception/rtk_asensing/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rtk_asensing</name>
+  <version>0.0.0</version>
+  <description>Asensing RTK protocol 4 data parser</description>
+  <maintainer email="zxwl@todo.todo">zxwl</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sweeper_interfaces</depend>
+  <depend>logger</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/perception/rtk_asensing/src/rtk_asensing_node.cpp

@@ -0,0 +1,162 @@
+#include <stdlib.h>
+#include <string.h>
+
+#include "asensing_protocol.hpp"
+#include "logger/logger.h"
+#include "rclcpp/rclcpp.hpp"
+#include "serial_read.hpp"
+#include "sweeper_interfaces/msg/rtk.hpp"
+
+class RtkAsensingNode : public rclcpp::Node
+{
+   public:
+    RtkAsensingNode(std::string name) : Node(name)
+    {
+        LOG_INFO("%s节点已经启动.", name.c_str());
+
+        // 初始化串口读取
+        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);
+        this->get_parameter("serial_port", serial_port);
+        this->get_parameter("baud_rate", baud_rate);
+        boost_serial->init(serial_port.c_str(), baud_rate);
+
+        // 初始化协议解析器
+        protocol_parser = new AsensingProtocol();
+
+        // 创建发布者
+        rtk_publisher_ = this->create_publisher<sweeper_interfaces::msg::Rtk>("rtk_message", 10);
+
+        // 创建定时器，100ms为周期，定时发布
+        timer_ =
+            this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&RtkAsensingNode::timer_callback, this));
+    }
+
+    ~RtkAsensingNode()
+    {
+        if (protocol_parser)
+        {
+            delete protocol_parser;
+        }
+        if (boost_serial)
+        {
+            delete boost_serial;
+        }
+    }
+
+   private:
+    void timer_callback()
+    {
+        try
+        {
+            // 读取串口数据
+            memset(serial_buf, 0, sizeof(serial_buf));
+            int num = boost_serial->serial_read(serial_buf, sizeof(serial_buf));
+
+            if (num <= 0)
+            {
+                return;
+            }
+
+            // 将数据送入协议解析器
+            int result = protocol_parser->feedData(reinterpret_cast<uint8_t*>(serial_buf), num);
+
+            if (result == 1)
+            {
+                // 成功解析一帧数据，发布ROS消息
+                publishData();
+            }
+        }
+        catch (const std::exception& e)
+        {
+            LOG_ERROR("定时器回调函数异常：%s", e.what());
+        }
+        catch (...)
+        {
+            LOG_ERROR("定时器回调函数未知异常");
+        }
+    }
+
+    void publishData()
+    {
+        try
+        {
+            auto message = sweeper_interfaces::msg::Rtk();
+
+            // 获取解析后的数据
+            double lat = protocol_parser->getLatitude();
+            double lon = protocol_parser->getLongitude();
+            double heading = protocol_parser->getYaw();
+            double speed = protocol_parser->getGroundSpeed();
+
+            // 获取定位质量
+            // 根据轮循数据类型获取状态
+            uint8_t pos_status = 0;
+            uint8_t sat_count = 0;
+            uint8_t heading_status = 0;
+            protocol_parser->getGnssStatus(pos_status, sat_count, heading_status);
+
+            // 映射到p_quality和h_quality
+            // 参考协议文档: 位置状态参考附录一
+            // 这里简化处理: 0=未定位, 1=单点定位, 4=RTK固定解, 5=RTK浮点解
+            int p_quality = pos_status;
+            int h_quality = heading_status;
+
+            message.lat = lat;
+            message.lon = lon;
+            message.head = heading;
+            message.speed = speed;
+            message.p_quality = p_quality;
+            message.h_quality = h_quality;
+
+            LOG_INFO("Asensing RTK: lat:%.9f, lon:%.9f, heading:%.2f, speed:%.2f, p_quality:%d, h_quality:%d",
+                     message.lat, message.lon, message.head, message.speed, message.p_quality, message.h_quality);
+
+            // 发布消息
+            rtk_publisher_->publish(message);
+        }
+        catch (const std::exception& e)
+        {
+            LOG_ERROR("发布数据时发生异常：%s", e.what());
+        }
+    }
+
+    // ROS定时器
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    // ROS发布者
+    rclcpp::Publisher<sweeper_interfaces::msg::Rtk>::SharedPtr rtk_publisher_;
+
+    // 串口读取类指针
+    Boost_serial* boost_serial;
+
+    // 协议解析器指针
+    AsensingProtocol* protocol_parser;
+
+    // 串口读取buffer
+    char serial_buf[300];
+};
+
+int main(int argc, char** argv)
+{
+    // 初始化日志系统
+    logger::Logger::Init("rtk_asensing", "./nodes_log");
+
+    rclcpp::init(argc, argv);
+
+    // 创建节点
+    auto node = std::make_shared<RtkAsensingNode>("rtk_asensing_node");
+
+    // 运行节点
+    rclcpp::spin(node);
+
+    rclcpp::shutdown();
+
+    // 关闭日志系统
+    logger::Logger::Shutdown();
+
+    return 0;
+}

src/perception/rtk_asensing/src/serial_read.cpp

@@ -0,0 +1,102 @@
+#include "serial_read.hpp"
+
+#include "logger/logger.h"
+
+Boost_serial::Boost_serial() : sp(nullptr) { ; }
+
+Boost_serial::~Boost_serial()
+{
+    if (sp)
+    {
+        try
+        {
+            if (sp->is_open())
+            {
+                sp->close();
+            }
+            delete sp;
+        }
+        catch (...)
+        {
+            LOG_ERROR("关闭串口时发生异常");
+        }
+    }
+}
+
+void Boost_serial::init(const string port_name, int baud_rate)
+{
+    try
+    {
+        sp = new serial_port(service);
+        sp->open(port_name);
+
+        if (sp->is_open())
+        {
+            sp->set_option(serial_port::baud_rate(baud_rate));
+            sp->set_option(serial_port::flow_control(serial_port::flow_control::none));
+            sp->set_option(serial_port::parity(serial_port::parity::none));
+            sp->set_option(serial_port::stop_bits(serial_port::stop_bits::one));
+            sp->set_option(serial_port::character_size(8));
+            LOG_INFO("打开串口成功！");
+        }
+        else
+        {
+            LOG_ERROR("打开串口失败！");
+            delete sp;
+            sp = nullptr;
+        }
+    }
+    catch (const exception& e)
+    {
+        LOG_ERROR("初始化串口时发生异常：%s", e.what());
+        if (sp)
+        {
+            delete sp;
+            sp = nullptr;
+        }
+    }
+    catch (...)
+    {
+        LOG_ERROR("初始化串口时发生未知异常");
+        if (sp)
+        {
+            delete sp;
+            sp = nullptr;
+        }
+    }
+}
+
+int Boost_serial::serial_read(char buf[], int size)
+{
+    if (sp == nullptr || !sp->is_open())
+    {
+        LOG_WARN("串口未初始化或未打开");
+        return -1;
+    }
+
+    try
+    {
+        int num = read(*sp, buffer(buf, size));
+        if (num < 0)
+        {
+            LOG_WARN("串口读取错误");
+            return -1;
+        }
+        else if (num == 0)
+        {
+            LOG_INFO("串口无数据可读");
+            return 0;
+        }
+        return num;
+    }
+    catch (const exception& e)
+    {
+        LOG_ERROR("串口读取异常：%s", e.what());
+        return -1;
+    }
+    catch (...)
+    {
+        LOG_ERROR("串口读取未知异常");
+        return -1;
+    }
+}