From f2ebd8e78ac7c6e16f19c0edd69a2f7f77016f9e Mon Sep 17 00:00:00 2001 From: HelixCopex Date: Mon, 23 Mar 2026 02:10:54 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=B6=E5=8F=91=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 71 +++++ CMakeLists.txt | 56 ++++ docs/TRANSMITTER_TEST.md | 178 +++++++++++++ launch/transmitter_test.launch.py | 112 ++++++++ lib/transmitter_sdk/50-usb2can.rules | 14 + package.xml | 21 ++ src/transmitter_test_node.cpp | 378 +++++++++++++++++++++++++++ 7 files changed, 830 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 docs/TRANSMITTER_TEST.md create mode 100644 launch/transmitter_test.launch.py create mode 100644 lib/transmitter_sdk/50-usb2can.rules create mode 100644 package.xml create mode 100644 src/transmitter_test_node.cpp diff --git a/.gitignore b/.gitignore index e69de29..46f11fc 100644 --- a/.gitignore +++ b/.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 \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..e781779 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/docs/TRANSMITTER_TEST.md b/docs/TRANSMITTER_TEST.md new file mode 100644 index 0000000..b01df10 --- /dev/null +++ b/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. 或者进行环路测试（自己发自己收） \ No newline at end of file diff --git a/launch/transmitter_test.launch.py b/launch/transmitter_test.launch.py new file mode 100644 index 0000000..d483756 --- /dev/null +++ b/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, + ]) \ No newline at end of file diff --git a/lib/transmitter_sdk/50-usb2can.rules b/lib/transmitter_sdk/50-usb2can.rules new file mode 100644 index 0000000..d45f7a0 --- /dev/null +++ b/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" \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..42f6997 --- /dev/null +++ b/package.xml @@ -0,0 +1,21 @@ + + + + amadeus_26 + 0.0.1 + Simba Robotics 2026 哨兵机器人识别、导航、决策算法 + maintainer + MIT + + ament_cmake + + rclcpp + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/src/transmitter_test_node.cpp b/src/transmitter_test_node.cpp new file mode 100644 index 0000000..4c129cd --- /dev/null +++ b/src/transmitter_test_node.cpp @@ -0,0 +1,378 @@ +/** + * @file transmitter_test_node.cpp + * @brief 收发模块测试节点 - 测试 USB2CAN 收发功能 + * + * 完全按照官方示例编写，SDK 自动查找 USB 设备 + */ + +#include +#include +#include +#include + +// 包含达妙 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(this->get_parameter("tx_channel").as_int()); + rx_channel_ = static_cast(this->get_parameter("rx_channel").as_int()); + + // 获取参数 - CAN ID + can_id_motion_ = static_cast(this->get_parameter("can_id_motion").as_int()); + can_id_attack_ = static_cast(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(this->get_parameter("rx_filter_id_min").as_int()); + rx_filter_id_max_ = static_cast(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("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(&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; // 云台俯仰 + + uint8_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; // 摩擦轮开启 + + uint8_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::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; +} \ No newline at end of file