初步更改
This commit is contained in:
309
src/uart_transmitter_node.cpp
Normal file
309
src/uart_transmitter_node.cpp
Normal file
@@ -0,0 +1,309 @@
|
||||
/**
|
||||
* @file uart_transmitter_node.cpp
|
||||
* @brief UART 串口收发模块测试节点 (CH340)
|
||||
*
|
||||
* 使用 CH340 USB 转串口模块发送控制指令
|
||||
* 波特率：921600 (与裁判系统一致)
|
||||
* 协议帧格式：
|
||||
* | 帧头1 | 帧头2 | 平动左右 | 平动前后 | 云台偏航 | 云台俯仰 | 拨弹轮 | 拨杆 | CRC8 | 帧尾 |
|
||||
* | 0xAA | 0x55 | 2 bytes | 2 bytes | 2 bytes | 2 bytes | 2 bytes|1 byte| 1byte| 0xFF |
|
||||
*/
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <std_msgs/msg/bool.hpp>
|
||||
#include <serial/serial.h>
|
||||
#include <thread>
|
||||
#include <cstring>
|
||||
#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 const char* DEFAULT_SERIAL_PORT = "/dev/ttyUSB0";
|
||||
constexpr int DEFAULT_BAUDRATE = 115200;
|
||||
|
||||
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]
|
||||
|
||||
// 获取参数
|
||||
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();
|
||||
|
||||
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(), "=================================");
|
||||
|
||||
// 初始化串口
|
||||
if (!initSerial()) {
|
||||
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(&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()
|
||||
{
|
||||
if (receive_thread_.joinable()) {
|
||||
running_ = false;
|
||||
receive_thread_.join();
|
||||
}
|
||||
if (serial_.isOpen()) {
|
||||
serial_.close();
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
void sendControlFrame()
|
||||
{
|
||||
if (!serial_.isOpen()) {
|
||||
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;
|
||||
|
||||
// 构建数据帧
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
std::vector<uint8_t> buffer;
|
||||
buffer.reserve(FRAME_LENGTH);
|
||||
|
||||
while (running_ && rclcpp::ok()) {
|
||||
if (!serial_.isOpen()) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
continue;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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_;
|
||||
};
|
||||
|
||||
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;
|
||||
}
|
||||
Reference in New Issue
Block a user