帧定义实现，串口工具更新
This commit is contained in:
47
.gitignore
vendored
47
.gitignore
vendored
@@ -0,0 +1,47 @@
|
||||
# ROS 2 / Colcon
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
|
||||
# CMake
|
||||
CMakeCache.txt
|
||||
CMakeFiles/
|
||||
cmake_install.cmake
|
||||
Makefile
|
||||
CTestTestfile.cmake
|
||||
Testing/
|
||||
|
||||
# C++ 编译输出
|
||||
*.o
|
||||
*.obj
|
||||
*.so
|
||||
*.a
|
||||
*.dll
|
||||
*.exe
|
||||
*.out
|
||||
|
||||
# IDE
|
||||
.vscode/
|
||||
.idea/
|
||||
*.swp
|
||||
*.swo
|
||||
*~
|
||||
|
||||
# Python
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
*$py.class
|
||||
*.so
|
||||
.Python
|
||||
|
||||
# 系统文件
|
||||
.DS_Store
|
||||
Thumbs.db
|
||||
|
||||
# 临时文件
|
||||
*.tmp
|
||||
*.temp
|
||||
*.log
|
||||
|
||||
# 保留 compile_commands.json 给 clangd 使用
|
||||
# (不忽略 build/compile_commands.json，通过 CMake 导出到项目根目录)
|
||||
28
.vscode/settings.json
vendored
28
.vscode/settings.json
vendored
@@ -1,8 +1,28 @@
|
||||
{
|
||||
"clangd.arguments": [
|
||||
// Clangd 配置
|
||||
"clangd.path": "/usr/bin/clangd",
|
||||
"clangd.arguments": [
|
||||
"--background-index",
|
||||
"--compile-commands-dir=${workspaceFolder}/build",
|
||||
"--completion-style=detailed",
|
||||
"--query-driver=/usr/bin/clang",
|
||||
"--header-insertion=never"
|
||||
"--header-insertion=iwyu",
|
||||
"--completion-style=bundled",
|
||||
"--pch-storage=memory",
|
||||
"--cross-file-rename"
|
||||
],
|
||||
// C++ 配置
|
||||
"C_Cpp.intelliSenseEngine": "disabled", // 使用 clangd 替代默认引擎
|
||||
"C_Cpp.autocomplete": "disabled",
|
||||
"C_Cpp.errorSquiggles": "disabled",
|
||||
// 文件关联
|
||||
"files.associations": {
|
||||
"*.h": "c",
|
||||
"*.hpp": "cpp",
|
||||
"*.cpp": "cpp"
|
||||
},
|
||||
// 编辑器配置
|
||||
"editor.formatOnSave": true,
|
||||
"editor.tabSize": 4,
|
||||
"editor.insertSpaces": true,
|
||||
// ROS 2 配置
|
||||
"ros.distro": "humble"
|
||||
}
|
||||
@@ -1,3 +1,4 @@
|
||||
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(amadeus_26)
|
||||
|
||||
@@ -5,14 +6,14 @@ project(amadeus_26)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
# 导出 compile_commands.json 给 clangd 使用
|
||||
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
||||
|
||||
# 查找依赖
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
|
||||
# 查找 serial 库 (用于 UART 节点)
|
||||
find_package(serial REQUIRED)
|
||||
|
||||
# 添加 SDK 库路径
|
||||
set(TRANSMITTER_SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib/transmitter_sdk)
|
||||
link_directories(${TRANSMITTER_SDK_PATH}/lib/linux/x64)
|
||||
@@ -23,37 +24,16 @@ include_directories(
|
||||
${TRANSMITTER_SDK_PATH}/inc
|
||||
)
|
||||
|
||||
# ==================== CAN 节点 ====================
|
||||
add_executable(transmitter_test_node src/transmitter_test_node.cpp)
|
||||
|
||||
set_target_properties(transmitter_test_node PROPERTIES
|
||||
BUILD_WITH_INSTALL_RPATH FALSE
|
||||
LINK_FLAGS "-Wl,-rpath,'${TRANSMITTER_SDK_PATH}/lib/linux/x64'"
|
||||
)
|
||||
|
||||
ament_target_dependencies(transmitter_test_node
|
||||
rclcpp
|
||||
std_msgs
|
||||
)
|
||||
|
||||
target_link_libraries(transmitter_test_node
|
||||
dm_device
|
||||
usb-1.0
|
||||
pthread
|
||||
)
|
||||
|
||||
# ==================== UART 节点 ====================
|
||||
add_executable(uart_transmitter_node src/uart_transmitter_node.cpp)
|
||||
|
||||
ament_target_dependencies(uart_transmitter_node
|
||||
rclcpp
|
||||
std_msgs
|
||||
serial
|
||||
)
|
||||
|
||||
# ==================== 安装目标 ====================
|
||||
install(TARGETS
|
||||
transmitter_test_node
|
||||
uart_transmitter_node
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
17
README.md
17
README.md
@@ -10,8 +10,8 @@
|
||||
|
||||
- MV-SUA133GC-T 工业相机 (USB)
|
||||
- NUC10i7 微型计算机 (主机)
|
||||
- FDI Systems DETA10 IMU 模块 (USB - CP210x 串口芯片 - UART) `/dev/ttyUSB0`
|
||||
- 达妙 USB2CAN-DUAL USB 转 CAN 模块 (USB - 模块 - CAN*2) `/dev/ttyACM0`
|
||||
- FDI Systems DETA10 IMU 模块 (USB - CP210x 串口芯片 - UART) `/dev/ttyUSB1`
|
||||
- CH340 USB 转 UART 模块 (USB - 模块 - UART) `/dev/ttyUSB0`
|
||||
|
||||
### 软件
|
||||
|
||||
@@ -19,12 +19,11 @@
|
||||
- OpenCV 4.13 with Contrib
|
||||
- Eigen 3
|
||||
- MVSDK (工业相机 SDK)
|
||||
- dm_device (USB 转 CAN 模块 SDK)
|
||||
- 待补充...
|
||||
|
||||
## 目标
|
||||
|
||||
感谢同济 SuperPower 战队！自瞄算法参考同济的 sp_vision_26 。项目目标为使 2026 年采用的
|
||||
感谢同济 SuperPower 战队！自瞄算法参考同济的 sp_vision_26 。
|
||||
|
||||
## 软件架构
|
||||
|
||||
@@ -40,14 +39,14 @@ ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为
|
||||
### 收发节点
|
||||
|
||||
- 功能：主机对外通信的封装节点。物理上读取收发模块
|
||||
- 连接：USB2CAN，协商 CAN 通信速率 1000kbps，`/dev/ttyACM0`
|
||||
IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyUSB0`
|
||||
- 接收：USB2CAN -> 裁判系统数据
|
||||
- 连接：CH340，协商 UART 通信速率 115200，`/dev/ttyUSB0`
|
||||
IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyUSB1`
|
||||
- 接收：CH340.RX -> 裁判系统数据
|
||||
- 订阅：中央节点 -> 控制指令
|
||||
- 发送：USB2CAN.CAN2 -> 控制指令
|
||||
- 发送：CH340.TX -> 控制指令
|
||||
- 发布：IMU 数据（地磁角、六轴角加速度、加速度、陀螺仪）
|
||||
裁判系统数据（血量、比赛当前状态）
|
||||
USB2CAN 、IMU 连接 OK 标志
|
||||
CH340 、IMU 连接 OK 标志
|
||||
|
||||
### 中央节点
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -1,10 +0,0 @@
|
||||
# 控制指令的发送说明
|
||||
|
||||
在主机外接收的控制板中的控制协议是：
|
||||
|
||||
| 帧头1 | 帧头2 | 平动左右 | 平动前后 | 云台偏航 | 云台俯仰 | 拨弹轮 | 左拨杆 | 右拨杆 | CRC8 | 帧尾 |
|
||||
| --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- |
|
||||
| 1 | 1 | 2 | 2 | 2 | 2 | 2 | 0.5 | 0.5 | 1 | 1 |
|
||||
|
||||
|
||||
两个 ID 分别是云台和底盘的控制板，在处于进点/回家状态时不应开启小陀螺，发射弹丸需要**打开摩擦轮 同时 进弹**，因此只有在处于攻击状态时开启摩擦轮，此时可以将进弹视为扳机。
|
||||
39
docs/Transmit/ctrl_frame.md
Normal file
39
docs/Transmit/ctrl_frame.md
Normal file
@@ -0,0 +1,39 @@
|
||||
# SRCP 通信协议帧结构
|
||||
|
||||
## 帧格式
|
||||
|
||||
| 字段 | 内容 | 长度 | 说明 |
|
||||
|------|------|------|------|
|
||||
| SOF | 0xBB 0x77 | 2 bytes | 帧头 |
|
||||
| x_move | -660~660 | 2 bytes | 平动左右 (小端序) |
|
||||
| y_move | -660~660 | 2 bytes | 平动前后 (小端序) |
|
||||
| yaw | -660~660 | 2 bytes | 云台偏航 (小端序) |
|
||||
| pitch | -660~660 | 2 bytes | 云台俯仰 (小端序) |
|
||||
| feed | -660~660 | 2 bytes | 拨弹轮 (小端序) |
|
||||
| key | 0~15 | 1 byte | 按键 |
|
||||
| crc8 | CRC8-MAXIM | 1 byte | 校验 |
|
||||
| EOF | 0xEE | 1 byte | 帧尾 |
|
||||
|
||||
## 帧长度
|
||||
|
||||
```
|
||||
SOF(2) + x_move(2) + y_move(2) + yaw(2) + pitch(2) + feed(2) + key(1) + crc8(1) + EOF(1) = 15 bytes
|
||||
```
|
||||
|
||||
**FRAME_LENGTH = 15**
|
||||
|
||||
## 字节序
|
||||
|
||||
- 所有 int16_t 类型字段（x_move, y_move, yaw, pitch, feed）使用 **小端序**
|
||||
|
||||
## 代码示例
|
||||
|
||||
```cpp
|
||||
constexpr int FRAME_LENGTH = 15; // 总帧长度
|
||||
|
||||
frame[0] = 0xBB; // SOF
|
||||
frame[1] = 0x77; // SOF
|
||||
frame[2] = x_move & 0xFF; // 低字节
|
||||
frame[3] = (x_move >> 8) & 0xFF; // 高字节
|
||||
// ... 其他字段
|
||||
frame[14] = 0xEE; // EOF
|
||||
87
docs/Transmit/usb_device_setup.md
Normal file
87
docs/Transmit/usb_device_setup.md
Normal file
@@ -0,0 +1,87 @@
|
||||
# USB 串口设备权限设置指南
|
||||
|
||||
## 文件说明
|
||||
|
||||
| 文件 | 说明 |
|
||||
|------|------|
|
||||
| `99-usb-serial.rules` | udev 规则文件，固定设备名称和权限 |
|
||||
| `setup_usb_permissions.sh` | 自动化设置脚本 |
|
||||
|
||||
## 操作步骤
|
||||
|
||||
### 方法一：使用自动化脚本（推荐）
|
||||
|
||||
```bash
|
||||
cd ~/code/amadeus_26
|
||||
./setup_usb_permissions.sh
|
||||
```
|
||||
|
||||
### 方法二：手动设置
|
||||
|
||||
1. **识别设备 VID/PID**
|
||||
|
||||
```bash
|
||||
# 查看 CH340 设备信息
|
||||
udevadm info -a -n /dev/ttyUSB0 | grep -E "idVendor|idProduct"
|
||||
|
||||
# 查看 IMU 设备信息
|
||||
udevadm info -a -n /dev/ttyUSB1 | grep -E "idVendor|idProduct"
|
||||
```
|
||||
|
||||
2. **安装 udev 规则**
|
||||
|
||||
```bash
|
||||
sudo cp 99-usb-serial.rules /etc/udev/rules.d/
|
||||
sudo udevadm control --reload-rules
|
||||
sudo udevadm trigger
|
||||
```
|
||||
|
||||
3. **添加用户权限**
|
||||
|
||||
```bash
|
||||
sudo usermod -aG dialout $USER
|
||||
```
|
||||
|
||||
4. **生效**
|
||||
|
||||
- 重新插拔 USB 设备，或重启系统
|
||||
- 注销并重新登录，使权限生效
|
||||
|
||||
## 设备映射
|
||||
|
||||
设置完成后，设备将以固定名称出现：
|
||||
|
||||
| 设备 | 固定名称 | 原名称 |
|
||||
|------|----------|--------|
|
||||
| CH340 发送模块 | `/dev/ttyCH340` | `/dev/ttyUSB0` |
|
||||
| IMU 模块 | `/dev/ttyIMU` | `/dev/ttyUSB1` |
|
||||
|
||||
## 验证
|
||||
|
||||
```bash
|
||||
# 检查设备链接
|
||||
ls -la /dev/ttyCH340 /dev/ttyIMU
|
||||
|
||||
# 检查权限
|
||||
ls -la /dev/ttyUSB*
|
||||
```
|
||||
|
||||
## 使用
|
||||
|
||||
修改 launch 文件中的默认设备路径：
|
||||
|
||||
```python
|
||||
# launch/uart_transmitter.launch.py
|
||||
serial_port_arg = DeclareLaunchArgument(
|
||||
'serial_port',
|
||||
default_value='/dev/ttyCH340', # 使用固定名称
|
||||
description='CH340 串口设备路径'
|
||||
)
|
||||
```
|
||||
|
||||
运行节点：
|
||||
|
||||
```bash
|
||||
ros2 launch amadeus_26 uart_transmitter.launch.py
|
||||
# 或指定 IMU 设备
|
||||
ros2 launch amadeus_26 uart_transmitter.launch.py serial_port:=/dev/ttyIMU
|
||||
@@ -8,19 +8,19 @@ def generate_launch_description():
|
||||
# 声明启动参数
|
||||
serial_port_arg = DeclareLaunchArgument(
|
||||
'serial_port',
|
||||
default_value='/dev/ttyUSB0',
|
||||
default_value='/dev/ttyCH340',
|
||||
description='CH340 串口设备路径'
|
||||
)
|
||||
|
||||
baudrate_arg = DeclareLaunchArgument(
|
||||
'baudrate',
|
||||
default_value='921600',
|
||||
default_value='115200',
|
||||
description='串口波特率'
|
||||
)
|
||||
|
||||
send_frequency_arg = DeclareLaunchArgument(
|
||||
'send_frequency',
|
||||
default_value='50.0',
|
||||
default_value='10.0',
|
||||
description='发送频率 (Hz)'
|
||||
)
|
||||
|
||||
|
||||
18
package.xml
Normal file
18
package.xml
Normal file
@@ -0,0 +1,18 @@
|
||||
<?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>amadeus_26</name>
|
||||
<version>0.0.1</version>
|
||||
<description>Amadeus 26 transmission and control package</description>
|
||||
<maintainer email="maintainer@example.com">maintainer</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,309 +1,357 @@
|
||||
/**
|
||||
* @file uart_transmitter_node.cpp
|
||||
* @brief UART 串口收发模块测试节点 (CH340)
|
||||
*
|
||||
* 使用 CH340 USB 转串口模块发送控制指令
|
||||
* 波特率：921600 (与裁判系统一致)
|
||||
*
|
||||
* 默认波特率：115200
|
||||
* 协议帧格式：
|
||||
* | 帧头1 | 帧头2 | 平动左右 | 平动前后 | 云台偏航 | 云台俯仰 | 拨弹轮 | 拨杆 | CRC8 | 帧尾 |
|
||||
* | 0xAA | 0x55 | 2 bytes | 2 bytes | 2 bytes | 2 bytes | 2 bytes|1 byte| 1byte| 0xFF |
|
||||
* | 0xBB | 0x77 | 平动左右 | 平动前后 | 云台偏航 | 云台俯仰 | 拨弹轮 | 拨杆 |
|
||||
* CRC8 | 帧尾 | | 0xAA | 0x55 | 2 bytes | 2 bytes | 2 bytes | 2 bytes | 2
|
||||
* bytes|1 byte| 1byte| 0xEE |
|
||||
*/
|
||||
|
||||
#include <cstring>
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <std_msgs/msg/bool.hpp>
|
||||
#include <serial/serial.h>
|
||||
#include <termios.h>
|
||||
#include <thread>
|
||||
#include <cstring>
|
||||
#include <unistd.h>
|
||||
#include <vector>
|
||||
|
||||
// 帧定义
|
||||
constexpr uint8_t FRAME_HEADER_1 = 0xAA;
|
||||
constexpr uint8_t FRAME_HEADER_2 = 0x55;
|
||||
constexpr uint8_t FRAME_TAIL = 0xFF;
|
||||
constexpr int FRAME_LENGTH = 14; // 总帧长度
|
||||
constexpr uint8_t FRAME_HEADER_1 = 0xBB;
|
||||
constexpr uint8_t FRAME_HEADER_2 = 0x77;
|
||||
constexpr uint8_t FRAME_TAIL = 0xEE;
|
||||
constexpr int FRAME_LENGTH = 15;
|
||||
|
||||
// 默认串口设备
|
||||
constexpr const char* DEFAULT_SERIAL_PORT = "/dev/ttyUSB0";
|
||||
constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyCH340";
|
||||
constexpr int DEFAULT_BAUDRATE = 115200;
|
||||
|
||||
class UartTransmitterNode : public rclcpp::Node
|
||||
{
|
||||
class UartTransmitterNode : public rclcpp::Node {
|
||||
public:
|
||||
UartTransmitterNode() : Node("uart_transmitter_node")
|
||||
{
|
||||
// 声明参数
|
||||
this->declare_parameter("serial_port", DEFAULT_SERIAL_PORT);
|
||||
this->declare_parameter("baudrate", DEFAULT_BAUDRATE);
|
||||
this->declare_parameter("send_frequency", 50.0); // Hz
|
||||
|
||||
// 控制参数
|
||||
this->declare_parameter("x_move", 0); // 平动左右 [-660, 660]
|
||||
this->declare_parameter("y_move", 0); // 平动前后 [-660, 660]
|
||||
this->declare_parameter("yaw", 0); // 云台偏航 [-660, 660]
|
||||
this->declare_parameter("pitch", 0); // 云台俯仰 [-660, 660]
|
||||
this->declare_parameter("feed", 0); // 拨弹轮 [-660, 660]
|
||||
this->declare_parameter("left_switch", 0); // 左拨杆 [0, 15]
|
||||
this->declare_parameter("right_switch", 0); // 右拨杆 [0, 15]
|
||||
UartTransmitterNode() : Node("uart_transmitter_node") {
|
||||
// 声明参数
|
||||
this->declare_parameter("serial_port", DEFAULT_SERIAL_PORT);
|
||||
this->declare_parameter("baudrate", DEFAULT_BAUDRATE);
|
||||
this->declare_parameter("send_frequency", 50.0); // Hz
|
||||
|
||||
// 获取参数
|
||||
serial_port_ = this->get_parameter("serial_port").as_string();
|
||||
baudrate_ = this->get_parameter("baudrate").as_int();
|
||||
send_frequency_ = this->get_parameter("send_frequency").as_double();
|
||||
// 控制参数
|
||||
this->declare_parameter("x_move", 0); // 平动左右 [-660, 660]
|
||||
this->declare_parameter("y_move", 0); // 平动前后 [-660, 660]
|
||||
this->declare_parameter("yaw", 0); // 云台偏航 [-660, 660]
|
||||
this->declare_parameter("pitch", 0); // 云台俯仰 [-660, 660]
|
||||
this->declare_parameter("feed", 0); // 拨弹轮 [-660, 660]
|
||||
this->declare_parameter("left_switch", 0); // 左拨杆 [1, 3]
|
||||
this->declare_parameter("right_switch", 0); // 右拨杆 [1, 3]
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "=================================");
|
||||
RCLCPP_INFO(this->get_logger(), "UART 收发模块测试节点启动");
|
||||
RCLCPP_INFO(this->get_logger(), "串口: %s", serial_port_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "波特率: %d", baudrate_);
|
||||
RCLCPP_INFO(this->get_logger(), "发送频率: %.1f Hz", send_frequency_);
|
||||
RCLCPP_INFO(this->get_logger(), "=================================");
|
||||
// 获取参数
|
||||
serial_port_ = this->get_parameter("serial_port").as_string();
|
||||
baudrate_ = this->get_parameter("baudrate").as_int();
|
||||
send_frequency_ = this->get_parameter("send_frequency").as_double();
|
||||
|
||||
// 初始化串口
|
||||
if (!initSerial()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "串口初始化失败，节点退出");
|
||||
rclcpp::shutdown();
|
||||
return;
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "---------------------------------");
|
||||
RCLCPP_INFO(this->get_logger(), "UART 收发节点启动");
|
||||
RCLCPP_INFO(this->get_logger(), "串口: %s", serial_port_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "波特率: %d", baudrate_);
|
||||
RCLCPP_INFO(this->get_logger(), "发送频率: %.1f Hz", send_frequency_);
|
||||
RCLCPP_INFO(this->get_logger(), "---------------------------------");
|
||||
|
||||
// 创建发布者
|
||||
connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>("transmitter/connection_status", 10);
|
||||
|
||||
// 创建定时器 - 发送数据
|
||||
auto send_period = std::chrono::duration<double>(1.0 / send_frequency_);
|
||||
send_timer_ = this->create_wall_timer(
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(send_period),
|
||||
std::bind(&UartTransmitterNode::sendControlFrame, this));
|
||||
|
||||
// 创建定时器 - 发布连接状态
|
||||
status_timer_ = this->create_wall_timer(
|
||||
std::chrono::seconds(1),
|
||||
std::bind(&UartTransmitterNode::publishStatus, this));
|
||||
|
||||
// 创建接收线程
|
||||
receive_thread_ = std::thread(&UartTransmitterNode::receiveLoop, this);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "UART 节点初始化完成");
|
||||
// 初始化串口
|
||||
if (!initSerial()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "串口初始化失败，节点退出");
|
||||
rclcpp::shutdown();
|
||||
return;
|
||||
}
|
||||
|
||||
~UartTransmitterNode()
|
||||
{
|
||||
if (receive_thread_.joinable()) {
|
||||
running_ = false;
|
||||
receive_thread_.join();
|
||||
}
|
||||
if (serial_.isOpen()) {
|
||||
serial_.close();
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "串口已关闭");
|
||||
// 创建发布者
|
||||
connection_status_pub_ = this->create_publisher<std_msgs::msg::Bool>(
|
||||
"transmitter/connection_status", 10);
|
||||
|
||||
// 创建定时器 - 发送数据
|
||||
auto send_period = std::chrono::duration<double>(1.0 / send_frequency_);
|
||||
send_timer_ = this->create_wall_timer(
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(send_period),
|
||||
std::bind(&UartTransmitterNode::sendControlFrame, this));
|
||||
|
||||
// 创建定时器 - 发布连接状态
|
||||
status_timer_ = this->create_wall_timer(
|
||||
std::chrono::seconds(1),
|
||||
std::bind(&UartTransmitterNode::publishStatus, this));
|
||||
|
||||
// 创建接收线程
|
||||
receive_thread_ = std::thread(&UartTransmitterNode::receiveLoop, this);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "UART 节点初始化完成");
|
||||
}
|
||||
|
||||
~UartTransmitterNode() {
|
||||
running_ = false;
|
||||
if (receive_thread_.joinable()) {
|
||||
receive_thread_.join();
|
||||
}
|
||||
if (serial_fd_ >= 0) {
|
||||
close(serial_fd_);
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "串口已关闭");
|
||||
}
|
||||
|
||||
private:
|
||||
bool initSerial()
|
||||
{
|
||||
try {
|
||||
RCLCPP_INFO(this->get_logger(), "正在打开串口 %s...", serial_port_.c_str());
|
||||
|
||||
serial_.setPort(serial_port_);
|
||||
serial_.setBaudrate(baudrate_);
|
||||
serial::Timeout timeout = serial::Timeout::simpleTimeout(1000);
|
||||
serial_.setTimeout(timeout);
|
||||
serial_.setBytesize(serial::eightbits);
|
||||
serial_.setParity(serial::parity_none);
|
||||
serial_.setStopbits(serial::stopbits_one);
|
||||
|
||||
serial_.open();
|
||||
|
||||
if (serial_.isOpen()) {
|
||||
RCLCPP_INFO(this->get_logger(), "串口打开成功");
|
||||
is_connected_ = true;
|
||||
return true;
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "串口打开失败");
|
||||
return false;
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "串口异常: %s", e.what());
|
||||
return false;
|
||||
}
|
||||
bool initSerial() {
|
||||
RCLCPP_INFO(this->get_logger(), "正在打开串口 %s...", serial_port_.c_str());
|
||||
|
||||
// 打开串口
|
||||
serial_fd_ = open(serial_port_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
|
||||
if (serial_fd_ < 0) {
|
||||
RCLCPP_ERROR(this->get_logger(), "无法打开串口 %s: %s",
|
||||
serial_port_.c_str(), strerror(errno));
|
||||
return false;
|
||||
}
|
||||
|
||||
void sendControlFrame()
|
||||
{
|
||||
if (!serial_.isOpen()) {
|
||||
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "串口未打开");
|
||||
return;
|
||||
}
|
||||
// 配置串口
|
||||
struct termios tty;
|
||||
memset(&tty, 0, sizeof(tty));
|
||||
|
||||
// 获取最新的控制参数
|
||||
int16_t x_move = static_cast<int16_t>(this->get_parameter("x_move").as_int());
|
||||
int16_t y_move = static_cast<int16_t>(this->get_parameter("y_move").as_int());
|
||||
int16_t yaw = static_cast<int16_t>(this->get_parameter("yaw").as_int());
|
||||
int16_t pitch = static_cast<int16_t>(this->get_parameter("pitch").as_int());
|
||||
int16_t feed = static_cast<int16_t>(this->get_parameter("feed").as_int());
|
||||
uint8_t left_switch = static_cast<uint8_t>(this->get_parameter("left_switch").as_int()) & 0x0F;
|
||||
uint8_t right_switch = static_cast<uint8_t>(this->get_parameter("right_switch").as_int()) & 0x0F;
|
||||
|
||||
// 构建数据帧
|
||||
std::vector<uint8_t> frame;
|
||||
frame.reserve(FRAME_LENGTH);
|
||||
|
||||
// 帧头
|
||||
frame.push_back(FRAME_HEADER_1);
|
||||
frame.push_back(FRAME_HEADER_2);
|
||||
|
||||
// 平动左右 (2 bytes, int16, 大端序)
|
||||
frame.push_back((x_move >> 8) & 0xFF);
|
||||
frame.push_back(x_move & 0xFF);
|
||||
|
||||
// 平动前后 (2 bytes)
|
||||
frame.push_back((y_move >> 8) & 0xFF);
|
||||
frame.push_back(y_move & 0xFF);
|
||||
|
||||
// 云台偏航 (2 bytes)
|
||||
frame.push_back((yaw >> 8) & 0xFF);
|
||||
frame.push_back(yaw & 0xFF);
|
||||
|
||||
// 云台俯仰 (2 bytes)
|
||||
frame.push_back((pitch >> 8) & 0xFF);
|
||||
frame.push_back(pitch & 0xFF);
|
||||
|
||||
// 拨弹轮 (2 bytes)
|
||||
frame.push_back((feed >> 8) & 0xFF);
|
||||
frame.push_back(feed & 0xFF);
|
||||
|
||||
// 拨杆 (1 byte: 高4位左拨杆，低4位右拨杆)
|
||||
uint8_t switches = (left_switch << 4) | right_switch;
|
||||
frame.push_back(switches);
|
||||
|
||||
// CRC8 (除帧头外的所有数据)
|
||||
uint8_t crc = calculateCRC8(frame.data() + 2, frame.size() - 2);
|
||||
frame.push_back(crc);
|
||||
|
||||
// 帧尾
|
||||
frame.push_back(FRAME_TAIL);
|
||||
|
||||
// 发送数据
|
||||
try {
|
||||
size_t written = serial_.write(frame.data(), frame.size());
|
||||
if (written != frame.size()) {
|
||||
RCLCPP_WARN(this->get_logger(), "发送数据不完整: %zu/%d", written, FRAME_LENGTH);
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "发送失败: %s", e.what());
|
||||
is_connected_ = false;
|
||||
}
|
||||
if (tcgetattr(serial_fd_, &tty) != 0) {
|
||||
RCLCPP_ERROR(this->get_logger(), "tcgetattr 错误: %s", strerror(errno));
|
||||
close(serial_fd_);
|
||||
serial_fd_ = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t calculateCRC8(const uint8_t* data, size_t len)
|
||||
{
|
||||
uint8_t crc = 0xFF; // 初始值
|
||||
for (size_t i = 0; i < len; i++) {
|
||||
crc ^= data[i];
|
||||
for (int j = 0; j < 8; j++) {
|
||||
if (crc & 0x80) {
|
||||
crc = (crc << 1) ^ 0x31; // CRC8-MAXIM 多项式
|
||||
// 设置波特率
|
||||
speed_t baud = convertBaudrate(baudrate_);
|
||||
cfsetospeed(&tty, baud);
|
||||
cfsetispeed(&tty, baud);
|
||||
|
||||
// 8N1
|
||||
tty.c_cflag &= ~PARENB; // 无校验
|
||||
tty.c_cflag &= ~CSTOPB; // 1位停止位
|
||||
tty.c_cflag &= ~CSIZE;
|
||||
tty.c_cflag |= CS8; // 8位数据
|
||||
tty.c_cflag |= CREAD | CLOCAL; // 启用接收，忽略控制线
|
||||
|
||||
// 禁用硬件流控
|
||||
tty.c_cflag &= ~CRTSCTS;
|
||||
|
||||
// 原始模式
|
||||
tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
|
||||
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
|
||||
tty.c_oflag &= ~OPOST;
|
||||
|
||||
// 设置超时
|
||||
tty.c_cc[VMIN] = 0;
|
||||
tty.c_cc[VTIME] = 1; // 100ms 超时
|
||||
|
||||
if (tcsetattr(serial_fd_, TCSANOW, &tty) != 0) {
|
||||
RCLCPP_ERROR(this->get_logger(), "tcsetattr 错误: %s", strerror(errno));
|
||||
close(serial_fd_);
|
||||
serial_fd_ = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 清空缓冲区
|
||||
tcflush(serial_fd_, TCIOFLUSH);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "串口打开成功");
|
||||
is_connected_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
speed_t convertBaudrate(int baudrate) {
|
||||
switch (baudrate) {
|
||||
case 9600:
|
||||
return B9600;
|
||||
case 19200:
|
||||
return B19200;
|
||||
case 38400:
|
||||
return B38400;
|
||||
case 57600:
|
||||
return B57600;
|
||||
case 115200:
|
||||
return B115200;
|
||||
case 230400:
|
||||
return B230400;
|
||||
case 460800:
|
||||
return B460800;
|
||||
case 921600:
|
||||
return B921600;
|
||||
default:
|
||||
RCLCPP_WARN(this->get_logger(), "不支持的波特率 %d，使用 115200",
|
||||
baudrate);
|
||||
return B115200;
|
||||
}
|
||||
}
|
||||
|
||||
void sendControlFrame() {
|
||||
if (serial_fd_ < 0) {
|
||||
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
|
||||
"串口未打开");
|
||||
return;
|
||||
}
|
||||
|
||||
// 获取控制参数
|
||||
int16_t x_move =
|
||||
static_cast<int16_t>(this->get_parameter("x_move").as_int());
|
||||
int16_t y_move =
|
||||
static_cast<int16_t>(this->get_parameter("y_move").as_int());
|
||||
int16_t yaw = static_cast<int16_t>(this->get_parameter("yaw").as_int());
|
||||
int16_t pitch = static_cast<int16_t>(this->get_parameter("pitch").as_int());
|
||||
int16_t feed = static_cast<int16_t>(this->get_parameter("feed").as_int());
|
||||
uint8_t left_switch =
|
||||
static_cast<uint8_t>(this->get_parameter("left_switch").as_int()) &
|
||||
0x0F;
|
||||
uint8_t right_switch =
|
||||
static_cast<uint8_t>(this->get_parameter("right_switch").as_int()) &
|
||||
0x0F;
|
||||
|
||||
// 构建数据帧
|
||||
uint8_t frame[FRAME_LENGTH];
|
||||
int idx = 0;
|
||||
|
||||
// 帧头
|
||||
frame[idx++] = FRAME_HEADER_1;
|
||||
frame[idx++] = FRAME_HEADER_2;
|
||||
|
||||
// 平动左右 (2 bytes, int16, 小端序)
|
||||
frame[idx++] = x_move & 0xFF;
|
||||
frame[idx++] = (x_move >> 8) & 0xFF;
|
||||
|
||||
// 平动前后 (2 bytes, 小端序)
|
||||
frame[idx++] = y_move & 0xFF;
|
||||
frame[idx++] = (y_move >> 8) & 0xFF;
|
||||
|
||||
// 云台偏航 (2 bytes, 小端序)
|
||||
frame[idx++] = yaw & 0xFF;
|
||||
frame[idx++] = (yaw >> 8) & 0xFF;
|
||||
|
||||
// 云台俯仰 (2 bytes, 小端序)
|
||||
frame[idx++] = pitch & 0xFF;
|
||||
frame[idx++] = (pitch >> 8) & 0xFF;
|
||||
|
||||
// 拨弹轮 (2 bytes, 小端序)
|
||||
frame[idx++] = feed & 0xFF;
|
||||
frame[idx++] = (feed >> 8) & 0xFF;
|
||||
|
||||
// 拨杆 (1 byte: 高4位左拨杆，低4位右拨杆)
|
||||
frame[idx++] = (left_switch << 4) | right_switch;
|
||||
|
||||
// CRC8 (除帧头和帧尾外的所有数据，不包括CRC本身)
|
||||
// 当前idx=13, 数据长度=11 (从frame[2]到frame[12])
|
||||
frame[idx++] = calculateCRC8(frame + 2, 11);
|
||||
|
||||
// 帧尾
|
||||
frame[idx++] = FRAME_TAIL;
|
||||
|
||||
// 发送数据
|
||||
ssize_t written = write(serial_fd_, frame, FRAME_LENGTH);
|
||||
if (written != FRAME_LENGTH) {
|
||||
RCLCPP_WARN(this->get_logger(), "发送数据不完整: %zd/%d", written,
|
||||
FRAME_LENGTH);
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t calculateCRC8(const uint8_t *data, size_t len) {
|
||||
uint8_t crc = 0xFF; // 初始值
|
||||
for (size_t i = 0; i < len; i++) {
|
||||
crc ^= data[i];
|
||||
for (int j = 0; j < 8; j++) {
|
||||
if (crc & 0x80) {
|
||||
crc = (crc << 1) ^ 0x31; // CRC8-MAXIM 多项式
|
||||
} else {
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
void receiveLoop() {
|
||||
uint8_t buffer[256];
|
||||
std::vector<uint8_t> frame_buffer;
|
||||
frame_buffer.reserve(FRAME_LENGTH);
|
||||
|
||||
while (running_ && rclcpp::ok()) {
|
||||
if (serial_fd_ < 0) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
continue;
|
||||
}
|
||||
|
||||
// 读取数据
|
||||
ssize_t n = read(serial_fd_, buffer, sizeof(buffer));
|
||||
if (n > 0) {
|
||||
for (ssize_t i = 0; i < n; i++) {
|
||||
frame_buffer.push_back(buffer[i]);
|
||||
|
||||
// 查找帧头
|
||||
if (frame_buffer.size() >= 2 && frame_buffer[0] == FRAME_HEADER_1 &&
|
||||
frame_buffer[1] == FRAME_HEADER_2) {
|
||||
|
||||
// 等待完整帧
|
||||
if (frame_buffer.size() >= FRAME_LENGTH) {
|
||||
// 检查帧尾
|
||||
if (frame_buffer[FRAME_LENGTH - 1] == FRAME_TAIL) {
|
||||
// 验证 CRC
|
||||
uint8_t rx_crc = frame_buffer[FRAME_LENGTH - 2];
|
||||
uint8_t calc_crc =
|
||||
calculateCRC8(frame_buffer.data() + 2, FRAME_LENGTH - 4);
|
||||
|
||||
if (rx_crc == calc_crc) {
|
||||
RCLCPP_INFO(this->get_logger(), "收到有效帧，CRC 验证通过");
|
||||
} else {
|
||||
crc <<= 1;
|
||||
RCLCPP_WARN(this->get_logger(),
|
||||
"CRC 错误: 接收=%02X, 计算=%02X", rx_crc,
|
||||
calc_crc);
|
||||
}
|
||||
}
|
||||
|
||||
// 清空缓冲区，准备下一帧
|
||||
frame_buffer.clear();
|
||||
}
|
||||
} else if (frame_buffer.size() > FRAME_LENGTH) {
|
||||
// 缓冲区溢出，清空
|
||||
frame_buffer.clear();
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
} else if (n < 0 && errno != EAGAIN) {
|
||||
RCLCPP_ERROR(this->get_logger(), "读取错误: %s", strerror(errno));
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
}
|
||||
|
||||
void receiveLoop()
|
||||
{
|
||||
std::vector<uint8_t> buffer;
|
||||
buffer.reserve(FRAME_LENGTH);
|
||||
void publishStatus() {
|
||||
auto msg = std_msgs::msg::Bool();
|
||||
msg.data = is_connected_ && (serial_fd_ >= 0);
|
||||
connection_status_pub_->publish(msg);
|
||||
}
|
||||
|
||||
while (running_ && rclcpp::ok()) {
|
||||
if (!serial_.isOpen()) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
continue;
|
||||
}
|
||||
// 成员变量
|
||||
std::string serial_port_;
|
||||
int baudrate_;
|
||||
double send_frequency_;
|
||||
|
||||
try {
|
||||
// 读取可用数据
|
||||
size_t available = serial_.available();
|
||||
if (available > 0) {
|
||||
std::vector<uint8_t> data = serial_.read(available);
|
||||
|
||||
// 处理接收到的数据（简单的帧解析）
|
||||
for (uint8_t byte : data) {
|
||||
buffer.push_back(byte);
|
||||
|
||||
// 查找帧头
|
||||
if (buffer.size() >= 2 &&
|
||||
buffer[0] == FRAME_HEADER_1 &&
|
||||
buffer[1] == FRAME_HEADER_2) {
|
||||
|
||||
// 等待完整帧
|
||||
if (buffer.size() >= FRAME_LENGTH) {
|
||||
// 检查帧尾
|
||||
if (buffer[FRAME_LENGTH - 1] == FRAME_TAIL) {
|
||||
// 验证 CRC
|
||||
uint8_t rx_crc = buffer[FRAME_LENGTH - 2];
|
||||
uint8_t calc_crc = calculateCRC8(
|
||||
buffer.data() + 2, FRAME_LENGTH - 4);
|
||||
|
||||
if (rx_crc == calc_crc) {
|
||||
RCLCPP_INFO(this->get_logger(),
|
||||
"收到有效帧，CRC 验证通过");
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(),
|
||||
"CRC 错误: 接收=%02X, 计算=%02X",
|
||||
rx_crc, calc_crc);
|
||||
}
|
||||
}
|
||||
|
||||
// 清空缓冲区，准备下一帧
|
||||
buffer.clear();
|
||||
}
|
||||
} else if (buffer.size() > FRAME_LENGTH) {
|
||||
// 缓冲区溢出，清空
|
||||
buffer.clear();
|
||||
}
|
||||
}
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(this->get_logger(), "接收异常: %s", e.what());
|
||||
is_connected_ = false;
|
||||
}
|
||||
int serial_fd_ = -1;
|
||||
bool is_connected_ = false;
|
||||
bool running_ = true;
|
||||
std::thread receive_thread_;
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
}
|
||||
|
||||
void publishStatus()
|
||||
{
|
||||
auto msg = std_msgs::msg::Bool();
|
||||
msg.data = is_connected_ && serial_.isOpen();
|
||||
connection_status_pub_->publish(msg);
|
||||
}
|
||||
|
||||
// 成员变量
|
||||
std::string serial_port_;
|
||||
int baudrate_;
|
||||
double send_frequency_;
|
||||
|
||||
serial::Serial serial_;
|
||||
bool is_connected_ = false;
|
||||
bool running_ = true;
|
||||
std::thread receive_thread_;
|
||||
|
||||
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
|
||||
rclcpp::TimerBase::SharedPtr send_timer_;
|
||||
rclcpp::TimerBase::SharedPtr status_timer_;
|
||||
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
|
||||
rclcpp::TimerBase::SharedPtr send_timer_;
|
||||
rclcpp::TimerBase::SharedPtr status_timer_;
|
||||
};
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
auto node = std::make_shared<UartTransmitterNode>();
|
||||
|
||||
if (rclcpp::ok()) {
|
||||
rclcpp::spin(node);
|
||||
}
|
||||
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
int main(int argc, char *argv[]) {
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
auto node = std::make_shared<UartTransmitterNode>();
|
||||
|
||||
if (rclcpp::ok()) {
|
||||
rclcpp::spin(node);
|
||||
}
|
||||
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
5
tool/99-usb-serial.rules
Normal file
5
tool/99-usb-serial.rules
Normal file
@@ -0,0 +1,5 @@
|
||||
# CH340 串口模块 (ttyUSB0 -> ttyCH340)
|
||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", SYMLINK+="ttyCH340", MODE="0666", GROUP="dialout"
|
||||
|
||||
# IMU 串口模块 (ttyUSB1 -> ttyIMU)
|
||||
SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", SYMLINK+="ttyIMU", MODE="0666", GROUP="dialout"
|
||||
38
tool/setup_usb_permissions.sh
Executable file
38
tool/setup_usb_permissions.sh
Executable file
@@ -0,0 +1,38 @@
|
||||
#!/bin/bash
|
||||
# USB 串口设备权限和固定名称设置脚本
|
||||
|
||||
set -e
|
||||
|
||||
echo "=================================="
|
||||
echo "USB 串口设备权限设置"
|
||||
echo "=================================="
|
||||
|
||||
# 1. 检查当前设备
|
||||
echo ""
|
||||
echo "当前连接的 USB 串口设备："
|
||||
ls -la /dev/ttyUSB* 2>/dev/null || echo "未发现 ttyUSB 设备"
|
||||
|
||||
echo ""
|
||||
echo "设备详细信息："
|
||||
for dev in /dev/ttyUSB*; do
|
||||
if [ -e "$dev" ]; then
|
||||
echo "--- $dev ---"
|
||||
udevadm info -a -n "$dev" | grep -E "(idVendor|idProduct|serial)" | head -5
|
||||
fi
|
||||
done
|
||||
|
||||
# 2. 安装规则文件
|
||||
echo ""
|
||||
echo "安装 udev 规则"
|
||||
sudo cp 99-usb-serial.rules /etc/udev/rules.d/
|
||||
|
||||
# 3. 重新加载 udev 规则
|
||||
echo "重新加载 udev 规则"
|
||||
sudo udevadm control --reload-rules
|
||||
sudo udevadm trigger
|
||||
|
||||
# 4. 添加用户到 dialout 组
|
||||
echo "将当前用户添加到 dialout 组"
|
||||
sudo usermod -aG dialout $USER
|
||||
|
||||
echo "设置完成，请重新插拔设备"
|
||||
Reference in New Issue
Block a user