/** * @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; // 云台俯仰 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::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; }