378 lines
14 KiB
C++
378 lines
14 KiB
C++
/**
|
||
* @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;
|
||
} |