节点流程

订阅 /cmd_vel
↓
取出 vx、vy、wz
↓
乘以 1000 转成 int16_t
↓
打包成 UART 数据帧
↓
通过 /dev/serial/by-id/... 或 /dev/ttyUSB0 发给 STM32

进入工作空间创建包，需要注意的是，加入geometry_msgs，因为twist是这个包里的

cd ~/ros2_ws/src

ros2 pkg create stm32_serial_bridge \
  --build-type ament_cmake \
  --dependencies rclcpp geometry_msgs

进入包的源码文件夹创建节点

cd ~/ros2_ws/src/stm32_serial_bridge/src
touch cmd_vel_serial_node.cpp

串口通信节点具体代码

包名：stm32_serial_bridge

节点名：cmd_vel_serial_node.cpp

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"

#include <termios.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>

#include <cmath>
#include <cstdint>
#include <cstring>
#include <string>
#include <mutex>
#include <algorithm>

class CmdVelSerialNode : public rclcpp::Node
{
public:
    CmdVelSerialNode() : Node("cmd_vel_serial_node")
    {
        this->declare_parameter<std::string>("serial_port", "/dev/ttyUSB0");
        this->declare_parameter<int>("baud_rate", 115200);

        serial_port_ = this->get_parameter("serial_port").as_string();
        baud_rate_ = this->get_parameter("baud_rate").as_int();

        if (!openSerialPort(serial_port_, baud_rate_))
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to open serial port: %s", serial_port_.c_str());
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Serial port opened: %s, baud rate: %d",
                        serial_port_.c_str(), baud_rate_);
        }

        cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
            "/cmd_vel",
            10,
            std::bind(&CmdVelSerialNode::cmdVelCallback, this, std::placeholders::_1)
        );

        RCLCPP_INFO(this->get_logger(), "cmd_vel_serial_node started, waiting for /cmd_vel...");
    }

    ~CmdVelSerialNode()
    {
        if (serial_fd_ >= 0)
        {
            close(serial_fd_);
            serial_fd_ = -1;
        }
    }

private:
    int serial_fd_ = -1;
    std::string serial_port_;
    int baud_rate_;
    std::mutex serial_mutex_;

    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;

private:
    speed_t getBaudRate(int baud_rate)
    {
        switch (baud_rate)
        {
            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(), "Unsupported baud rate %d, use 115200 instead.", baud_rate);
                return B115200;
        }
    }

    bool openSerialPort(const std::string &port_name, int baud_rate)
    {
        serial_fd_ = open(port_name.c_str(), O_RDWR | O_NOCTTY);

        if (serial_fd_ < 0)
        {
            RCLCPP_ERROR(this->get_logger(), "open() failed: %s", strerror(errno));
            return false;
        }

        struct termios tty;
        memset(&tty, 0, sizeof(tty));

        if (tcgetattr(serial_fd_, &tty) != 0)
        {
            RCLCPP_ERROR(this->get_logger(), "tcgetattr() failed: %s", strerror(errno));
            close(serial_fd_);
            serial_fd_ = -1;
            return false;
        }

        cfmakeraw(&tty);

        speed_t speed = getBaudRate(baud_rate);
        cfsetispeed(&tty, speed);
        cfsetospeed(&tty, speed);

        tty.c_cflag |= CLOCAL;
        tty.c_cflag |= CREAD;

        tty.c_cflag &= ~CSIZE;
        tty.c_cflag |= CS8;

        tty.c_cflag &= ~PARENB;
        tty.c_cflag &= ~CSTOPB;
        tty.c_cflag &= ~CRTSCTS;

        tty.c_cc[VMIN] = 0;
        tty.c_cc[VTIME] = 1;

        tcflush(serial_fd_, TCIOFLUSH);

        if (tcsetattr(serial_fd_, TCSANOW, &tty) != 0)
        {
            RCLCPP_ERROR(this->get_logger(), "tcsetattr() failed: %s", strerror(errno));
            close(serial_fd_);
            serial_fd_ = -1;
            return false;
        }

        return true;
    }

    int16_t speedToInt16(double value)
    {
        double scaled = value * 1000.0;

        scaled = std::clamp(scaled, -32768.0, 32767.0);

        return static_cast<int16_t>(std::round(scaled));
    }

    void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
    {
        double vx = msg->linear.x;
        double vy = msg->linear.y;
        double wz = msg->angular.z;

        RCLCPP_INFO_THROTTLE(
            this->get_logger(),
            *this->get_clock(),
            1000,
            "recv /cmd_vel: vx=%.3f m/s, vy=%.3f m/s, wz=%.3f rad/s",
            vx, vy, wz
        );

        sendCmdVelFrame(vx, vy, wz);
    }

    void sendCmdVelFrame(double vx, double vy, double wz)
    {
        if (serial_fd_ < 0)
        {
            RCLCPP_WARN_THROTTLE(
                this->get_logger(),
                *this->get_clock(),
                1000,
                "Serial port not opened."
            );
            return;
        }

        int16_t vx_data = speedToInt16(vx);
        int16_t vy_data = speedToInt16(vy);
        int16_t wz_data = speedToInt16(wz);

        uint8_t frame[11];

        frame[0] = 0xAA;
        frame[1] = 0x55;
        frame[2] = 0x07;
        frame[3] = 0x01;

        frame[4] = static_cast<uint8_t>(vx_data & 0xFF);
        frame[5] = static_cast<uint8_t>((vx_data >> 8) & 0xFF);

        frame[6] = static_cast<uint8_t>(vy_data & 0xFF);
        frame[7] = static_cast<uint8_t>((vy_data >> 8) & 0xFF);

        frame[8] = static_cast<uint8_t>(wz_data & 0xFF);
        frame[9] = static_cast<uint8_t>((wz_data >> 8) & 0xFF);

        uint8_t checksum = 0;
        for (int i = 2; i <= 9; i++)
        {
            checksum += frame[i];
        }

        frame[10] = checksum;

        std::lock_guard<std::mutex> lock(serial_mutex_);

        ssize_t write_len = write(serial_fd_, frame, sizeof(frame));

        if (write_len != static_cast<ssize_t>(sizeof(frame)))
        {
            RCLCPP_WARN(this->get_logger(),
                        "Serial write incomplete. expected=%ld, actual=%ld, errno=%s",
                        sizeof(frame), write_len, strerror(errno));
        }
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<CmdVelSerialNode>();

    rclcpp::spin(node);

    rclcpp::shutdown();

    return 0;
}

launch脚本（重点）

注意ROS2中不再使用xml文件

而是使用.py文件写launch 

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='stm32_serial_bridge',
            executable='cmd_vel_serial_node',
            name='cmd_vel_serial_node',
            output='screen',
            parameters=[{
                'serial_port': '/dev/serial/by-id/usb-1a86_USB_Serial-if00-port0',
                'baud_rate': 115200
            }]
        )
    ])

串口通信失败怎么排查？

我会按从底层到上层的顺序排查。

第一步，看 NUC 是否识别到 USB-TTL，比如使用 ls /dev/ttyUSB* 或 dmesg | grep tty。

第二步，检查 TX/RX 是否交叉连接，GND 是否共地，电平是否匹配。

第三步，绕开 ROS2 和复杂协议，先让 STM32 周期性打印固定字符串，看 NUC 能不能收到；再让 NUC 发送字符，看 STM32 能不能响应。

第四步，检查波特率、数据位、停止位、校验位和流控设置是否一致。

第五步，再检查自定义协议，包括帧头、长度、校验、大小端、数据类型和粘包半包处理。

最后才检查 ROS2 串口通信节点，比如设备名、权限、节点是否运行、串口是否被占用。

先验证物理链路，再验证基础收发，最后再查协议和 ROS2 应用层。

ROS2串口通信节点排查：

设备名排查：

在设备名排查时，我不会直接依赖 /dev/ttyUSB0，因为它是 Linux 按 USB 枚举顺序动态分配的，重启或插拔后可能变化。我会在 launch 文件中把串口设备作为 serial_port 参数传给串口通信节点，参数值使用 udev 生成的持久化设备路径，比如 /dev/serial/by-id/。这样即使底层设备节点从 ttyUSB0 变成 ttyUSB1，串口节点仍然能打开正确的 STM32 串口。

比如我用的是CH340

输入：

ls -l /dev/serial/by-id/

输出类似：

usb-1a86_USB_Serial-if00-port0 -> ../../ttyUSB0

那你在 launch 或 yaml 里就写：

serial_port: /dev/serial/by-id/usb-1a86_USB_Serial-if00-port0  （符号链接名称）
baud_rate: 115200

面试回答：

例如我使用的是 CH340 USB-TTL 模块，插入 Linux 后，不直接使用 /dev/ttyUSB0，而是先通过 ls -l /dev/serial/by-id/ 查看 udev 生成的持久化设备路径。假设系统显示 usb-1a86_USB_Serial-if00-port0 -> ../../ttyUSB0，那么我在 ROS2 launch 或 yaml 参数中会把串口参数写成 /dev/serial/by-id/usb-1a86_USB_Serial-if00-port0。这样即使重启后底层设备节点从 /dev/ttyUSB0 变成 /dev/ttyUSB1，这个 by-id 符号链接仍然会指向同一个 USB-TTL 设备，串口通信节点就不会连错设备。

注意：

/dev/serial/by-id/... 后面的内容不是自己随便编的，而是用 ls -l /dev/serial/by-id/ 查出来的设备符号链接名称。

节点是否运行 + 是否正确订阅节点：

直接用这个图形化界面查看

ros2 run rqt_graph rqt_graph

write() 有没有真的把数据写出去检查

write() 返回成功，只能说明数据已经成功写入 Linux 串口驱动的发送缓冲区，不一定等于 STM32 已经收到并解析成功。

1. 软件层：write() 返回值是否等于发送字节数
2. 驱动层：数据是否从串口发送缓冲区排空
3. 硬件/下位机层：STM32 是否真的收到并解析成功

这里主要流程如下

ROS2 串口节点 write()
↓
Linux 串口驱动发送
↓
USB-TTL TX
↓
STM32 UART RX
↓
STM32 协议解析成功
↓
STM32 回 ACK
↓
ROS2 串口节点收到 ACK

1.检查 write() 返回值

n > 0 ：成功写入了 n 个字节
n == 0 ：这次没有写入任何字节，比较少见
n == -1 ：写入失败，要看 errno

ssize_t n = write(serial_fd_, frame, sizeof(frame));

if (n < 0)
{
    RCLCPP_ERROR(this->get_logger(), "write failed: %s", strerror(errno));
}
else if (n != static_cast<ssize_t>(sizeof(frame)))
{
    RCLCPP_WARN(this->get_logger(),
                "write incomplete: expected=%ld, actual=%ld",
                sizeof(frame), n);
}
else
{
    RCLCPP_INFO(this->get_logger(), "write ok: %ld bytes", n);
}

如果输出：

write ok: 11 bytes

说明这 11 个字节已经被 Linux 接收进串口发送缓冲区。

如果返回：

write failed

说明根本没写成功。

如果返回：

write incomplete: expected=11, actual=5

2.工程上建议写成 writeAll()

在write外面再套一层

可以写一个完整发送函数：

bool writeAll(int fd, const uint8_t *data, size_t length)
{
    size_t total_written = 0;

    while (total_written < length)
    {
        ssize_t n = write(fd, data + total_written, length - total_written);

        if (n < 0)
        {
            if (errno == EINTR)
            {
                continue;
            }

            RCLCPP_ERROR(rclcpp::get_logger("serial"),
                         "write failed: %s", strerror(errno));
            return false;
        }

        if (n == 0)
        {
            RCLCPP_WARN(rclcpp::get_logger("serial"),
                        "write returned 0 byte.");
            return false;
        }

        total_written += static_cast<size_t>(n);
    }

    return true;
}

发送时：

bool ok = writeAll(serial_fd_, frame, sizeof(frame));

if (ok)
{
    RCLCPP_INFO(this->get_logger(), "serial write ok: 11 bytes");
}

这样比直接 write() 更可靠。

3.使用 tcdrain() 确认发送缓冲区排空

bool ok = writeAll(serial_fd_, frame, sizeof(frame));

if (ok)
{
    tcdrain(serial_fd_);
    RCLCPP_INFO(this->get_logger(), "serial frame drained to UART driver.");
}

但是注意，它仍然只能说明上位机串口侧已经尽量发出，不代表 STM32 一定解析成功。

4.让 STM32 回 ACK，最可靠

面试回答：

write() 返回 11 只能说明写入上位机串口缓冲区成功；tcdrain() 说明上位机侧发送完成；STM32 回 ACK 才能证明下位机真正收到并解析成功。

我会分层验证 write() 是否真的把数据发出去。首先在 ROS2 串口节点中检查 write() 的返回值，因为 write() 返回值表示实际写入串口驱动缓冲区的字节数。如果我设计的一帧速度命令是 11 字节，那么返回值必须等于 11，才说明这一帧在软件层写入成功。如果返回负数，就根据 errno 判断是否是权限、设备断开或串口异常；如果返回值小于 11，就说明发生了不完整写入，需要继续发送剩余字节。

其次，我可以在 write() 后调用 tcdrain()，等待串口发送缓冲区排空，进一步确认数据已经从上位机串口侧发出。同时我会打印发送帧的十六进制内容，确认帧头、长度、命令字、速度数据和 checksum 都正确。

但是 write() 成功并不等于 STM32 一定收到。因此工程上更可靠的做法是让 STM32 在成功解析数据帧后回传 ACK，上位机收到 ACK 后，才能确认数据完成了从 ROS2 节点到 STM32 协议解析的闭环。如果还要验证物理层，可以用逻辑分析仪或示波器查看 USB-TTL 的 TX 引脚是否真的有 UART 波形。

STM32回ACK的问题:

为什么不建议 /cmd_vel 每帧都 ACK？

NUC 发速度帧
↓
STM32 回 ACK
↓
NUC 再发下一帧

这样就变成

串口通信量增加
通信延迟增加
上位机发送逻辑变复杂
如果等 ACK 再发，可能影响速度实时性
旧速度帧 ACK 回来时，速度命令可能已经过时

所以对于 /cmd_vel 这种实时速度流，工程上通常不要求每帧 ACK。

更常见做法是：

NUC 周期性发送 /cmd_vel
STM32 持续接收最新速度
STM32 只保留最新目标速度
如果超过一定时间没收到新速度，就自动停车

比如：

超过 500 ms 没有收到速度指令
↓
STM32 Safety_Task 清零 target_vx、target_vy、target_wz
↓
发送 0 电流给 C620
↓
底盘停车

哪些命令适合 ACK？

模式切换命令
急停命令
清除故障命令
参数设置命令
进入建图/导航/回充模式命令
请求版本号/设备状态命令

比如模式切换：

NUC 发送：切换到自动导航模式
STM32 回 ACK：模式切换成功 / 失败

急停命令：

NUC 发送：急停
STM32 回 ACK：已进入急停状态

参数设置命令：

NUC 发送：修改 PID 参数
STM32 回 ACK：参数写入成功 / 校验失败