From 99ed9a7b84dd368a55445b7d16e52600f27ef08c Mon Sep 17 00:00:00 2001 From: Alvin-lyq Date: Wed, 29 Apr 2026 10:10:08 +0800 Subject: [PATCH] Asensing RTK --- src/perception/rtk_asensing/4 数据输出协议.md | 70 ++++ src/perception/rtk_asensing/CMakeLists.txt | 46 +++ .../config/rtk_asensing_params.yaml | 6 + .../rtk_asensing/asensing_protocol.hpp | 301 ++++++++++++++++++ .../include/rtk_asensing/serial_read.hpp | 23 ++ .../launch/rtk_asensing.launch.py | 25 ++ src/perception/rtk_asensing/package.xml | 23 ++ .../rtk_asensing/src/rtk_asensing_node.cpp | 162 ++++++++++ .../rtk_asensing/src/serial_read.cpp | 102 ++++++ 9 files changed, 758 insertions(+) create mode 100644 src/perception/rtk_asensing/4 数据输出协议.md create mode 100644 src/perception/rtk_asensing/CMakeLists.txt create mode 100644 src/perception/rtk_asensing/config/rtk_asensing_params.yaml create mode 100644 src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp create mode 100644 src/perception/rtk_asensing/include/rtk_asensing/serial_read.hpp create mode 100644 src/perception/rtk_asensing/launch/rtk_asensing.launch.py create mode 100644 src/perception/rtk_asensing/package.xml create mode 100644 src/perception/rtk_asensing/src/rtk_asensing_node.cpp create mode 100644 src/perception/rtk_asensing/src/serial_read.cpp diff --git a/src/perception/rtk_asensing/4 数据输出协议.md b/src/perception/rtk_asensing/4 数据输出协议.md new file mode 100644 index 0000000..4af4e34 --- /dev/null +++ b/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=正常 | + +#### \ No newline at end of file diff --git a/src/perception/rtk_asensing/CMakeLists.txt b/src/perception/rtk_asensing/CMakeLists.txt new file mode 100644 index 0000000..25dfc13 --- /dev/null +++ b/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() diff --git a/src/perception/rtk_asensing/config/rtk_asensing_params.yaml b/src/perception/rtk_asensing/config/rtk_asensing_params.yaml new file mode 100644 index 0000000..4f6afde --- /dev/null +++ b/src/perception/rtk_asensing/config/rtk_asensing_params.yaml @@ -0,0 +1,6 @@ +rtk_asensing: + ros__parameters: + # 串口设备名称 + serial_port: "/dev/ttyTHS1" + # 波特率 + baud_rate: 460800 diff --git a/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp b/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp new file mode 100644 index 0000000..5ccea95 --- /dev/null +++ b/src/perception/rtk_asensing/include/rtk_asensing/asensing_protocol.hpp @@ -0,0 +1,301 @@ +#ifndef __ASENSING_PROTOCOL_H__ +#define __ASENSING_PROTOCOL_H__ + +#include +#include +#include + +// 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(raw_data_.roll) * 360.0 / 32768.0; } + + // 获取俯仰角 (deg) + double getPitch() const { return static_cast(raw_data_.pitch) * 360.0 / 32768.0; } + + // 获取方位角 (deg) + double getYaw() const { return static_cast(raw_data_.yaw) * 360.0 / 32768.0; } + + // 获取陀螺x轴 (deg/s) + double getGyroX() const { return static_cast(raw_data_.gyro_x) * 300.0 / 32768.0; } + + // 获取陀螺y轴 (deg/s) + double getGyroY() const { return static_cast(raw_data_.gyro_y) * 300.0 / 32768.0; } + + // 获取陀螺z轴 (deg/s) + double getGyroZ() const { return static_cast(raw_data_.gyro_z) * 300.0 / 32768.0; } + + // 获取加速度x轴 (g) + double getAccelX() const { return static_cast(raw_data_.accel_x) * 12.0 / 32768.0; } + + // 获取加速度y轴 (g) + double getAccelY() const { return static_cast(raw_data_.accel_y) * 12.0 / 32768.0; } + + // 获取加速度z轴 (g) + double getAccelZ() const { return static_cast(raw_data_.accel_z) * 12.0 / 32768.0; } + + // 获取纬度 (deg) + double getLatitude() const { return static_cast(raw_data_.latitude) * 1e-7; } + + // 获取经度 (deg) + double getLongitude() const { return static_cast(raw_data_.longitude) * 1e-7; } + + // 获取高度 (m) + double getAltitude() const { return static_cast(raw_data_.altitude) * 1e-3; } + + // 获取北向速度 (m/s) + double getVelNorth() const { return static_cast(raw_data_.vel_north) * 100.0 / 32768.0; } + + // 获取东向速度 (m/s) + double getVelEast() const { return static_cast(raw_data_.vel_east) * 100.0 / 32768.0; } + + // 获取地向速度 (m/s) + double getVelDown() const { return static_cast(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(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(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(raw_data_.data1 & 0xFF); + sat_count = static_cast((raw_data_.data2 >> 8) & 0xFF); + heading_status = static_cast(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 diff --git a/src/perception/rtk_asensing/include/rtk_asensing/serial_read.hpp b/src/perception/rtk_asensing/include/rtk_asensing/serial_read.hpp new file mode 100644 index 0000000..7e0701b --- /dev/null +++ b/src/perception/rtk_asensing/include/rtk_asensing/serial_read.hpp @@ -0,0 +1,23 @@ +#ifndef __UART_READ_H__ +#define __UART_READ_H__ + +#include +#include + +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 diff --git a/src/perception/rtk_asensing/launch/rtk_asensing.launch.py b/src/perception/rtk_asensing/launch/rtk_asensing.launch.py new file mode 100644 index 0000000..3d97cd3 --- /dev/null +++ b/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", + ), + ] + ) diff --git a/src/perception/rtk_asensing/package.xml b/src/perception/rtk_asensing/package.xml new file mode 100644 index 0000000..c254fbd --- /dev/null +++ b/src/perception/rtk_asensing/package.xml @@ -0,0 +1,23 @@ + + + + rtk_asensing + 0.0.0 + Asensing RTK protocol 4 data parser + zxwl + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + sweeper_interfaces + logger + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/src/perception/rtk_asensing/src/rtk_asensing_node.cpp b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp new file mode 100644 index 0000000..295ac55 --- /dev/null +++ b/src/perception/rtk_asensing/src/rtk_asensing_node.cpp @@ -0,0 +1,162 @@ +#include +#include + +#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("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(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::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("rtk_asensing_node"); + + // 运行节点 + rclcpp::spin(node); + + rclcpp::shutdown(); + + // 关闭日志系统 + logger::Logger::Shutdown(); + + return 0; +} diff --git a/src/perception/rtk_asensing/src/serial_read.cpp b/src/perception/rtk_asensing/src/serial_read.cpp new file mode 100644 index 0000000..79528b5 --- /dev/null +++ b/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; + } +}