略微更改文件架构

.gitignore

@@ -0,0 +1,71 @@
+# ROS2
+build/
+install/
+log/
+
+# C/C++
+*.o
+*.so
+*.a
+*.exe
+*.out
+*.app
+*.i
+*.ii
+*.s
+*.su
+*.gcno
+*.gcda
+*.dSYM/
+*.stackdump
+
+# Python
+__pycache__/
+*.py[cod]
+*$py.class
+.Python
+*.egg-info/
+dist/
+build/
+*.egg
+
+# IDE
+.vscode/
+.idea/
+*.swp
+*.swo
+*~
+.DS_Store
+
+# Compiled files
+*.com
+*.class
+*.dll
+*.so
+*.dylib
+
+# Test files
+test_debug
+test_c
+*.test
+
+# Temporary files
+*.tmp
+*.temp
+*.log
+*.bak
+
+# Core dumps
+core
+core.*
+vgcore.*
+
+# Documentation build
+docs/_build/
+
+# Libraries (keep headers, ignore compiled)
+# Note: We keep lib/ for SDK headers and .so files that are part of the project
+
+# System specific
+Thumbs.db
+.directory

CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.8)
+project(amadeus_26)
+
+# 使用 C++17
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# 查找依赖
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+# 添加 SDK 库路径
+set(TRANSMITTER_SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib/transmitter_sdk)
+link_directories(${TRANSMITTER_SDK_PATH}/lib/linux/x64)
+
+# 包含目录
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}
+  ${TRANSMITTER_SDK_PATH}/inc
+)
+
+# 创建可执行文件
+add_executable(transmitter_test_node src/transmitter_test_node.cpp)
+
+# 设置 RPATH，使可执行文件能找到共享库
+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
+)
+
+# 链接 SDK 库和依赖
+target_link_libraries(transmitter_test_node
+  dm_device
+  usb-1.0
+  pthread
+)
+
+# 安装目标
+install(TARGETS transmitter_test_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# 安装 launch 文件（如果有）
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+  OPTIONAL
+)
+
+ament_package()

docs/TRANSMITTER_TEST.md

@@ -0,0 +1,178 @@
+# 收发模块测试节点使用说明
+
+## 功能概述
+
+`transmitter_test_node` 是一个用于测试 USB2CAN 收发模块功能的 ROS2 节点，主要功能包括：
+
+1. **设备初始化**：自动查找并连接 USB2CAN 设备
+2. **定时发送**：以指定频率发送测试控制指令
+   - ID 0x149: 运动控制指令 (X/Y 平动、偏航、俯仰)
+   - ID 0x189: 攻击控制指令 (拨弹盘、小陀螺、摩擦轮)
+3. **数据接收**：接收并打印 CAN 总线上的数据
+4. **状态监控**：发布连接状态到 ROS 话题
+5. **环路测试**：支持双通道环路测试（通道0发，通道1收）
+
+## 编译
+
+```bash
+# 在工作空间根目录执行
+colcon build --packages-select amadeus_26
+```
+
+## 运行
+
+### 方式一：直接运行
+
+```bash
+# 设置环境
+source install/setup.bash
+
+# 运行节点（使用默认参数）
+ros2 run amadeus_26 transmitter_test_node
+
+# 或指定参数运行
+ros2 run amadeus_26 transmitter_test_node --ros-args \
+    -p tx_channel:=0 \
+    -p rx_channel:=1 \
+    -p can_id_motion:=329 \
+    -p can_id_attack:=393
+```
+
+### 方式二：使用 Launch 文件
+
+```bash
+# 设置环境
+source install/setup.bash
+
+# 使用默认参数启动
+ros2 launch amadeus_26 transmitter_test.launch.py
+
+# 或指定参数启动
+ros2 launch amadeus_26 transmitter_test.launch.py \
+    tx_channel:=0 \
+    rx_channel:=1 \
+    can_id_motion:=329 \
+    can_id_attack:=393 \
+    use_extended_frame:=false \
+    use_canfd:=false
+```
+
+## 参数说明
+
+| 参数名 | 类型 | 默认值 | 说明 |
+|--------|------|--------|------|
+| `send_frequency` | double | 50.0 | CAN 发送频率 (Hz) |
+| `tx_channel` | int | 0 | CAN 发送通道 (0 或 1) |
+| `rx_channel` | int | 1 | CAN 接收通道 (0 或 1) |
+| `can_id_motion` | int | 329 (0x149) | 运动控制 CAN ID |
+| `can_id_attack` | int | 393 (0x189) | 攻击控制 CAN ID |
+| `use_extended_frame` | bool | false | 是否使用扩展帧(29位ID) |
+| `use_canfd` | bool | false | 是否使用 CANFD |
+| `use_brs` | bool | false | CANFD 是否启用波特率切换 |
+| `rx_filter_enabled` | bool | false | 是否启用接收 ID 过滤 |
+| `rx_filter_id_min` | int | 0 | 接收过滤 ID 最小值 |
+| `rx_filter_id_max` | int | 2047 | 接收过滤 ID 最大值 |
+
+## 话题
+
+### 发布的话题
+
+- `/transmitter/connection_status` (std_msgs/Bool): USB2CAN 连接状态
+
+## CAN 协议
+
+### 运动控制 (ID: 0x149)
+
+数据格式 (8 bytes):
+```
+[0-1]: X轴平动 (左右), 范围 [-660, 660]
+[2-3]: Y轴平动 (前后), 范围 [-660, 660]
+[4-5]: 云台偏航, 范围 [-660, 660]
+[6-7]: 云台俯仰, 范围 [-660, 660]
+```
+
+### 攻击控制 (ID: 0x189)
+
+数据格式 (8 bytes):
+```
+[0-1]: 拨弹盘 (660=进弹, 0=关闭, -660=退弹)
+[2-3]: 小陀螺开关 (660=开启, 0=关闭)
+[4-5]: 摩擦轮开关 (660=开启, 0=关闭)
+[6-7]: 预留
+```
+
+## 环路测试
+
+用于调试收发功能，将两个 CAN 端口连接在一起：
+
+### 物理连接
+
+```
+USB2CAN 模块
+├── 通道0 (CAN0)  ──┐
+│   - CAN_H        │
+│   - CAN_L        ├── 短接在一起（H-H, L-L）
+├── 通道1 (CAN1)  ──┘
+    - CAN_H
+    - CAN_L
+```
+
+**注意**：需要在总线两端各加一个 **120Ω 终端电阻**！
+
+### 运行环路测试
+
+```bash
+ros2 launch amadeus_26 transmitter_test.launch.py \
+    tx_channel:=0 \
+    rx_channel:=1
+```
+
+### 预期输出
+
+```
+[发送回调] ID: 0x149
+[发送回调] ID: 0x189
+[接收回调] 通道1 ID: 0x149, 长度: 8, 数据: ...
+[接收回调] 通道1 ID: 0x189, 长度: 8, 数据: ...
+```
+
+## 注意事项
+
+1. **设备权限**：确保当前用户有权限访问 USB 设备
+   ```bash
+   sudo usermod -aG dialout $USER
+   # 重新登录后生效
+   ```
+
+2. **udev 规则**：设置 USB 设备权限规则
+   ```bash
+   sudo cp lib/transmitter_sdk/50-usb2can.rules /etc/udev/rules.d/
+   sudo udevadm control --reload-rules
+   sudo udevadm trigger
+   ```
+
+3. **终端电阻**：CAN 总线必须在两端各加 120Ω 终端电阻
+
+4. **波特率**：默认使用 1000000 bps (1Mbps)，确保所有设备波特率一致
+
+## 常见问题
+
+### Q: 节点启动时提示 "未找到 USB2CAN 设备"
+A: 检查设备是否连接，使用 `lsusb` 查看设备是否存在
+
+### Q: 运行时报错 "cannot open shared object file"
+A: 设置库文件路径：
+```bash
+export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$(pwd)/lib/transmitter_sdk/lib/linux/x64
+```
+
+### Q: 接收不到数据
+A: 
+1. 确认 CAN 线正确连接（H-H, L-L）
+2. 检查是否有 120Ω 终端电阻
+3. 确认波特率设置正确
+
+### Q: 发送回调突然停止
+A: 这是 CAN 协议的 ACK 机制。如果没有设备响应 ACK，发送会停止。确保：
+1. 总线上至少有一个设备能响应 ACK
+2. 或者进行环路测试（自己发自己收）

launch/transmitter_test.launch.py

@@ -0,0 +1,112 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    # 声明启动参数 - 基本设置
+    send_frequency_arg = DeclareLaunchArgument(
+        'send_frequency',
+        default_value='50.0',
+        description='CAN 发送频率 (Hz)'
+    )
+    
+    tx_channel_arg = DeclareLaunchArgument(
+        'tx_channel',
+        default_value='0',
+        description='CAN 发送通道 (0 或 1)'
+    )
+    
+    rx_channel_arg = DeclareLaunchArgument(
+        'rx_channel',
+        default_value='1',
+        description='CAN 接收通道 (0 或 1)'
+    )
+    
+    # 声明启动参数 - CAN ID 设置
+    can_id_motion_arg = DeclareLaunchArgument(
+        'can_id_motion',
+        default_value='329',  # 0x149 = 329
+        description='运动控制 CAN ID (十进制，默认 0x149=329)'
+    )
+    
+    can_id_attack_arg = DeclareLaunchArgument(
+        'can_id_attack',
+        default_value='393',  # 0x189 = 393
+        description='攻击控制 CAN ID (十进制，默认 0x189=393)'
+    )
+    
+    # 声明启动参数 - CAN 帧格式
+    use_extended_frame_arg = DeclareLaunchArgument(
+        'use_extended_frame',
+        default_value='false',
+        description='是否使用扩展帧(29位ID)，false=标准帧(11位)'
+    )
+    
+    use_canfd_arg = DeclareLaunchArgument(
+        'use_canfd',
+        default_value='false',
+        description='是否使用 CANFD，false=CAN2.0'
+    )
+    
+    use_brs_arg = DeclareLaunchArgument(
+        'use_brs',
+        default_value='false',
+        description='CANFD 是否启用波特率切换(BRS)'
+    )
+    
+    # 声明启动参数 - 接收过滤
+    rx_filter_enabled_arg = DeclareLaunchArgument(
+        'rx_filter_enabled',
+        default_value='false',
+        description='是否启用接收 ID 过滤'
+    )
+    
+    rx_filter_id_min_arg = DeclareLaunchArgument(
+        'rx_filter_id_min',
+        default_value='0',
+        description='接收过滤 ID 最小值'
+    )
+    
+    rx_filter_id_max_arg = DeclareLaunchArgument(
+        'rx_filter_id_max',
+        default_value='2047',  # 0x7FF
+        description='接收过滤 ID 最大值'
+    )
+
+    # 创建节点
+    transmitter_test_node = Node(
+        package='amadeus_26',
+        executable='transmitter_test_node',
+        name='transmitter_test_node',
+        output='screen',
+        parameters=[{
+            'send_frequency': LaunchConfiguration('send_frequency'),
+            'tx_channel': LaunchConfiguration('tx_channel'),
+            'rx_channel': LaunchConfiguration('rx_channel'),
+            'can_id_motion': LaunchConfiguration('can_id_motion'),
+            'can_id_attack': LaunchConfiguration('can_id_attack'),
+            'use_extended_frame': LaunchConfiguration('use_extended_frame'),
+            'use_canfd': LaunchConfiguration('use_canfd'),
+            'use_brs': LaunchConfiguration('use_brs'),
+            'rx_filter_enabled': LaunchConfiguration('rx_filter_enabled'),
+            'rx_filter_id_min': LaunchConfiguration('rx_filter_id_min'),
+            'rx_filter_id_max': LaunchConfiguration('rx_filter_id_max'),
+        }],
+    )
+
+    return LaunchDescription([
+        send_frequency_arg,
+        tx_channel_arg,
+        rx_channel_arg,
+        can_id_motion_arg,
+        can_id_attack_arg,
+        use_extended_frame_arg,
+        use_canfd_arg,
+        use_brs_arg,
+        rx_filter_enabled_arg,
+        rx_filter_id_min_arg,
+        rx_filter_id_max_arg,
+        transmitter_test_node,
+    ])

lib/transmitter_sdk/50-usb2can.rules

@@ -0,0 +1,14 @@
+# 达妙 USB2CAN 设备权限规则
+# 使用方法:
+# 1. 复制到 /etc/udev/rules.d/: sudo cp 50-usb2can.rules /etc/udev/rules.d/
+# 2. 重新加载规则: sudo udevadm control --reload-rules && sudo udevadm trigger
+# 3. 重新插拔设备或重启
+
+# 达妙 USB2CAN-DUAL (根据 lsusb 输出的 34b7:6632)
+SUBSYSTEM=="usb", ATTR{idVendor}=="34b7", ATTR{idProduct}=="6632", MODE="0666", GROUP="dialout"
+
+# 备用常见 VID:PID (STM32 虚拟串口等)
+SUBSYSTEM=="usb", ATTR{idVendor}=="0483", ATTR{idProduct}=="5740", MODE="0666", GROUP="dialout"
+
+# ACM 设备 (ttyACM*)
+KERNEL=="ttyACM[0-9]*", MODE="0666", GROUP="dialout"

package.xml

@@ -0,0 +1,21 @@
+<?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>Simba Robotics 2026 哨兵机器人识别、导航、决策算法</description>
+  <maintainer email="your-email@example.com">maintainer</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/transmitter_test_node.cpp

@@ -0,0 +1,378 @@
+/**
+ * @file transmitter_test_node.cpp
+ * @brief 收发模块测试节点 - 测试 USB2CAN 收发功能
+ * 
+ * 完全按照官方示例编写，SDK 自动查找 USB 设备
+ */
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <thread>
+#include <cstring>
+
+// 包含达妙 SDK 头文件
+extern "C" {
+#include "lib/transmitter_sdk/inc/pub_user.h"
+}
+
+// 默认 CAN ID 定义
+constexpr uint32_t DEFAULT_CAN_ID_MOTION = 0x149;   // 运动控制
+constexpr uint32_t DEFAULT_CAN_ID_ATTACK = 0x189;   // 攻击控制
+constexpr int CAN_BAUDRATE = 1000000;               // 1Mbps
+
+class TransmitterTestNode : public rclcpp::Node
+{
+public:
+    TransmitterTestNode() : Node("transmitter_test_node")
+    {
+        // 声明参数 - 通道设置
+        this->declare_parameter("send_frequency", 50.0);              // Hz
+        this->declare_parameter("tx_channel", 0);                     // 发送通道
+        this->declare_parameter("rx_channel", 1);                     // 接收通道
+        
+        // 声明参数 - CAN ID 设置 (发送)
+        this->declare_parameter("can_id_motion", 0x149);              // 运动控制 ID (默认 0x149)
+        this->declare_parameter("can_id_attack", 0x189);              // 攻击控制 ID (默认 0x189)
+        
+        // 声明参数 - CAN 帧格式设置
+        this->declare_parameter("use_extended_frame", false);         // 是否使用扩展帧 (29位ID)
+        this->declare_parameter("use_canfd", false);                  // 是否使用 CANFD
+        this->declare_parameter("use_brs", false);                    // CANFD 是否启用波特率切换
+        
+        // 声明参数 - CAN ID 过滤 (接收)
+        this->declare_parameter("rx_filter_enabled", false);          // 是否启用接收过滤
+        this->declare_parameter("rx_filter_id_min", 0x000);           // 接收 ID 最小值
+        this->declare_parameter("rx_filter_id_max", 0x7FF);           // 接收 ID 最大值
+        
+        // 获取参数 - 通道
+        send_frequency_ = this->get_parameter("send_frequency").as_double();
+        tx_channel_ = static_cast<uint8_t>(this->get_parameter("tx_channel").as_int());
+        rx_channel_ = static_cast<uint8_t>(this->get_parameter("rx_channel").as_int());
+        
+        // 获取参数 - CAN ID
+        can_id_motion_ = static_cast<uint32_t>(this->get_parameter("can_id_motion").as_int());
+        can_id_attack_ = static_cast<uint32_t>(this->get_parameter("can_id_attack").as_int());
+        
+        // 获取参数 - CAN 帧格式
+        use_extended_frame_ = this->get_parameter("use_extended_frame").as_bool();
+        use_canfd_ = this->get_parameter("use_canfd").as_bool();
+        use_brs_ = this->get_parameter("use_brs").as_bool();
+        
+        // 获取参数 - 接收过滤
+        rx_filter_enabled_ = this->get_parameter("rx_filter_enabled").as_bool();
+        rx_filter_id_min_ = static_cast<uint32_t>(this->get_parameter("rx_filter_id_min").as_int());
+        rx_filter_id_max_ = static_cast<uint32_t>(this->get_parameter("rx_filter_id_max").as_int());
+
+        RCLCPP_INFO(this->get_logger(), "=================================");
+        RCLCPP_INFO(this->get_logger(), "收发模块测试节点启动");
+        RCLCPP_INFO(this->get_logger(), "发送频率: %.1f Hz", send_frequency_);
+        RCLCPP_INFO(this->get_logger(), "发送通道: %d, 接收通道: %d", tx_channel_, rx_channel_);
+        RCLCPP_INFO(this->get_logger(), "发送 CAN ID - 运动: 0x%03X, 攻击: 0x%03X", can_id_motion_, can_id_attack_);
+        RCLCPP_INFO(this->get_logger(), "帧格式: %s, %s", 
+            use_extended_frame_ ? "扩展帧(29位)" : "标准帧(11位)",
+            use_canfd_ ? "CANFD" : "CAN2.0");
+        if (rx_filter_enabled_) {
+            RCLCPP_INFO(this->get_logger(), "接收过滤: 启用 [0x%03X - 0x%03X]", rx_filter_id_min_, rx_filter_id_max_);
+        } else {
+            RCLCPP_INFO(this->get_logger(), "接收过滤: 禁用 (接收所有 ID)");
+        }
+        RCLCPP_INFO(this->get_logger(), "=================================");
+
+        // 初始化设备
+        if (!initDevice()) {
+            RCLCPP_ERROR(this->get_logger(), "设备初始化失败，节点退出");
+            rclcpp::shutdown();
+            return;
+        }
+
+        // 创建发布者
+        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(&TransmitterTestNode::sendTestData, this));
+
+        // 创建定时器 - 发布连接状态
+        status_timer_ = this->create_wall_timer(
+            std::chrono::seconds(1),
+            std::bind(&TransmitterTestNode::publishStatus, this));
+
+        RCLCPP_INFO(this->get_logger(), "测试节点初始化完成");
+    }
+
+    ~TransmitterTestNode()
+    {
+        cleanup();
+    }
+
+private:
+    bool initDevice()
+    {
+        RCLCPP_INFO(this->get_logger(), "正在初始化 USB2CAN 设备...");
+
+        // 初始化模块句柄
+        damiao_handle_ = damiao_handle_create(DEV_USB2CANFD_DUAL);
+        if (!damiao_handle_) {
+            RCLCPP_ERROR(this->get_logger(), "创建句柄失败");
+            return false;
+        }
+
+        // 打印 SDK 版本
+        damiao_print_version(damiao_handle_);
+
+        // 查找对应类型模块的设备数量
+        RCLCPP_INFO(this->get_logger(), "正在查找设备...");
+        int device_cnt = damiao_handle_find_devices(damiao_handle_);
+
+        if (device_cnt == 0) {
+            RCLCPP_ERROR(this->get_logger(), "未找到 USB2CAN 设备！");
+            damiao_handle_destroy(damiao_handle_);
+            return false;
+        }
+
+        RCLCPP_INFO(this->get_logger(), "找到 %d 个设备", device_cnt);
+
+        int handle_cnt = 0;
+        device_handle* dev_list[16] = {nullptr};
+        
+        // 获取设备信息
+        RCLCPP_INFO(this->get_logger(), "正在获取设备列表...");
+        damiao_handle_get_devices(damiao_handle_, dev_list, &handle_cnt);
+
+        if (handle_cnt == 0 || dev_list[0] == nullptr) {
+            RCLCPP_ERROR(this->get_logger(), "获取设备列表失败");
+            damiao_handle_destroy(damiao_handle_);
+            return false;
+        }
+
+        // 使用第一个设备
+        device_ = dev_list[0];
+
+        // 打开设备
+        RCLCPP_INFO(this->get_logger(), "正在打开设备...");
+        if (device_open(device_)) {
+            RCLCPP_INFO(this->get_logger(), "设备已打开");
+        } else {
+            RCLCPP_ERROR(this->get_logger(), "打开设备失败！");
+            damiao_handle_destroy(damiao_handle_);
+            return false;
+        }
+
+        // 获取模块版本信息
+        char strBuf[256] = {0};
+        device_get_version(device_, strBuf, sizeof(strBuf));
+        RCLCPP_INFO(this->get_logger(), "设备版本: %s", strBuf);
+        
+        // 获取序列号信息
+        device_get_serial_number(device_, strBuf, sizeof(strBuf));
+        RCLCPP_INFO(this->get_logger(), "设备序列号: %s", strBuf);
+
+        // 设置发送通道波特率（简化版，自动计算采样点）
+        RCLCPP_INFO(this->get_logger(), "正在设置发送通道 %d 波特率 %d bps...", tx_channel_, CAN_BAUDRATE);
+        if (!device_channel_set_baud(device_, tx_channel_, use_canfd_, CAN_BAUDRATE, 0)) {
+            RCLCPP_ERROR(this->get_logger(), "设置发送通道波特率失败");
+        } else {
+            RCLCPP_INFO(this->get_logger(), "发送通道波特率设置成功");
+        }
+        
+        // 设置接收通道波特率（简化版，自动计算采样点）
+        RCLCPP_INFO(this->get_logger(), "正在设置接收通道 %d 波特率 %d bps...", rx_channel_, CAN_BAUDRATE);
+        if (!device_channel_set_baud(device_, rx_channel_, use_canfd_, CAN_BAUDRATE, 0)) {
+            RCLCPP_ERROR(this->get_logger(), "设置接收通道波特率失败");
+        } else {
+            RCLCPP_INFO(this->get_logger(), "接收通道波特率设置成功");
+        }
+
+        // 开启发送通道
+        RCLCPP_INFO(this->get_logger(), "正在打开发送通道 %d...", tx_channel_);
+        device_open_channel(device_, tx_channel_);
+        
+        // 开启接收通道
+        RCLCPP_INFO(this->get_logger(), "正在打开接收通道 %d...", rx_channel_);
+        device_open_channel(device_, rx_channel_);
+        
+        // 获取并验证波特率设置（在通道打开后）
+        device_baud_t baud = {0};
+        if (device_channel_get_baudrate(device_, tx_channel_, &baud)) {
+            RCLCPP_INFO(this->get_logger(), "发送通道 CAN 波特率: %d bps, CANFD 波特率: %d bps", 
+                baud.can_baudrate, baud.canfd_baudrate);
+        } else {
+            RCLCPP_WARN(this->get_logger(), "获取发送通道波特率失败");
+        }
+
+        // 注册回调
+        RCLCPP_INFO(this->get_logger(), "正在注册回调...");
+        device_hook_to_rec(device_, &TransmitterTestNode::receiveCallback);
+        device_hook_to_sent(device_, &TransmitterTestNode::sentCallback);
+        device_hook_to_err(device_, &TransmitterTestNode::errorCallback);
+
+        RCLCPP_INFO(this->get_logger(), "设备初始化成功");
+        RCLCPP_INFO(this->get_logger(), "=== 环路测试模式 ===");
+        RCLCPP_INFO(this->get_logger(), "请将通道%d和通道%d的CAN总线连接在一起", tx_channel_, rx_channel_);
+        is_connected_ = true;
+        return true;
+    }
+
+    void cleanup()
+    {
+        if (device_) {
+            // 关闭 CAN 通道
+            device_close_channel(device_, tx_channel_);
+            device_close_channel(device_, rx_channel_);
+            // 关闭设备
+            device_close(device_);
+        }
+        if (damiao_handle_) {
+            // 销毁模块句柄
+            damiao_handle_destroy(damiao_handle_);
+        }
+        RCLCPP_INFO(this->get_logger(), "设备已关闭");
+    }
+
+    void sendTestData()
+    {
+        static int test_counter = 0;
+        test_counter++;
+
+        // 发送运动控制指令 - 使用配置的运动控制 CAN ID
+        {
+            int16_t x_move = 0;    // 平动 X 轴 (左右)
+            int16_t y_move = 0;    // 平动 Y 轴 (前后)
+            int16_t yaw = 0;       // 云台偏航
+            int16_t pitch = 0;     // 云台俯仰
+
+            uint16_t data[8];
+            data[0] = (x_move >> 8) & 0xFF;
+            data[1] = x_move & 0xFF;
+            data[2] = (y_move >> 8) & 0xFF;
+            data[3] = y_move & 0xFF;
+            data[4] = (yaw >> 8) & 0xFF;
+            data[5] = yaw & 0xFF;
+            data[6] = (pitch >> 8) & 0xFF;
+            data[7] = pitch & 0xFF;
+
+            device_channel_send_fast(device_, tx_channel_, can_id_motion_, 1, 
+                use_extended_frame_, use_canfd_, use_brs_, 8, data);
+            
+            if (test_counter % 50 == 0) {
+                RCLCPP_INFO(this->get_logger(), 
+                    "通道%d 发送运动控制 [0x%03X]: X=%d, Y=%d, Yaw=%d, Pitch=%d", 
+                    tx_channel_, can_id_motion_, x_move, y_move, yaw, pitch);
+            }
+        }
+
+        // 发送攻击控制指令 - 使用配置的攻击控制 CAN ID
+        {
+            int16_t feed = 0;      // 拨弹轮
+            int16_t spin = 2;      // 小陀螺开启
+            int16_t friction = 2;  // 摩擦轮开启
+
+            uint16_t data[8];
+            data[0] = (feed >> 8) & 0xFF;
+            data[1] = feed & 0xFF;
+            data[2] = (spin >> 8) & 0xFF;
+            data[3] = spin & 0xFF;
+            data[4] = (friction >> 8) & 0xFF;
+            data[5] = friction & 0xFF;
+            data[6] = 0;
+            data[7] = 0;
+
+            device_channel_send_fast(device_, tx_channel_, can_id_attack_, 1, 
+                use_extended_frame_, use_canfd_, use_brs_, 8, data);
+            
+            if (test_counter % 50 == 0) {
+                RCLCPP_INFO(this->get_logger(), 
+                    "通道%d 发送攻击控制 [0x%03X]: Feed=%d, Spin=%d, Friction=%d", 
+                    tx_channel_, can_id_attack_, feed, spin, friction);
+            }
+        }
+    }
+
+    void publishStatus()
+    {
+        auto msg = std_msgs::msg::Bool();
+        msg.data = is_connected_;
+        connection_status_pub_->publish(msg);
+    }
+
+    // 发送帧回传回调函数
+    static void sentCallback(usb_rx_frame_t* frame)
+    {
+        if (!frame) return;
+        printf("[发送回调] ID: 0x%X\n", frame->head.can_id);
+    }
+
+    // 接收帧回传回调函数
+    static void receiveCallback(usb_rx_frame_t* frame)
+    {
+        if (!frame) return;
+        printf("[接收回调] 通道%d ID: 0x%X, 长度: %d, 数据: ", 
+               frame->head.channel, frame->head.can_id, frame->head.dlc);
+        for (int i = 0; i < frame->head.dlc; i++) {
+            printf("%02X ", frame->payload[i]);
+        }
+        printf("\n");
+    }
+
+    // 错误回调函数
+    static void errorCallback(usb_rx_frame_t* frame)
+    {
+        if (!frame) return;
+        printf("[错误回调] 通道%d ID: 0x%X, 错误类型: ", 
+               frame->head.channel, frame->head.can_id);
+        // 根据错误码判断错误类型
+        if (frame->head.esi) {
+            printf("错误状态指示(ESI) ");
+        }
+        if (frame->head.dir) {
+            printf("发送方向 ");
+        } else {
+            printf("接收方向 ");
+        }
+        printf("\n");
+    }
+
+    // 成员变量 - 通道和频率
+    double send_frequency_;
+    uint8_t tx_channel_;
+    uint8_t rx_channel_;
+    
+    // 成员变量 - CAN ID
+    uint32_t can_id_motion_;
+    uint32_t can_id_attack_;
+    
+    // 成员变量 - CAN 帧格式
+    bool use_extended_frame_;
+    bool use_canfd_;
+    bool use_brs_;
+    
+    // 成员变量 - 接收过滤
+    bool rx_filter_enabled_;
+    uint32_t rx_filter_id_min_;
+    uint32_t rx_filter_id_max_;
+    
+    // 成员变量 - 设备句柄
+    damiao_handle* damiao_handle_ = nullptr;
+    device_handle* device_ = nullptr;
+    bool is_connected_ = false;
+
+    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<TransmitterTestNode>();
+    
+    if (rclcpp::ok()) {
+        rclcpp::spin(node);
+    }
+    
+    rclcpp::shutdown();
+    return 0;
+}