diff --git a/.gitignore b/.gitignore index e69de29..5079921 100644 --- a/.gitignore +++ b/.gitignore @@ -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 导出到项目根目录) \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index aa46b6d..2a31b6f 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -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" } \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 92af353..473f354 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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} ) diff --git a/README.md b/README.md index 3c56262..674e417 100644 --- a/README.md +++ b/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 标志 ### 中央节点 diff --git a/camera_sdk/lib/linux/arm64/libMVSDK.so b/camera_sdk/lib/linux/arm64/libMVSDK.so deleted file mode 100644 index 402ea21..0000000 Binary files a/camera_sdk/lib/linux/arm64/libMVSDK.so and /dev/null differ diff --git a/camera_sdk/lib/linux/x64/libMVSDK.so b/camera_sdk/lib/linux/x64/libMVSDK.so deleted file mode 100644 index 625adcb..0000000 Binary files a/camera_sdk/lib/linux/x64/libMVSDK.so and /dev/null differ diff --git a/docs/Transmit/ctrl_command.md b/docs/Transmit/ctrl_command.md deleted file mode 100644 index b80bec6..0000000 --- a/docs/Transmit/ctrl_command.md +++ /dev/null @@ -1,10 +0,0 @@ -# 控制指令的发送说明 - -在主机外接收的控制板中的控制协议是： - -| 帧头1 | 帧头2 | 平动左右 | 平动前后 | 云台偏航 | 云台俯仰 | 拨弹轮 | 左拨杆 | 右拨杆 | CRC8 | 帧尾 | -| --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | -| 1 | 1 | 2 | 2 | 2 | 2 | 2 | 0.5 | 0.5 | 1 | 1 | - - -两个 ID 分别是云台和底盘的控制板，在处于进点/回家状态时不应开启小陀螺，发射弹丸需要**打开摩擦轮 同时 进弹**，因此只有在处于攻击状态时开启摩擦轮，此时可以将进弹视为扳机。 \ No newline at end of file diff --git a/docs/Transmit/ctrl_frame.md b/docs/Transmit/ctrl_frame.md new file mode 100644 index 0000000..3628804 --- /dev/null +++ b/docs/Transmit/ctrl_frame.md @@ -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 \ No newline at end of file diff --git a/docs/Transmit/usb_device_setup.md b/docs/Transmit/usb_device_setup.md new file mode 100644 index 0000000..a96ac44 --- /dev/null +++ b/docs/Transmit/usb_device_setup.md @@ -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 \ No newline at end of file diff --git a/launch/uart_transmitter.launch.py b/launch/uart_transmitter.launch.py index 9a63af6..316ca7d 100644 --- a/launch/uart_transmitter.launch.py +++ b/launch/uart_transmitter.launch.py @@ -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)' ) diff --git a/camera_sdk/inc/CameraApi.h b/lib/camera_sdk/inc/CameraApi.h similarity index 100% rename from camera_sdk/inc/CameraApi.h rename to lib/camera_sdk/inc/CameraApi.h diff --git a/camera_sdk/inc/CameraDefine.h b/lib/camera_sdk/inc/CameraDefine.h similarity index 100% rename from camera_sdk/inc/CameraDefine.h rename to lib/camera_sdk/inc/CameraDefine.h diff --git a/camera_sdk/inc/CameraStatus.h b/lib/camera_sdk/inc/CameraStatus.h similarity index 100% rename from camera_sdk/inc/CameraStatus.h rename to lib/camera_sdk/inc/CameraStatus.h diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..bc57f4d --- /dev/null +++ b/package.xml @@ -0,0 +1,18 @@ + + + + amadeus_26 + 0.0.1 + Amadeus 26 transmission and control package + maintainer + MIT + + ament_cmake + + rclcpp + std_msgs + + + ament_cmake + + \ No newline at end of file diff --git a/src/uart_transmitter_node.cpp b/src/uart_transmitter_node.cpp index cfc630d..1fc5931 100644 --- a/src/uart_transmitter_node.cpp +++ b/src/uart_transmitter_node.cpp @@ -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 +#include +#include #include #include -#include +#include #include -#include +#include #include // 帧定义 -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("transmitter/connection_status", 10); - - // 创建定时器 - 发送数据 - auto send_period = std::chrono::duration(1.0 / send_frequency_); - send_timer_ = this->create_wall_timer( - std::chrono::duration_cast(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( + "transmitter/connection_status", 10); + + // 创建定时器 - 发送数据 + auto send_period = std::chrono::duration(1.0 / send_frequency_); + send_timer_ = this->create_wall_timer( + std::chrono::duration_cast(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(this->get_parameter("x_move").as_int()); - int16_t y_move = static_cast(this->get_parameter("y_move").as_int()); - int16_t yaw = static_cast(this->get_parameter("yaw").as_int()); - int16_t pitch = static_cast(this->get_parameter("pitch").as_int()); - int16_t feed = static_cast(this->get_parameter("feed").as_int()); - uint8_t left_switch = static_cast(this->get_parameter("left_switch").as_int()) & 0x0F; - uint8_t right_switch = static_cast(this->get_parameter("right_switch").as_int()) & 0x0F; - - // 构建数据帧 - std::vector 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(this->get_parameter("x_move").as_int()); + int16_t y_move = + static_cast(this->get_parameter("y_move").as_int()); + int16_t yaw = static_cast(this->get_parameter("yaw").as_int()); + int16_t pitch = static_cast(this->get_parameter("pitch").as_int()); + int16_t feed = static_cast(this->get_parameter("feed").as_int()); + uint8_t left_switch = + static_cast(this->get_parameter("left_switch").as_int()) & + 0x0F; + uint8_t right_switch = + static_cast(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 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 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 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::SharedPtr connection_status_pub_; - rclcpp::TimerBase::SharedPtr send_timer_; - rclcpp::TimerBase::SharedPtr status_timer_; + rclcpp::Publisher::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(); - - 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(); + + if (rclcpp::ok()) { + rclcpp::spin(node); + } + + rclcpp::shutdown(); + return 0; } \ No newline at end of file diff --git a/tool/99-usb-serial.rules b/tool/99-usb-serial.rules new file mode 100644 index 0000000..a798e46 --- /dev/null +++ b/tool/99-usb-serial.rules @@ -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" \ No newline at end of file diff --git a/tool/setup_usb_permissions.sh b/tool/setup_usb_permissions.sh new file mode 100755 index 0000000..fdac62e --- /dev/null +++ b/tool/setup_usb_permissions.sh @@ -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 "设置完成，请重新插拔设备" \ No newline at end of file