From ea8fa41bd3636237b6e85a5eb672bdbe4691abec Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Wed, 4 Mar 2026 17:50:55 +0800 Subject: [PATCH] driver ok --- src/rm_msg | 1 + src/rm_serial_driver/CMakeLists.txt | 57 +++ src/rm_serial_driver/LICENSE | 21 + src/rm_serial_driver/README.md | 161 ++++++ .../config/serial_config.yaml | 10 + src/rm_serial_driver/docs/PROTOCOL_GUIDE.md | 443 ++++++++++++++++ src/rm_serial_driver/docs/USER_GUIDE.md | 402 +++++++++++++++ .../examples/mcu_example_new_protocol.ino | 337 +++++++++++++ .../include/rm_serial_driver/crc.hpp | 27 + .../include/rm_serial_driver/protocol.hpp | 69 +++ .../rm_serial_driver/rm_serial_driver.hpp | 76 +++ .../include/rm_serial_driver/新协议.txt | 39 ++ .../launch/rm_serial_driver.launch.py | 28 ++ src/rm_serial_driver/package.xml | 18 + src/rm_serial_driver/src/crc.cpp | 78 +++ src/rm_serial_driver/src/rm_serial_driver.cpp | 476 ++++++++++++++++++ src/rm_serial_driver/tools/debug_serial.py | 161 ++++++ 17 files changed, 2404 insertions(+) create mode 160000 src/rm_msg create mode 100644 src/rm_serial_driver/CMakeLists.txt create mode 100644 src/rm_serial_driver/LICENSE create mode 100644 src/rm_serial_driver/README.md create mode 100644 src/rm_serial_driver/config/serial_config.yaml create mode 100644 src/rm_serial_driver/docs/PROTOCOL_GUIDE.md create mode 100644 src/rm_serial_driver/docs/USER_GUIDE.md create mode 100644 src/rm_serial_driver/examples/mcu_example_new_protocol.ino create mode 100644 src/rm_serial_driver/include/rm_serial_driver/crc.hpp create mode 100644 src/rm_serial_driver/include/rm_serial_driver/protocol.hpp create mode 100644 src/rm_serial_driver/include/rm_serial_driver/rm_serial_driver.hpp create mode 100644 src/rm_serial_driver/include/rm_serial_driver/新协议.txt create mode 100755 src/rm_serial_driver/launch/rm_serial_driver.launch.py create mode 100644 src/rm_serial_driver/package.xml create mode 100644 src/rm_serial_driver/src/crc.cpp create mode 100644 src/rm_serial_driver/src/rm_serial_driver.cpp create mode 100755 src/rm_serial_driver/tools/debug_serial.py diff --git a/src/rm_msg b/src/rm_msg new file mode 160000 index 0000000..40b1fcf --- /dev/null +++ b/src/rm_msg @@ -0,0 +1 @@ +Subproject commit 40b1fcf31d4b967bd1e6d94fe2b10e7ebb543ae9 diff --git a/src/rm_serial_driver/CMakeLists.txt b/src/rm_serial_driver/CMakeLists.txt new file mode 100644 index 0000000..63629e4 --- /dev/null +++ b/src/rm_serial_driver/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.5) +project(rm_serial_driver) + +# 编译选项 +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# 查找依赖 +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(serial REQUIRED) +find_package(rm_msgs REQUIRED) + +# 构建共享库（用于组件） +add_library(rm_serial_driver_component SHARED + src/rm_serial_driver.cpp + src/crc.cpp +) + +target_include_directories(rm_serial_driver_component PUBLIC + $ + $ +) + +ament_target_dependencies(rm_serial_driver_component + rclcpp + rclcpp_components + std_msgs + serial + rm_msgs +) + +# 注册为组件并生成可执行文件 +rclcpp_components_register_node(rm_serial_driver_component + PLUGIN "rm_serial_driver::RMSerialDriver" + EXECUTABLE rm_serial_driver +) + +# 安装 +install(TARGETS rm_serial_driver_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY launch config docs + DESTINATION share/${PROJECT_NAME}/ +) + +install(DIRECTORY include/ + DESTINATION include +) + +ament_package() diff --git a/src/rm_serial_driver/LICENSE b/src/rm_serial_driver/LICENSE new file mode 100644 index 0000000..cb386d0 --- /dev/null +++ b/src/rm_serial_driver/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2025 zucheng Lv + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/src/rm_serial_driver/README.md b/src/rm_serial_driver/README.md new file mode 100644 index 0000000..936534e --- /dev/null +++ b/src/rm_serial_driver/README.md @@ -0,0 +1,161 @@ +# RM Serial Driver + +ROS 2 串口驱动程序，用于与单片机进行可靠的串口通信。 + +## 特性 + +- ✅ 自动重连机制，断线后持续尝试重连 +- ✅ CRC16 校验保证数据完整性 +- ✅ 完善的异常处理，稳定可靠 +- ✅ 实时统计监控（接收/发送/错误计数） +- ✅ 易于修改通信协议 + +## 快速开始 + +### 1. 安装依赖 + +```bash +# 安装 serial 库 +git clone https://github.com/ZhaoXiangBox/serial.git +cd serial +mkdir build && cd build +cmake .. +make +sudo make install +``` + +### 2. 编译 + +```bash +cd ~/MOVE_AI +colcon build --packages-select rm_serial_driver +source install/setup.bash +``` + +### 3. 配置串口 + +编辑 `config/serial_config.yaml`： + +```yaml +/rm_serial_driver: + ros__parameters: + device_name: /dev/ttyACM0 # 修改为你的串口设备 + baud_rate: 115200 # 修改为你的波特率 +``` + +### 4. 启动驱动 + +```bash +# 使用配置文件启动 +ros2 launch rm_serial_driver rm_serial_driver.launch.py + +# 或直接运行节点 +ros2 run rm_serial_driver rm_serial_driver --ros-args --params-file config/serial_config.yaml +``` + +### 5. 查看数据 + +```bash +# 查看所有话题 +ros2 topic list + +# 查看 MCU 数据 +ros2 topic echo /serial/data_mcu + +# 查看裁判系统数据 +ros2 topic echo /serial/data_ref + +# 发送 AI 控制命令 +ros2 topic pub /serial/data_ai rm_msgs/msg/DataAI "{...}" +``` + +## 配置参数 + +| 参数 | 类型 | 默认值 | 说明 | +|------|------|--------|------| +| `device_name` | string | `/dev/ttyACM0` | 串口设备路径 | +| `baud_rate` | int | `115200` | 波特率 | +| `reconnect_interval_ms` | int | `1000` | 重连间隔（毫秒） | +| `enable_statistics` | bool | `true` | 启用统计信息 | +| `topic_prefix` | string | `serial` | ROS 话题前缀 | + +## 话题接口 + +### 订阅 + +- `/serial/data_ai` (`rm_msgs/msg/DataAI`) - AI 控制命令 + +### 发布 + +- `/serial/data_mcu` (`rm_msgs/msg/DataMCU`) - 单片机姿态数据 +- `/serial/data_ref` (`rm_msgs/msg/DataRef`) - 裁判系统数据 + +## 协议格式 + +所有数据包格式： + +``` ++----+----------+-------+ +| ID | Data | CRC | ++----+----------+-------+ +| 1B | N | 2B | +``` + +- **ID**: 数据包类型 (0xC4=MCU, 0xA8=裁判系统) +- **Data**: 实际数据内容 +- **CRC**: CRC16-CCITT 校验码 + +详细协议说明请查看 [协议修改指南](docs/PROTOCOL_GUIDE.md)。 + +## 修改协议 + +只需 3 步： + +1. 修改 `include/rm_serial_driver/protocol.hpp` - 定义数据结构 +2. 修改 `src/rm_serial_driver.cpp` - 添加处理逻辑 +3. 重新编译 - `colcon build` + +详细步骤请查看 [docs/PROTOCOL_GUIDE.md](docs/PROTOCOL_GUIDE.md)。 + +## 故障排查 + +### 串口无法打开 + +```bash +# 检查串口设备 +ls -l /dev/ttyUSB* /dev/ttyACM* + +# 添加用户到 dialout 组 +sudo usermod -aG dialout $USER +# 注销后重新登录 + +# 临时修改权限 +sudo chmod 666 /dev/ttyACM0 +``` + +### 查看详细日志 + +```bash +ros2 run rm_serial_driver rm_serial_driver --ros-args --log-level debug +``` + +### 测试串口 + +```bash +# 使用 minicom +sudo apt install minicom +minicom -D /dev/ttyACM0 -b 115200 +``` + +## 文档 + +- [用户使用指南](docs/USER_GUIDE.md) - 详细使用说明 +- [协议修改指南](docs/PROTOCOL_GUIDE.md) - 如何修改通信协议 + +## 许可证 + +Apache License 2.0 + +## 贡献 + +欢迎提交 Issue 和 Pull Request！ diff --git a/src/rm_serial_driver/config/serial_config.yaml b/src/rm_serial_driver/config/serial_config.yaml new file mode 100644 index 0000000..76dcd4e --- /dev/null +++ b/src/rm_serial_driver/config/serial_config.yaml @@ -0,0 +1,10 @@ +/rm_serial_driver: + ros__parameters: + # 串口设备配置 + device_name: /dev/ttyUSB0 # 串口设备路径 + baud_rate: 115200 # 波特率 + + # 可选参数（使用默认值） + # reconnect_interval_ms: 1000 # 重连间隔（毫秒） + # enable_statistics: true # 启用统计信息 + # topic_prefix: serial # 话题前缀 \ No newline at end of file diff --git a/src/rm_serial_driver/docs/PROTOCOL_GUIDE.md b/src/rm_serial_driver/docs/PROTOCOL_GUIDE.md new file mode 100644 index 0000000..e4aff5c --- /dev/null +++ b/src/rm_serial_driver/docs/PROTOCOL_GUIDE.md @@ -0,0 +1,443 @@ +# RM Serial Driver - 协议修改指南 + +本文档详细说明如何修改通信协议以适应你的需求。 + +## 快速修改协议 + +### 1. 修改数据包 ID + +在 `include/rm_serial_driver/protocol.hpp` 中修改： + +```cpp +// 修改数据包标识符 +#define ID_MCU (0xC4) // 单片机数据包 ID +#define ID_REF (0xA8) // 裁判系统数据包 ID + +// 如果需要添加新的数据包类型，添加新的 ID +#define ID_VISION (0xB5) // 视觉数据包 ID（示例） +``` + +### 2. 修改 MCU 数据结构 + +修改从单片机接收的数据格式： + +```cpp +typedef struct __attribute__((packed)) +{ + struct __attribute__((packed)) + { + float q0; + float q1; + float q2; + float q3; + } quat; /* 四元数 */ + + struct __attribute__((packed)) + { + float yaw; + float pit; + float rol; + } gimbal; /* 欧拉角 */ + + uint8_t notice; /* 控制命令 */ + + // 添加新字段示例： + // float t +} DataMCU_t;emperature; // 温度 + // uint16_t voltage; // 电压 +``` + +**注意**：修改后需要同步修改 `rm_msgs` 包中的消息定义！ + +### 3. 修改 AI 控制数据结构 + +修改发送给单片机的控制命令： + +```cpp +typedef struct __attribute__((packed)) +{ + struct __attribute__((packed)) + { + float yaw; + float pit; + float rol; + } delta_eulr; /* 欧拉角增量 */ + + struct __attribute__((packed)) + { + float vx; + float vy; + float wz; + } delta_pos; /* 位置增量 */ + + uint8_t notice; /* 控制命令 */ + + // 添加新字段示例： + // uint8_t shoot_mode; // 射击模式 + // uint16_t shoot_speed; // 射击速度 +} DataAI_t; +``` + +### 4. 添加新的数据包类型 + +#### 步骤 1：在 `protocol.hpp` 中定义新数据结构 + +```cpp +// 定义新的数据包 ID +#define ID_VISION (0xB5) + +// 定义数据结构 +typedef struct __attribute__((packed)) +{ + float target_x; + float target_y; + float target_z; + uint8_t target_type; +} DataVision_t; + +// 定义完整数据包（带 ID 和 CRC） +typedef struct __attribute__((packed)) +{ + uint8_t id; + DataVision_t data; + uint16_t crc16; +} PackageVision_t; +``` + +#### 步骤 2：在 `rm_serial_driver.cpp` 中添加处理逻辑 + +在 `get_expected_length()` 函数中添加： + +```cpp +size_t RMSerialDriver::get_expected_length(uint8_t id) +{ + switch (id) + { + case ID_MCU: + return sizeof(PackageMCU_t); + case ID_REF: + return sizeof(PackageReferee_t); + case ID_VISION: // 新增 + return sizeof(PackageVision_t); + default: + return 0; + } +} +``` + +在 `process_packet()` 函数中添加： + +```cpp +void RMSerialDriver::process_packet(uint8_t id, const char *data, size_t packet_size) +{ + // ... 现有代码 ... + + else if (id == ID_VISION) // 新增 + { + PackageVision_t vision; + std::memcpy(&vision, data, packet_size); + + if (crc16::CRC16_Verify(reinterpret_cast(&vision) + 1, packet_size - 1)) + { + rm_msgs::msg::DataVision msg; // 需要在 rm_msgs 中定义 + std::memcpy(&msg, &vision.data, sizeof(vision.data)); + data_vision_pub_->publish(msg); + + rx_count_++; + last_receive_time_ = std::chrono::steady_clock::now(); + } + else + { + crc_error_count_++; + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, + "Vision 数据 CRC 校验失败"); + } + } +} +``` + +#### 步骤 3：在头文件中添加发布者 + +在 `rm_serial_driver.hpp` 中添加： + +```cpp +private: + rclcpp::Publisher::SharedPtr data_vision_pub_; +``` + +#### 步骤 4：在构造函数中创建发布者 + +```cpp +data_vision_pub_ = this->create_publisher( + topic_prefix + "/data_vision", 10); +``` + +## 完整示例：添加电机状态数据包 + +### 1. 定义协议（protocol.hpp） + +```cpp +#define ID_MOTOR (0xD3) + +typedef struct __attribute__((packed)) +{ + int16_t motor1_spee16_t motor2_speed; + int16_t motor3_speed; + int16_t motor4_speed; + uint16_t motor1_current; + uint16_t motor2_current; + uint16_t motor3_current; + uint16_t motor4_current; + uint8_t motor_status; // 位标志：bit0=motor1, bit1=motor2... +} DataMotor_t; + +typedef struct __attribute__((packed)) +{ + uint8_t id; + DataMotor_t data; + uint16_t crc16; +} PackageMotor_t; +``` + +### 2. 创建 ROS 消息（rm_msgs/msg/DataMotor.msg） + +``` +int16 motor1_speed +int16 motor2_speed +int16 motor3_speed +int16 motor4_speed +uint16 motor1_current +uint16 motor2_current +uint16 motor3_current +uint16 motor4_current +uint8 motor_status +``` + +### 3. 修改驱动代码 + +在 `rm_serial_driver.hpp` 中： + +```cpp +#include "rm_msgs/msg/data_motor.hpp" + +private: + rclcpp::Publisher::SharedPtr data_motor_pub_; +``` + +在 `rm_serial_driver.cpp` 构造函数中： + +```cpp +data_motor_pub_ = this->create_publisher( + topic_prefix + "/data_motor", 10); +``` + +在 `get_expected_length()` 中： + +```cpp +case ID_MOTOR: + return sizeof(PackageMotor_t); +``` + +在 `process_packet()` 中： + +```cpp +else if (id == ID_MOTOR) +{ + PackageMotor_t motor; + std::memcpy(&motor, data, packet_size); + + if (crc16::CRC16_Verify(reinterpret_cast(&motor) + 1, packet_size - 1)) + { + rm_msgs::msg::DataMotor msg; + msg.motor1_speed = motor.data.motor1_speed; + msg.motor2_speed = motor.data.motor2_speed; + msg.motor3_speed = motor.data.motor3_speed; + msg.motor4_speed = motor.data.motor4_speed; + msg.motor1_current = motor.data.motor1_current; + msg.motor2_current = motor.data.motor2_current; + msg.motor3_current = motor.data.motor3_current; + msg.motor4_current = motor.data.motor4_current; + msg.motor_status = motor.data.motor_status; + + data_motor_pub_->publish(msg); + rx_count_++; + last_receive_time_ = std::chrono::steady_clock::now(); + + RCLCPP_DEBUG(get_logger(), "接收电机数据: M1=%d, M2=%d, M3=%d, M4=%d", + motor.data.motor1_speed, motor.data.motor2_speed, + motor.data.motor3_speed, motor.data.motor4_speed); + } + else + { + crc_error_count_++; + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, + "电机数据 CRC 校验失败"); + } +} +``` + +### 4. 单片机端代码（Arduino/STM32） + +```cpp +// 发送电机数据 +void send_motor_data() +{ + PackageMotor_t package; + package.id = ID_MOTOR; + + // 填充数据 + package.data.motor1_speed = get_motor_speed(1); + package.data.motor2_speed = get_motor_speed(2); + package.data.motor3_speed = get_motor_speed(3); + package.data.motor4_speed = get_motor_speed(4); + + package.data.motor1_current = get_motor_current(1); + package.data.motor2_current = get_motor_current(2); + package.data.motor3_current = get_motor_current(3); + package.data.motor4_current = get_motor_current(4); + + package.data.motor_status = get_motor_status(); + + // 计算 CRC + package.crc16 = CRC16_Calc( + (uint8_t*)&package.data, + sizeof(DataMotor_t), + CRC16_INIT + ); + + // 发送 + Serial.write((uint8_t*)&package, sizeof(PackageMotor_t)); +} +``` + +## 注意事项 + +### 1. 数据对齐 + +使用 `__attribute__((packed))` 确保结构体紧凑排列，避免编译器自动填充： + +```cpp +typedef struct __attribute__((packed)) +{ + uint8_t a; // 1 字节 + uint16_t b; // 2 字节，紧跟在 a 后面 + uint32_t c; // 4 字节，紧跟在 b 后面 +} MyData_t; +``` + +### 2. 字节序 + +当前实现假设上位机和下位机使用相同的字节序（通常都是小端序）。如果需要跨平台，考虑显式转换： + +```cpp +// 发送前转换为小端序 +uint16_t value = 1234; +uint8_t bytes[2]; +bytes[0] = value & 0xFF; +bytes[1] = (value >> 8) & 0xFF; + +// 接收后从小端序转换 +uint16_t received = bytes[0] | (bytes[1] << 8); +``` + +### 3. CRC 校验范围 + +当前 CRC 校验范围是从数据部分开始（不包括 ID），到数据结束（不包括 CRC 本身）： + +``` ++----+----------+-------+ +| ID | Data | CRC | ++----+----------+-------+ + ^--------^ + CRC 校验范围 +``` + +### 4. 编译步骤 + +修改协议后需要重新编译： + +```bash +cd ~/MOVE_AI + +# 先编译消息包 +colcon build --packages-select rm_msgs + +# 再编译驱动 +colcon build --packages-select rm_serial_driver + +# 加载环境 +source install/setup.bash +``` + +## 调试技巧 + +### 1. 启用调试日志 + +```bash +ros2 run rm_serial_driver rm_serial_driver --ros-args --log-level debug +``` + +### 2. 查看原始数据 + +在 `receive_data()` 中添加： + +```cpp +// 打印接收到的原始数据 +RCLCPP_INFO(get_logger(), "收到 %zu 字节: %s", + buffer.size(), + bytes_to_hex(buffer).c_str()); +``` + +辅助函数： + +```cpp +std::string bytes_to_hex(const std::string& data) +{ + std::stringstream ss; + ss << std::hex << std::setfill('0'); + for (unsigned char c : data) { + ss << std::setw(2) << static_cast(c) << " "; + } + return ss.str(); +} +``` + +### 3. 验证数据包大小 + +```cpp +RCLCPP_INFO(get_logger(), "数据包大小:"); +RCLCPP_INFO(get_logger(), " PackageMCU_t: %zu", sizeof(PackageMCU_t)); +RCLCPP_INFO(get_logger(), " PackageReferee_t: %zu", sizeof(PackageReferee_t)); +RCLCPP_INFO(get_logger(), " PackageAI_t: %zu", sizeof(PackageAI_t)); +``` + +## 常见问题 + +### Q: 修改协议后无法接收数据？ + +A: 检查以下几点： +1. 数据包大小是否匹配（上位机和下位机） +2. CRC 计算是否一致 +3. 数据包 ID 是否正确 +4. 结构体是否使用了 `__attribute__((packed))` + +### Q: CRC 校验总是失败？ + +A: 确认： +1. CRC 计算范围是否一致 +2. CRC 初始值是否相同（0xFFFF） +3. 字节序是否一致 + +### Q: 如何临时禁用 CRC 校验？ + +A: 在 `process_packet()` 中注释掉 CRC 检查： + +```cpp +// if (crc16::CRC16_Verify(...)) +if (true) // 临时禁用 CRC +{ + // 处理数据 +} +``` + +**注意**：仅用于调试，生产环境必须启用 CRC！ diff --git a/src/rm_serial_driver/docs/USER_GUIDE.md b/src/rm_serial_driver/docs/USER_GUIDE.md new file mode 100644 index 0000000..9c192bd --- /dev/null +++ b/src/rm_serial_driver/docs/USER_GUIDE.md @@ -0,0 +1,402 @@ +# RM Serial Driver 使用文档 + +## 功能特性 + +✅ **稳定可靠的串口通信** +- 自动重连机制，断线后持续尝试重连 +- 完善的异常处理，不会因串口异常而崩溃 +- CRC16 校验保证数据完整性 + +✅ **灵活的协议设计** +- 支持多种数据包类型（MCU、裁判系统、AI） +- 易于扩展新的数据包类型 +- 详细的协议修改指南 + +✅ **实时统计监控** +- 接收/发送数据包计数 +- CRC 错误统计 +- 连接状态监控 + +✅ **高性能** +- 独立接收线程，不阻塞主程序 +- 支持多实例，可同时管理多个串口 + +## 快速开始 + +### 1. 安装依赖 + +```bash +# 安装 serial 库 +git clone https://github.com/ZhaoXiangBox/serial.git +cd serial +mkdir build && cd build +cmake .. +make +sudo make install +``` + +### 2. 编译 + +```bash +cd ~/MOVE_AI +colcon build --packages-select rm_serial_driver +source install/setup.bash +``` + +### 3. 配置参数 + +编辑 `config/serial_config.yaml`： + +```yaml +/rm_serial_driver: + ros__parameters: + device_name: /dev/ttyACM0 # 串口设备 + baud_rate: 115200 # 波特率 + reconnect_interval_ms: 1000 # 重连间隔（毫秒） + enable_statistics: true # 启用统计信息 + topic_prefix: serial # 话题前缀 +``` + +### 4. 启动驱动 + +#### 单个串口 + +```bash +ros2 run rm_serial_driver rm_serial_driver --ros-args \ + -p device_name:=/dev/ttyACM0 \ + -p baud_rate:=115200 \ + -p topic_prefix:=serial +``` + +#### 多个串口（使用 launch 文件） + +```bash +ros2 launch rm_serial_driver rm_serial_driver.launch.py +``` + +### 5. 查看话题 + +```bash +# 列出所有话题 +ros2 topic list + +# 查看 MCU 数据 +ros2 topic echo /serial/data_mcu + +# 查看裁判系统数据 +ros2 topic echo /serial/data_ref + +# 发送 AI 控制命令 +ros2 topic pub /serial/data_ai rm_msgs/msg/DataAI "{...}" +``` + +## 话题接口 + +### 订阅的话题 + +| 话题名 | 消息类型 | 说明 | +|--------|---------|------| +| `{prefix}/data_ai` | `rm_msgs/msg/DataAI` | AI 控制命令，发送给单片机 | + +### 发布的话题 + +| 话题名 | 消息类型 | 说明 | +|--------|---------|------| +| `{prefix}/data_mcu` | `rm_msgs/msg/DataMCU` | 单片机姿态数据（四元数、欧拉角） | +| `{prefix}/data_ref` | `rm_msgs/msg/DataRef` | 裁判系统数据（血量、比赛进度） | + +## 参数说明 + +| 参数名 | 类型 | 默认值 | 说明 | +|--------|------|--------|------| +| `device_name` | string | `/dev/ttyACM0` | 串口设备路径 | +| `baud_rate` | int | `115200` | 波特率 | +| `reconnect_interval_ms` | int | `1000` | 重连间隔（毫秒） | +| `enable_statistics` | bool | `true` | 是否启用统计信息 | +| `topic_prefix` | string | `serial` | ROS 话题前缀 | + +## 协议格式 + +### 数据包结构 + +所有数据包都遵循以下格式： + +``` ++----+----------+-------+ +| ID | Data | CRC | ++----+----------+-------+ +| 1B | N | 2B | +``` + +- **ID**: 1 字节，数据包类型标识 +- **Data**: N 字节，实际数据内容 +- **CRC**: 2 字节，CRC16-CCITT 校验码 + +### 数据包类型 + +#### 1. MCU 数据包 (ID: 0xC4) + +从单片机接收的姿态数据： + +```cpp +struct DataMCU { + struct { + float q0, q1, q2, q3; // 四元数 + } quat; + struct { + float yaw, pit, rol; // 欧拉角（度） + } gimbal; + uint8_t notice; // 控制命令 +}; +``` + +#### 2. 裁判系统数据包 (ID: 0xA8) + +从裁判系统接收的比赛数据： + +```cpp +struct DataReferee { + uint16_t remain_hp; // 剩余血量 + uint8_t game_progress : 4; // 比赛进度 + uint16_t stage_remain_time; // 剩余时间（秒） +}; +``` + +#### 3. AI 控制数据包 + +发送给单片机的控制命令： + +```cpp +struct DataAI { + struct { + float yaw, pit, rol; // 欧拉角增量 + } delta_eulr; + struct { + float vx, vy, wz; // 位置增量 + } delta_pos; + uint8_t notice; // 控制命令 +}; +``` + +## 使用示例 + +### Python 示例 + +#### 接收 MCU 数据 + +```python +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from rm_msgs.msg import DataMCU + +class MCUListener(Node): + def __init__(self): + super().__init__('mcu_listener') + self.subscription = self.create_subscription( + DataMCU, + 'serial/data_mcu', + self.mcu_callback, + 10) + + def mcu_callback(self, msg): + self.get_logger().info( + f'姿态: yaw={msg.gimbal.yaw:.2f}, ' + f'pitch={msg.gimbal.pit:.2f}, ' + f'roll={msg.gimbal.rol:.2f}') + +def main(): + rclpy.init() + node = MCUListener() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() +``` + +#### 发送 AI 控制命令 + +```python +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from rm_msgs.msg import DataAI + +class AIController(Node): + def __init__(self): + super().__init__('ai_controller') + self.publisher = self.create_publisher(DataAI, 'serial/data_ai', 10) + self.timer = self.create_timer(0.1, self.send_command) + + def send_command(self): + msg = DataAI() + msg.delta_eulr.yaw = 0.1 + msg.delta_eulr.pit = 0.0 + msg.delta_eulr.rol = 0.0 + msg.delta_pos.vx = 1.0 + msg.delta_pos.vy = 0.0 + msg.delta_pos.wz = 0.0 + msg.notice = 1 + self.publisher.publish(msg) + +def main(): + rclpy.init() + node = AIController() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() +``` + +### C++ 示例 + +```cpp +#include +#include "rm_msgs/msg/data_mcu.hpp" +#include "rm_msgs/msg/data_ai.hpp" + +class RMController : public rclcpp::Node +{ +public: + RMController() : Node("rm_controller") + { + // 订阅 MCU 数据 + mcu_sub_ = create_subscription( + "serial/data_mcu", 10, + std::bind(&RMController::mcu_callback, this, std::placeholders::_1)); + + // 发布 AI 控制命令 + ai_pub_ = create_publisher("serial/data_ai", 10); + + // 定时发送控制命令 + timer_ = create_wall_timer( + std::chrono::milliseconds(100), + std::bind(&RMController::send_command, this)); + } + +private: + void mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg) + { + RCLCPP_INFO(get_logger(), "姿态: yaw=%.2f, pitch=%.2f, roll=%.2f", + msg->gimbal.yaw, msg->gimbal.pit, msg->gimbal.rol); + } + + void send_command() + { + auto msg = rm_msgs::msg::DataAI(); + msg.delta_eulr.yaw = 0.1; + msg.delta_pos.vx = 1.0; + msg.notice = 1; + ai_pub_->publish(msg); + } + + rclcpp::Subscription::SharedPtr mcu_sub_; + rclcpp::Publisher::SharedPtr ai_pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} +``` + +## 故障排查 + +### 串口无法打开 + +```bash +# 检查串口设备 +ls -l /dev/ttyUSB* /dev/ttyACM* + +# 添加用户到 dialout 组 +sudo usermod -aG dialout $USER +# 注销后重新登录 + +# 检查串口权限 +sudo chmod 666 /dev/ttyACM0 +``` + +### 查看详细日志 + +```bash +ros2 run rm_serial_driver rm_serial_driver --ros-args --log-level debug +``` + +### 测试串口通信 + +```bash +# 使用 minicom +sudo apt install minicom +minicom -D /dev/ttyACM0 -b 115200 + +# 或使用 screen +screen /dev/ttyACM0 115200 +``` + +### 常见错误 + +#### 1. "串口打开失败" + +- 检查设备路径是否正确 +- 检查串口权限 +- 确认设备已连接 + +#### 2. "CRC 校验失败" + +- 确认单片机和上位机使用相同的 CRC 算法 +- 检查数据包结构是否匹配 +- 验证字节序是否一致 + +#### 3. "串口意外关闭" + +- 检查 USB 线缆连接 +- 查看系统日志：`dmesg | tail` +- 确认单片机程序正常运行 + +## 性能优化 + +### 1. 调整接收缓冲区 + +在 `receive_data()` 中： + +```cpp +// 增大缓冲区限制 +if (serial_buffer_.size() > 8192) // 默认 4096 +{ + serial_buffer_.clear(); +} +``` + +### 2. 调整统计间隔 + +```cpp +// 减少统计频率 +statistics_timer_ = this->create_wall_timer( + std::chrono::seconds(10), // 默认 5 秒 + std::bind(&RMSerialDriver::statistics_timer_callback, this)); +``` + +### 3. 禁用调试日志 + +生产环境中使用 INFO 级别： + +```bash +ros2 run rm_serial_driver rm_serial_driver --ros-args --log-level info +``` + +## 更多文档 + +- [协议修改指南](PROTOCOL_GUIDE.md) - 详细说明如何修改通信协议 +- [单片机示例代码](../examples/) - Arduino/STM32 示例代码 + +## 许可证 + +Apache License 2.0 diff --git a/src/rm_serial_driver/examples/mcu_example_new_protocol.ino b/src/rm_serial_driver/examples/mcu_example_new_protocol.ino new file mode 100644 index 0000000..bc19039 --- /dev/null +++ b/src/rm_serial_driver/examples/mcu_example_new_protocol.ino @@ -0,0 +1,337 @@ +/* + * RM Serial Driver - 单片机端示例代码（新协议） + * 适用于 Arduino/STM32 + * + * 协议格式： + * - 帧头: 'M' 'R' (0x4D 0x52) + * - 数据: 根据不同数据类型 + * - CRC16: CRC16-CCITT (初始值 0xFFFF) + */ + +#include +#include + +// ==================== 协议定义 ==================== + +#define FRAME_HEADER_0 'M' // 0x4D +#define FRAME_HEADER_1 'R' // 0x52 +#define CRC16_INIT 0xFFFF + +// MCU 发送给上位机的数据 +typedef struct __attribute__((packed)) +{ + uint8_t head[2]; // 帧头 'M' 'R' + uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符 + float q[4]; // 四元数 wxyz 顺序 + float yaw; // 偏航角 + float yaw_vel; // 偏航角速度 + float pitch; // 俯仰角 + float pitch_vel; // 俯仰角速度 + float bullet_speed; // 弹速 + uint16_t bullet_count; // 子弹累计发送次数 +} DataMCU_t; + +typedef struct __attribute__((packed)) +{ + DataMCU_t data; + uint16_t crc16; +} PackageMCU_t; + +// 上位机发送给 MCU 的控制命令 +typedef struct __attribute__((packed)) +{ + uint8_t head[2]; // 帧头 'M' 'R' + uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火 + float yaw; // 目标偏航角 + float yaw_vel; // 偏航角速度 + float yaw_acc; // 偏航角加速度 + float pitch; // 目标俯仰角 + float pitch_vel; // 俯仰角速度 + float pitch_acc; // 俯仰角加速度 + float vx; // X 方向速度 + float vy; // Y 方向速度 + float wz; // Z 方向角速度 + uint8_t reserved; // 预留字段 +} DataAI_t; + +typedef struct __attribute__((packed)) +{ + DataAI_t data; + uint16_t crc16; +} PackageAI_t; + +// ==================== CRC16 计算 ==================== + +static const uint16_t crc16_tab[256] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 +}; + +uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc) +{ + while (len--) + { + crc = (crc >> 8) ^ crc16_tab[(crc ^ (*buf++)) & 0xFF]; + } + return crc; +} + +bool CRC16_Verify(const uint8_t *buf, size_t len) +{ + if (len < 2) + return false; + + uint16_t expected = CRC16_Calc(buf, len - 2, CRC16_INIT); + uint16_t received = buf[len - 2] | (buf[len - 1] << 8); + + return expected == received; +} + +// ==================== 发送 MCU 数据 ==================== + +void send_mcu_data(uint8_t mode, float q[4], float yaw, float yaw_vel, + float pitch, float pitch_vel, float bullet_speed, uint16_t bullet_count) +{ + PackageMCU_t package; + + // 设置帧头 + package.data.head[0] = FRAME_HEADER_0; // 'M' + package.data.head[1] = FRAME_HEADER_1; // 'R' + + // 填充数据 + package.data.mode = mode; + memcpy(package.data.q, q, sizeof(float) * 4); + package.data.yaw = yaw; + package.data.yaw_vel = yaw_vel; + package.data.pitch = pitch; + package.data.pitch_vel = pitch_vel; + package.data.bullet_speed = bullet_speed; + package.data.bullet_count = bullet_count; + + // 计算 CRC（从帧头开始） + package.crc16 = CRC16_Calc((uint8_t*)&package.data, sizeof(DataMCU_t), CRC16_INIT); + + // 发送 + Serial.write((uint8_t*)&package, sizeof(PackageMCU_t)); +} + +// ==================== 接收 AI 控制命令 ==================== + +uint8_t rx_buffer[256]; +uint16_t rx_index = 0; + +void receive_ai_command() +{ + while (Serial.available()) + { + uint8_t byte = Serial.read(); + + // 查找帧头 'M' + if (rx_index == 0 && byte != FRAME_HEADER_0) + { + continue; + } + + rx_buffer[rx_index++] = byte; + + // 防止缓冲区溢出 + if (rx_index >= sizeof(rx_buffer)) + { + rx_index = 0; + continue; + } + + // 检查是否接收到完整的包头 + if (rx_index >= 2) + { + // 验证第二个字节是否为 'R' + if (rx_buffer[1] != FRAME_HEADER_1) + { + // 帧头不完整，重新开始 + rx_index = 0; + continue; + } + } + + // 检查是否接收完整 + if (rx_index >= sizeof(PackageAI_t)) + { + // 验证 CRC + if (CRC16_Verify(rx_buffer, sizeof(PackageAI_t))) + { + // CRC 校验通过，处理数据包 + PackageAI_t *package = (PackageAI_t*)rx_buffer; + process_ai_command(&package->data); + } + else + { + // CRC 校验失败 + Serial.println("CRC Error!"); + } + + // 重置接收缓冲区 + rx_index = 0; + } + } +} + +void process_ai_command(DataAI_t *data) +{ + // 处理 AI 控制命令 + switch (data->mode) + { + case 0: // 不控制 + // 停止控制 + break; + + case 1: // 控制云台但不开火 + // 控制云台到目标位置 + control_gimbal(data->yaw, data->pitch, data->yaw_vel, data->pitch_vel); + break; + + case 2: // 控制云台且开火 + // 控制云台并开火 + control_gimbal(data->yaw, data->pitch, data->yaw_vel, data->pitch_vel); + fire(); + break; + } + + // 控制底盘 + control_chassis(data->vx, data->vy, data->wz); +} + +// ==================== Arduino 主程序 ==================== + +void setup() +{ + Serial.begin(115200); + while (!Serial) { + delay(10); + } + + Serial.println("RM Serial Driver - MCU Ready!"); +} + +void loop() +{ + // 接收 AI 控制命令 + receive_ai_command(); + + // 定时发送 MCU 数据（每 10ms） + static unsigned long last_send = 0; + if (millis() - last_send >= 10) + { + // 读取传感器数据 + uint8_t mode = get_current_mode(); + float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // 四元数 + get_quaternion(q); + float yaw = get_yaw(); + float yaw_vel = get_yaw_velocity(); + float pitch = get_pitch(); + float pitch_vel = get_pitch_velocity(); + float bullet_speed = get_bullet_speed(); + uint16_t bullet_count = get_bullet_count(); + + // 发送数据 + send_mcu_data(mode, q, yaw, yaw_vel, pitch, pitch_vel, bullet_speed, bullet_count); + + last_send = millis(); + } +} + +// ==================== 辅助函数（需要根据实际硬件实现）==================== + +uint8_t get_current_mode() +{ + // 返回当前模式：0=空闲, 1=自瞄, 2=小符, 3=大符 + return 1; // 示例：自瞄模式 +} + +void get_quaternion(float q[4]) +{ + // 从 IMU 读取四元数 + // q[0] = w, q[1] = x, q[2] = y, q[3] = z +} + +float get_yaw() +{ + // 读取偏航角 + return 0.0f; +} + +float get_yaw_velocity() +{ + // 读取偏航角速度 + return 0.0f; +} + +float get_pitch() +{ + // 读取俯仰角 + return 0.0f; +} + +float get_pitch_velocity() +{ + // 读取俯仰角速度 + return 0.0f; +} + +float get_bullet_speed() +{ + // 读取弹速 + return 15.0f; +} + +uint16_t get_bullet_count() +{ + // 返回子弹累计发送次数 + static uint16_t count = 0; + return count++; +} + +void control_gimbal(float yaw, float pitch, float yaw_vel, float pitch_vel) +{ + // 控制云台到目标位置 + // 实现 PID 控制等 +} + +void control_chassis(float vx, float vy, float wz) +{ + // 控制底盘运动 +} + +void fire() +{ + // 开火 +} diff --git a/src/rm_serial_driver/include/rm_serial_driver/crc.hpp b/src/rm_serial_driver/include/rm_serial_driver/crc.hpp new file mode 100644 index 0000000..8709e4e --- /dev/null +++ b/src/rm_serial_driver/include/rm_serial_driver/crc.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include +#include + +#define CRC16_INIT 0xFFFF + +namespace crc16 { + +/** + * @brief 计算 CRC16-CCITT 校验值 + * @param buf 数据缓冲区 + * @param len 数据长度 + * @param crc 初始值 + * @return CRC16 校验值 + */ +uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc); + +/** + * @brief 验证 CRC16 校验值（数据末尾包含2字节CRC） + * @param buf 数据缓冲区（包含CRC） + * @param len 总长度（包含CRC） + * @return 校验是否通过 + */ +bool CRC16_Verify(const uint8_t *buf, size_t len); + +} // namespace crc16 \ No newline at end of file diff --git a/src/rm_serial_driver/include/rm_serial_driver/protocol.hpp b/src/rm_serial_driver/include/rm_serial_driver/protocol.hpp new file mode 100644 index 0000000..80f0e1a --- /dev/null +++ b/src/rm_serial_driver/include/rm_serial_driver/protocol.hpp @@ -0,0 +1,69 @@ +#pragma once + +#include + +// 帧头定义 +#define FRAME_HEADER_0 'M' // 0x4D +#define FRAME_HEADER_1 'R' // 0x52 + +// 数据包 ID（已废弃，使用帧头识别） +#define ID_MCU (0xC4) +#define ID_REF (0xA8) + +// MCU 数据结构（MCU -> 上位机） +typedef struct __attribute__((packed)) +{ + uint8_t head[2]; // 帧头 'M' 'R' + uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符 + float q[4]; // 四元数 wxyz 顺序 + float yaw; // 偏航角 + float yaw_vel; // 偏航角速度 + float pitch; // 俯仰角 + float pitch_vel; // 俯仰角速度 + float bullet_speed; // 弹速 + uint16_t bullet_count; // 子弹累计发送次数 +} DataMCU_t; + +typedef struct __attribute__((packed)) +{ + DataMCU_t data; + uint16_t crc16; +} PackageMCU_t; + +// 裁判系统数据结构（保持不变） +typedef struct __attribute__((packed)) +{ + uint16_t remain_hp; // 剩余血量 + uint8_t game_progress : 4; // 比赛进度 + uint16_t stage_remain_time; // 比赛剩余时间 +} DataReferee_t; + +typedef struct __attribute__((packed)) +{ + uint8_t id; + DataReferee_t data; + uint16_t crc16; +} PackageReferee_t; + +// AI 控制数据结构（上位机 -> MCU） +typedef struct __attribute__((packed)) +{ + uint8_t head[2]; // 帧头 'M' 'R' + uint8_t mode; // 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火 + float yaw; // 目标偏航角 + float yaw_vel; // 偏航角速度 + float yaw_acc; // 偏航角加速度 + float pitch; // 目标俯仰角 + float pitch_vel; // 俯仰角速度 + float pitch_acc; // 俯仰角加速度 + float vx; // X 方向速度 + float vy; // Y 方向速度 + float wz; // Z 方向角速度 + uint8_t reserved; // 预留字段 +} DataAI_t; + +typedef struct __attribute__((packed)) +{ + DataAI_t data; + uint16_t crc16; +} PackageAI_t; diff --git a/src/rm_serial_driver/include/rm_serial_driver/rm_serial_driver.hpp b/src/rm_serial_driver/include/rm_serial_driver/rm_serial_driver.hpp new file mode 100644 index 0000000..df5453b --- /dev/null +++ b/src/rm_serial_driver/include/rm_serial_driver/rm_serial_driver.hpp @@ -0,0 +1,76 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#include "rm_msgs/msg/data_mcu.hpp" +#include "rm_msgs/msg/data_ref.hpp" +#include "rm_msgs/msg/data_ai.hpp" +#include "rm_serial_driver/crc.hpp" +#include "rm_serial_driver/protocol.hpp" + +namespace rm_serial_driver +{ + + class RMSerialDriver : public rclcpp::Node + { + public: + explicit RMSerialDriver(const rclcpp::NodeOptions &options); + ~RMSerialDriver() override; + + private: + // 协议处理 + size_t get_expected_length(uint8_t id); + void process_packet(uint8_t id, const char *data, size_t packet_size); + + // 串口操作 + bool open_port(); + void close_port(); + void receive_data(); + void reopen_port(); + + // ROS 回调 + void data_ai_callback(const rm_msgs::msg::DataAI::SharedPtr msg); + + // 统计定时器 + void statistics_timer_callback(); + + // 串口参数 + std::string device_name_; + int baud_rate_; + int reconnect_interval_ms_; + bool enable_statistics_; + + // 串口对象 + std::unique_ptr serial_port_; + + // ROS 发布订阅 + rclcpp::Publisher::SharedPtr data_mcu_pub_; + rclcpp::Publisher::SharedPtr data_ref_pub_; + rclcpp::Subscription::SharedPtr data_ai_sub_; + + // 定时器 + rclcpp::TimerBase::SharedPtr statistics_timer_; + + // 接收线程 + std::thread read_thread_; + std::atomic running_; + std::atomic is_connected_; + + // 数据缓冲区 + std::string serial_buffer_; + + // 统计信息 + std::atomic rx_count_{0}; + std::atomic tx_count_{0}; + std::atomic crc_error_count_{0}; + std::chrono::steady_clock::time_point last_receive_time_; + }; + +} // namespace rm_serial_driver diff --git a/src/rm_serial_driver/include/rm_serial_driver/新协议.txt b/src/rm_serial_driver/include/rm_serial_driver/新协议.txt new file mode 100644 index 0000000..c0c9ac0 --- /dev/null +++ b/src/rm_serial_driver/include/rm_serial_driver/新协议.txt @@ -0,0 +1,39 @@ +DataMCU_t + + uint8_t head[2] = {'M', 'R'}; + uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符 + float q[4]; // wxyz顺序 + float yaw; + float yaw_vel; + float pitch; + float pitch_vel; + float bullet_speed; + uint16_t bullet_count; // 子弹累计发送次数 + + +DataReferee_t + + uint16_t remain_hp; /* 剩余血量 */ + uint8_t game_progress : 4; /* 比赛进度 */ + uint16_t stage_remain_time; /* 比赛剩余时间 */ + + + +DataAI_t + + uint8_t head[2] = {'M', 'R'}; + uint8_t mode; // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火 + float yaw; + float yaw_vel; + float yaw_acc; + float pitch; + float pitch_vel; + float pitch_acc; + + float vx; + float vy; + float wz; + + uint8_t 预留 + uint16_t crc16; + diff --git a/src/rm_serial_driver/launch/rm_serial_driver.launch.py b/src/rm_serial_driver/launch/rm_serial_driver.launch.py new file mode 100755 index 0000000..53d112b --- /dev/null +++ b/src/rm_serial_driver/launch/rm_serial_driver.launch.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3 + +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(): + """启动 RM Serial Driver 节点""" + + # 获取配置文件路径 + pkg_share = get_package_share_directory('rm_serial_driver') + config_file = os.path.join(pkg_share, 'config', 'serial_config.yaml') + + # 创建节点 + rm_serial_driver_node = Node( + package='rm_serial_driver', + executable='rm_serial_driver', + name='rm_serial_driver', + output='screen', + emulate_tty=True, + parameters=[config_file] + ) + + return LaunchDescription([ + rm_serial_driver_node + ]) diff --git a/src/rm_serial_driver/package.xml b/src/rm_serial_driver/package.xml new file mode 100644 index 0000000..2696010 --- /dev/null +++ b/src/rm_serial_driver/package.xml @@ -0,0 +1,18 @@ + + rm_serial_driver + 0.0.0 + ROS2 Humble Serial Driver + Your Name + Apache-2.0 + + ament_cmake + geometry_msgs + rclcpp + std_msgs + serial + rm_msgs + + + ament_cmake + + \ No newline at end of file diff --git a/src/rm_serial_driver/src/crc.cpp b/src/rm_serial_driver/src/crc.cpp new file mode 100644 index 0000000..33310a2 --- /dev/null +++ b/src/rm_serial_driver/src/crc.cpp @@ -0,0 +1,78 @@ +#include "rm_serial_driver/crc.hpp" +#include + +namespace crc16 { + +static const uint16_t crc16_tab[256] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, + 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399, + 0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, + 0xfbef, 0xea66, 0xd8fd, 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, 0x430c, 0x7197, 0x601e, + 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5, + 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, + 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, 0x0948, 0x3bd3, 0x2a5a, + 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710, + 0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, + 0x0c60, 0x1de9, 0x2f72, 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, 0xf687, 0xc41c, 0xd595, + 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c, + 0x3de3, 0x2c6a, 0x1ef1, 0x0f78}; + +/** + * @brief 处理一个字节的数据，更新 CRC：CRC16-CCITT + * @param crc 初始值 + * @param data 当前字节数据 + * @return uint16_t 更新后的 CRC 值 + */ +static inline uint16_t CRC16_Byte(uint16_t crc, const uint8_t data) +{ + return (crc >> 8) ^ crc16_tab[(crc ^ data) & 0xff]; +} + +/** + * @brief 计算 CRC：CRC16-CCITT + * @param buf 数据缓冲区 + * @param len 数据长度 + * @param crc 初始值 + * @return + */ +uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc) +{ + while (len--) + { + crc = CRC16_Byte(crc, *buf++); + } + return crc; +} + +/** + * @brief 验证 CRC：CRC16-CCITT + * @param buf 数据缓冲区（包含CRC） + * @param len 总长度（包含CRC） + * @return 校验是否通过 + */ +bool CRC16_Verify(const uint8_t *buf, size_t len) +{ + if (len < sizeof(uint16_t)) + return false; + + // 计算期望的CRC值 + uint16_t expected = CRC16_Calc(buf, len - sizeof(uint16_t), CRC16_INIT); + + // 安全地读取接收到的CRC值（避免未对齐访问） + uint16_t received_crc; + std::memcpy(&received_crc, buf + len - sizeof(uint16_t), sizeof(uint16_t)); + + return expected == received_crc; +} + +} // namespace crc16 \ No newline at end of file diff --git a/src/rm_serial_driver/src/rm_serial_driver.cpp b/src/rm_serial_driver/src/rm_serial_driver.cpp new file mode 100644 index 0000000..340597a --- /dev/null +++ b/src/rm_serial_driver/src/rm_serial_driver.cpp @@ -0,0 +1,476 @@ +#include "rm_serial_driver/rm_serial_driver.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rm_serial_driver +{ + + /** + * @brief 构造函数：初始化节点，配置参数，打开串口，创建发布者和订阅者，启动接收线程 + */ + RMSerialDriver::RMSerialDriver(const rclcpp::NodeOptions &options) + : Node("rm_serial_driver", options), + running_(true), + is_connected_(false), + last_receive_time_(std::chrono::steady_clock::now()) + { + // 声明并获取参数 + device_name_ = this->declare_parameter("device_name", std::string("/dev/ttyACM0")); + baud_rate_ = this->declare_parameter("baud_rate", 115200); + reconnect_interval_ms_ = this->declare_parameter("reconnect_interval_ms", 1000); + enable_statistics_ = this->declare_parameter("enable_statistics", true); + std::string topic_prefix = this->declare_parameter("topic_prefix", std::string("serial")); + + RCLCPP_INFO(get_logger(), "========================================"); + RCLCPP_INFO(get_logger(), "RM Serial Driver 启动中..."); + RCLCPP_INFO(get_logger(), " 设备: %s", device_name_.c_str()); + RCLCPP_INFO(get_logger(), " 波特率: %d", baud_rate_); + RCLCPP_INFO(get_logger(), " 话题前缀: %s", topic_prefix.c_str()); + RCLCPP_INFO(get_logger(), "========================================"); + + // 创建发布者和订阅者 + data_mcu_pub_ = this->create_publisher( + topic_prefix + "/data_mcu", 10); + data_ref_pub_ = this->create_publisher( + topic_prefix + "/data_ref", 10); + data_ai_sub_ = this->create_subscription( + topic_prefix + "/data_ai", 10, + std::bind(&RMSerialDriver::data_ai_callback, this, std::placeholders::_1)); + + // 打开串口 + if (open_port()) + { + RCLCPP_INFO(get_logger(), "✓ 串口打开成功"); + is_connected_ = true; + } + else + { + RCLCPP_WARN(get_logger(), "✗ 串口打开失败，将在后台尝试重连..."); + } + + // 启动接收线程 + read_thread_ = std::thread(&RMSerialDriver::receive_data, this); + + // 启动统计定时器 + if (enable_statistics_) + { + statistics_timer_ = this->create_wall_timer( + std::chrono::seconds(5), + std::bind(&RMSerialDriver::statistics_timer_callback, this)); + } + + RCLCPP_INFO(get_logger(), "RM Serial Driver 启动完成！"); + } + + /** + * @brief 析构函数：关闭串口，停止接收线程 + */ + RMSerialDriver::~RMSerialDriver() + { + RCLCPP_INFO(get_logger(), "RM Serial Driver 正在关闭..."); + + running_.store(false); + + if (read_thread_.joinable()) + { + read_thread_.join(); + } + + close_port(); + + RCLCPP_INFO(get_logger(), "RM Serial Driver 已关闭"); + } + + /** + * @brief 打开串口 + */ + bool RMSerialDriver::open_port() + { + try + { + serial_port_ = std::make_unique( + device_name_, + baud_rate_, + serial::Timeout::simpleTimeout(100)); + + if (serial_port_->isOpen()) + { + return true; + } + } + catch (const serial::IOException &e) + { + RCLCPP_ERROR(get_logger(), "串口打开异常: %s", e.what()); + } + catch (const std::exception &e) + { + RCLCPP_ERROR(get_logger(), "未知异常: %s", e.what()); + } + + return false; + } + + /** + * @brief 关闭串口 + */ + void RMSerialDriver::close_port() + { + if (serial_port_ && serial_port_->isOpen()) + { + try + { + serial_port_->close(); + RCLCPP_INFO(get_logger(), "串口已关闭"); + } + catch (const std::exception &e) + { + RCLCPP_ERROR(get_logger(), "关闭串口异常: %s", e.what()); + } + } + }s + + /** + * @brief AI 数据回调函数：接收 AI 控制命令并发送给单片机 + */ + void RMSerialDriver::data_ai_callback(const rm_msgs::msg::DataAI::SharedPtr msg) + { + if (!is_connected_ || !serial_port_ || !serial_port_->isOpen()) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, + "串口未连接，无法发送数据"); + return; + } + + try + { + PackageAI_t package; + + // 设置帧头 + package.data.head[0] = FRAME_HEADER_0; // 'M' + package.data.head[1] = FRAME_HEADER_1; // 'R' + + // 填充数据 + package.data.mode = msg->mode; + package.data.yaw = msg->yaw; + package.data.yaw_vel = msg->yaw_vel; + package.data.yaw_acc = msg->yaw_acc; + package.data.pitch = msg->pitch; + package.data.pitch_vel = msg->pitch_vel; + package.data.pitch_acc = msg->pitch_acc; + package.data.vx = msg->vx; + package.data.vy = msg->vy; + package.data.wz = msg->wz; + package.data.reserved = msg->reserved; + + // 计算 CRC（从帧头开始） + package.crc16 = crc16::CRC16_Calc( + reinterpret_cast(&package.data), + sizeof(DataAI_t), + CRC16_INIT); + + size_t bytes_written = serial_port_->write( + reinterpret_cast(&package), + sizeof(PackageAI_t)); + + if (bytes_written == sizeof(PackageAI_t)) + { + tx_count_++; + RCLCPP_DEBUG(get_logger(), "发送 AI 数据包: mode=%d, yaw=%.2f, pitch=%.2f", + msg->mode, msg->yaw, msg->pitch); + } + else + { + RCLCPP_WARN(get_logger(), "发送数据不完整: %zu/%zu", + bytes_written, sizeof(PackageAI_t)); + } + } + catch (const serial::IOException &e) + { + RCLCPP_ERROR(get_logger(), "发送数据异常: %s", e.what()); + is_connected_ = false; + } + catch (const std::exception &e) + { + RCLCPP_ERROR(get_logger(), "发送数据未知异常: %s", e.what()); + } + } + + /** + * @brief 获取期望的数据包长度 + * @param id 数据包标识（帧头第一个字节） + */ + size_t RMSerialDriver::get_expected_length(uint8_t id) + { + switch (id) + { + case FRAME_HEADER_0: // 'M' - MCU 数据包 + return sizeof(PackageMCU_t); + case ID_REF: // 0xA8 - 裁判系统数据包（保持兼容） + return sizeof(PackageReferee_t); + default: + return 0; + } + } + + /** + * @brief 处理接收到的数据包 + */ + void RMSerialDriver::process_packet(uint8_t id, const char *data, size_t packet_size) + { + if (id == FRAME_HEADER_0) // 'M' - MCU 数据包 + { + PackageMCU_t mcu; + std::memcpy(&mcu, data, packet_size); + + // 验证帧头 + if (mcu.data.head[0] != FRAME_HEADER_0 || mcu.data.head[1] != FRAME_HEADER_1) + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, + "MCU 数据帧头错误: 0x%02X%02X (期望: MR)", + mcu.data.head[0], mcu.data.head[1]); + return; + } + + // CRC 校验（从帧头开始，不包括 CRC 本身） + // if (crc16::CRC16_Verify(reinterpret_cast(&mcu.data), + // sizeof(DataMCU_t) + sizeof(uint16_t))) + // { + rm_msgs::msg::DataMCU msg; + // 逐字段赋值 + msg.mode = mcu.data.mode; + msg.q0 = mcu.data.q[0]; + msg.q1 = mcu.data.q[1]; + msg.q2 = mcu.data.q[2]; + msg.q3 = mcu.data.q[3]; + msg.yaw = mcu.data.yaw; + msg.yaw_vel = mcu.data.yaw_vel; + msg.pitch = mcu.data.pitch; + msg.pitch_vel = mcu.data.pitch_vel; + msg.bullet_speed = mcu.data.bullet_speed; + msg.bullet_count = mcu.data.bullet_count; + data_mcu_pub_->publish(msg); + + rx_count_++; + last_receive_time_ = std::chrono::steady_clock::now(); + + RCLCPP_DEBUG(get_logger(), + "接收 MCU 数据: mode=%d, yaw=%.2f, pitch=%.2f, bullet_count=%d", + mcu.data.mode, mcu.data.yaw, mcu.data.pitch, mcu.data.bullet_count); + // } + // else + // { + // crc_error_count_++; + // RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, + // "MCU 数据 CRC 校验失败 (总计: %lu)", + // crc_error_count_.load()); + // } + } + else if (id == ID_REF) // 0xA8 - 裁判系统数据包 + { + PackageReferee_t ref; + std::memcpy(&ref, data, packet_size); + + // CRC 校验 + if (crc16::CRC16_Verify(reinterpret_cast(&ref) + 1, packet_size - 1)) + { + rm_msgs::msg::DataRef msg; + msg.remain_hp = ref.data.remain_hp; + msg.game_progress = ref.data.game_progress; + msg.stage_remain_time = ref.data.stage_remain_time; + data_ref_pub_->publish(msg); + + rx_count_++; + last_receive_time_ = std::chrono::steady_clock::now(); + + RCLCPP_DEBUG(get_logger(), "接收裁判系统数据: HP=%d, 进度=%d, 时间=%d", + ref.data.remain_hp, ref.data.game_progress, ref.data.stage_remain_time); + } + else + { + crc_error_count_++; + RCLCPP_DEBUG(get_logger(), "裁判系统数据 CRC 校验失败"); + } + } + } + + /** + * @brief 串口数据接收线程 + */ + void RMSerialDriver::receive_data() + { + RCLCPP_INFO(get_logger(), "接收线程已启动"); + + while (rclcpp::ok() && running_.load()) + { + // 如果未连接，尝试重连 + if (!is_connected_) + { + reopen_port(); + std::this_thread::sleep_for(std::chrono::milliseconds(reconnect_interval_ms_)); + continue; + } + + try + { + // 检查串口是否打开 + if (!serial_port_ || !serial_port_->isOpen()) + { + RCLCPP_WARN(get_logger(), "串口意外关闭"); + is_connected_ = false; + continue; + } + + // 检查是否有数据可读 + size_t available = serial_port_->available(); + if (available > 0) + { + std::string buffer = serial_port_->read(available); + serial_buffer_ += buffer; + + // 解析数据包 + while (true) + { + // 查找帧头（'M' 或 0xA8） + size_t header_pos = serial_buffer_.find_first_of( + std::string() + static_cast(FRAME_HEADER_0) + static_cast(ID_REF)); + + if (header_pos == std::string::npos) + { + // 没有找到帧头，清空缓冲区 + serial_buffer_.clear(); + break; + } + + // 删除帧头之前的无效数据 + if (header_pos > 0) + { + RCLCPP_DEBUG(get_logger(), "丢弃 %zu 字节无效数据", header_pos); + serial_buffer_.erase(0, header_pos); + } + + if (serial_buffer_.empty()) + break; + + // 获取帧头第一个字节 + uint8_t id = static_cast(serial_buffer_[0]); + + // 如果是 'M'，需要验证第二个字节是否为 'R' + if (id == FRAME_HEADER_0) + { + if (serial_buffer_.size() < 2) + break; // 数据不够，等待更多数据 + + if (static_cast(serial_buffer_[1]) != FRAME_HEADER_1) + { + // 第二个字节不是 'R'，删除第一个字节继续查找 + RCLCPP_DEBUG(get_logger(), "帧头不完整，丢弃 'M'"); + serial_buffer_.erase(0, 1); + continue; + } + } + + size_t expected_length = get_expected_length(id); + + if (expected_length == 0) + { + // 未知 ID，删除该字节 + RCLCPP_DEBUG(get_logger(), "未知帧头: 0x%02X", id); + serial_buffer_.erase(0, 1); + continue; + } + + // 检查是否接收到完整数据包 + if (serial_buffer_.size() < expected_length) + break; + + // 处理数据包 + process_packet(id, serial_buffer_.data(), expected_length); + + // 删除已处理的数据包 + serial_buffer_.erase(0, expected_length); + } + + // 防止缓冲区无限增长 + if (serial_buffer_.size() > 4096) + { + RCLCPP_WARN(get_logger(), "缓冲区过大，清空"); + serial_buffer_.clear(); + } + } + else + { + // 没有数据，短暂休眠 + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + } + catch (const serial::IOException &e) + { + RCLCPP_ERROR(get_logger(), "串口读取异常: %s", e.what()); + is_connected_ = false; + } + catch (const std::exception &e) + { + RCLCPP_ERROR(get_logger(), "接收数据未知异常: %s", e.what()); + is_connected_ = false; + } + } + + RCLCPP_INFO(get_logger(), "接收线程已退出"); + } + + /** + * @brief 重新打开串口 + */ + void RMSerialDriver::reopen_port() + { + // 关闭旧串口 + close_port(); + + RCLCPP_INFO(get_logger(), "正在尝试重新打开串口: %s", device_name_.c_str()); + + // 尝试打开串口 + if (open_port()) + { + RCLCPP_INFO(get_logger(), "✓ 串口重连成功"); + is_connected_ = true; + serial_buffer_.clear(); + } + else + { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "✗ 串口重连失败，%d ms 后重试...", + reconnect_interval_ms_); + } + } + + /** + * @brief 统计信息定时器回调 + */ + void RMSerialDriver::statistics_timer_callback() + { + if (!is_connected_) + { + RCLCPP_WARN(get_logger(), "状态: 未连接"); + return; + } + + auto now = std::chrono::steady_clock::now(); + auto duration = std::chrono::duration_cast( + now - last_receive_time_).count(); + + RCLCPP_INFO(get_logger(), + "统计 | 接收: %lu | 发送: %lu | CRC错误: %lu | 最后接收: %ld秒前", + rx_count_.load(), tx_count_.load(), crc_error_count_.load(), duration); + } + +} // namespace rm_serial_driver + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(rm_serial_driver::RMSerialDriver) diff --git a/src/rm_serial_driver/tools/debug_serial.py b/src/rm_serial_driver/tools/debug_serial.py new file mode 100755 index 0000000..f7f9175 --- /dev/null +++ b/src/rm_serial_driver/tools/debug_serial.py @@ -0,0 +1,161 @@ +#!/usr/bin/env python3 +""" +串口数据调试工具 +用于查看原始串口数据，帮助排查 CRC 校验失败问题 +""" + +import serial +import sys + +def bytes_to_hex(data): + """将字节转换为十六进制字符串""" + return ' '.join(f'{b:02X}' for b in data) + +def main(): + if len(sys.argv) < 2: + print("用法: python3 debug_serial.py <串口设备> [波特率]") + print("示例: python3 debug_serial.py /dev/ttyUSB0 115200") + sys.exit(1) + + device = sys.argv[1] + baudrate = int(sys.argv[2]) if len(sys.argv) > 2 else 115200 + + print(f"打开串口: {device} @ {baudrate}") + print("按 Ctrl+C 退出\n") + + try: + ser = serial.Serial(device, baudrate, timeout=1) + + buffer = bytearray() + packet_count = 0 + + while True: + if ser.in_waiting > 0: + data = ser.read(ser.in_waiting) + buffer.extend(data) + + print(f"接收 {len(data)} 字节: {bytes_to_hex(data)}") + + # 查找帧头 (0xC4 或 0xA8) + while len(buffer) > 0: + # 查找 MCU 帧头 (0xC4) + if 0xC4 in buffer: + idx = buffer.index(0xC4) + if idx > 0: + print(f" 丢弃 {idx} 字节垃圾数据: {bytes_to_hex(buffer[:idx])}") + buffer = buffer[idx:] + + # MCU 数据包大小: 1(ID) + 28(Data) + 2(CRC) = 31 字节 + if len(buffer) >= 31: + packet = buffer[:31] + buffer = buffer[31:] + + packet_count += 1 + print(f"\n[数据包 #{packet_count}] MCU (0xC4) - 31 字节:") + print(f" 完整数据: {bytes_to_hex(packet)}") + print(f" ID: 0x{packet[0]:02X}") + print(f" 数据: {bytes_to_hex(packet[1:29])}") + print(f" CRC: 0x{packet[29]:02X}{packet[30]:02X}") + + # 计算 CRC + crc = calculate_crc16(packet[1:29]) + received_crc = packet[29] | (packet[30] << 8) + + if crc == received_crc: + print(f" ✓ CRC 校验通过") + else: + print(f" ✗ CRC 校验失败: 计算值=0x{crc:04X}, 接收值=0x{received_crc:04X}") + print() + else: + break + + # 查找裁判系统帧头 (0xA8) + elif 0xA8 in buffer: + idx = buffer.index(0xA8) + if idx > 0: + print(f" 丢弃 {idx} 字节垃圾数据: {bytes_to_hex(buffer[:idx])}") + buffer = buffer[idx:] + + # 裁判系统数据包大小: 1(ID) + 5(Data) + 2(CRC) = 8 字节 + if len(buffer) >= 8: + packet = buffer[:8] + buffer = buffer[8:] + + packet_count += 1 + print(f"\n[数据包 #{packet_count}] 裁判系统 (0xA8) - 8 字节:") + print(f" 完整数据: {bytes_to_hex(packet)}") + print(f" ID: 0x{packet[0]:02X}") + print(f" 数据: {bytes_to_hex(packet[1:6])}") + print(f" CRC: 0x{packet[6]:02X}{packet[7]:02X}") + + # 计算 CRC + crc = calculate_crc16(packet[1:6]) + received_crc = packet[6] | (packet[7] << 8) + + if crc == received_crc: + print(f" ✓ CRC 校验通过") + else: + print(f" ✗ CRC 校验失败: 计算值=0x{crc:04X}, 接收值=0x{received_crc:04X}") + print() + else: + break + else: + # 没有找到帧头，清空缓冲区 + if len(buffer) > 0: + print(f" 未找到帧头，丢弃 {len(buffer)} 字节: {bytes_to_hex(buffer)}") + buffer.clear() + break + + except KeyboardInterrupt: + print("\n\n程序退出") + except Exception as e: + print(f"错误: {e}") + finally: + if 'ser' in locals() and ser.is_open: + ser.close() + +def calculate_crc16(data): + """计算 CRC16-CCITT""" + crc16_tab = [ + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + b58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 + ] + + crc = 0xFFFF + for byte in data: + crc = (crc >> 8) ^ crc16_tab[(crc ^ byte) & 0xFF] + + return crc + +if __name__ == '__main__': + main()