VSAG/amadeus_26
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

帧定义实现，串口工具更新

Browse Source
This commit is contained in:
Huang Haoyu
2026-03-23 06:52:23 +08:00
parent 53aa847cc9
commit 3cf42d0f0d
17 changed files with 587 additions and 316 deletions
Download Patch File Download Diff File Expand all files Collapse all files

580
src/uart_transmitter_node.cpp
View File

@@ -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 <cstring>
#include <errno.h>
#include <fcntl.h>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/bool.hpp>
#include <serial/serial.h>
#include <termios.h>
#include <thread>
#include <cstring>
#include <unistd.h>
#include <vector>
// 帧定义
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<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(&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<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(&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<int16_t>(this->get_parameter("x_move").as_int());
int16_t y_move = static_cast<int16_t>(this->get_parameter("y_move").as_int());
int16_t yaw = static_cast<int16_t>(this->get_parameter("yaw").as_int());
int16_t pitch = static_cast<int16_t>(this->get_parameter("pitch").as_int());
int16_t feed = static_cast<int16_t>(this->get_parameter("feed").as_int());
uint8_t left_switch = static_cast<uint8_t>(this->get_parameter("left_switch").as_int()) & 0x0F;
uint8_t right_switch = static_cast<uint8_t>(this->get_parameter("right_switch").as_int()) & 0x0F;
// 构建数据帧
std::vector<uint8_t> 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<int16_t>(this->get_parameter("x_move").as_int());
int16_t y_move =
static_cast<int16_t>(this->get_parameter("y_move").as_int());
int16_t yaw = static_cast<int16_t>(this->get_parameter("yaw").as_int());
int16_t pitch = static_cast<int16_t>(this->get_parameter("pitch").as_int());
int16_t feed = static_cast<int16_t>(this->get_parameter("feed").as_int());
uint8_t left_switch =
static_cast<uint8_t>(this->get_parameter("left_switch").as_int()) &
0x0F;
uint8_t right_switch =
static_cast<uint8_t>(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<uint8_t> 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<uint8_t> 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<uint8_t> 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<std_msgs::msg::Bool>::SharedPtr connection_status_pub_;
rclcpp::TimerBase::SharedPtr send_timer_;
rclcpp::TimerBase::SharedPtr status_timer_;
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<UartTransmitterNode>();
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<UartTransmitterNode>();
if (rclcpp::ok()) {
rclcpp::spin(node);
}
rclcpp::shutdown();
return 0;
}