ROS2--Tiwst数据类型转换到帧类型发给STM32

上位机

串口通信节点读取cmd_vel话题数据：

cpp 复制代码

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

#include <functional>
#include <memory>
#include <cstdint>
#include <unistd.h>

// 假设 serial_fd 是已经打开好的串口文件描述符
int serial_fd = -1;

class CmdVelSerialNode : public rclcpp::Node
{
public:
    CmdVelSerialNode() : Node("cmd_vel_serial_node")
    {
        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.");
    }

private:
    void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
    {
        double vx = msg->linear.x;     // 前后线速度，单位 m/s
        double vy = msg->linear.y;     // 横向线速度，单位 m/s
        double wz = msg->angular.z;    // 旋转角速度，单位 rad/s

        sendCmdVel(vx, vy, wz);
    }

    void sendCmdVel(double vx, double vy, double wz)
    {
        int16_t vx_data = static_cast<int16_t>(vx * 1000.0);  // m/s → mm/s
        int16_t vy_data = static_cast<int16_t>(vy * 1000.0);
        int16_t wz_data = static_cast<int16_t>(wz * 1000.0);  // rad/s → mrad/s

        uint8_t frame[11];

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

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

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

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

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

        frame[10] = checksum;

        if (serial_fd >= 0)
        {
            write(serial_fd, frame, sizeof(frame));
        }
    }

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

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

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

    rclcpp::spin(node);

    rclcpp::shutdown();

    return 0;
}

流程：

cpp 复制代码

double 浮点数，通常 64 位
↓ 乘以 1000
double 浮点数
↓ static_cast<int16_t>
int16_t，16 位有符号整数
↓ 拆成两个 uint8_t
两个 8 位字节
↓ 串口逐字节发送

具体代码：

cpp 复制代码

void sendCmdVel(double vx, double vy, double wz)
{
    int16_t vx_data = static_cast<int16_t>(vx * 1000.0);  // m/s → mm/s
    int16_t vy_data = static_cast<int16_t>(vy * 1000.0);
    int16_t wz_data = static_cast<int16_t>(wz * 1000.0);  // rad/s → mrad/s

    uint8_t frame[11];

    frame[0] = 0xAA;   // 帧头1
    frame[1] = 0x55;   // 帧头2
    frame[2] = 0x07;   // 长度：命令字1字节 + 数据6字节
    frame[3] = 0x01;   // 命令字：速度控制命令

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

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

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

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

    // 这里调用串口发送函数
    write(serial_fd, frame, sizeof(frame));
}

解释：

/cmd_vel 里的速度经常是小数：

乘以1000保证 0.001位肯定时能保留下来的。

cpp 复制代码

wz = 0.500;   // rad/s
vx = 0.300;   // m/s

转换成16位，后面拆成两个8位发送

cpp 复制代码

int16_t wz_data = static_cast<int16_t>(wz * 1000.0);

意思是把 wz 从 double 类型转成 int16_t 类型，也就是 16 位有符号整数，不是 8 位整数。

举例：

cpp 复制代码

double wz = 0.500;   // 单位 rad/s

乘以 1000：

cpp 复制代码

0.500 × 1000 = 500

此时 wz_data 是 16 位整数。

500 的十六进制是：

cpp 复制代码

500 = 0x01F4

因为串口是一个字节一个字节发，所以要把这个 16 位数拆成两个 8 位字节

cpp 复制代码

frame[8] = wz_data & 0xFF;          // 低 8 位
frame[9] = (wz_data >> 8) & 0xFF;   // 高 8 位

结果是：

cpp 复制代码

wz_data = 0x01F4

低字节：0xF4
高字节：0x01

所以串口实际发送的是：

cpp 复制代码

F4 01

cpp 复制代码

wz_data = 0x01F4

低字节：0xF4
高字节：0x01

整个过程可以看成

cpp 复制代码

double 0.500
→ int16_t 500
→ uint8_t 0xF4 + uint8_t 0x01

/cmd_vel 中的 angular.z 原本是 double 类型的浮点数。我先将它乘以 1000，把 rad/s 转换成 mrad/s 对应的整数值，再强制转换成 int16_t。因为 int16_t 占 2 个字节，而串口按字节发送，所以再把这个 16 位整数拆成低字节和高字节，分别放到发送数组中。

为什么系数选择1000？

因为放大了 1000 倍，所以 int16_t 原本能表示的 32767，要反过来除以 1000，得到原始速度最大约 32.767。

这对于扫地机器人足够了。扫地机器人的速度从不可能达到两位数。

如果你不乘以 1000，直接转成 int16_t的结果：

因为整数不能保存小数部分，0.500 会被截断成 0。这样 STM32 收到后就以为角速度是 0，机器人就不旋转了。

下位机

流程

cpp 复制代码

USART 接收字节
→ 找帧头 AA 55
→ 收满 11 字节
→ 校验 checksum
→ 合成 vx、vy、wz
→ 除以 1000 恢复速度
→ 交给底盘控制任务

变量定义

cpp 复制代码

#include "main.h"
#include <stdint.h>

#define FRAME_HEAD_1    0xAA
#define FRAME_HEAD_2    0x55
#define CMD_VEL         0x01
#define FRAME_LEN       11

extern UART_HandleTypeDef huart1;   // 假设 USART1 接 NUC

uint8_t uart_rx_byte;               // 每次接收 1 个字节
uint8_t frame_buf[FRAME_LEN];        // 存放一整帧
uint8_t frame_index = 0;

volatile float target_vx = 0.0f;     // 前后速度，m/s
volatile float target_vy = 0.0f;     // 横向速度，m/s
volatile float target_wz = 0.0f;     // 旋转角速度，rad/s

volatile uint32_t last_cmd_time_ms = 0;

初始化时开启串口接收

cpp 复制代码

HAL_UART_Receive_IT(&huart1, &uart_rx_byte, 1);

让 USART1 开始中断接收，每次接收 1 个字节。

串口接收回调函数

cpp 复制代码

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if (huart->Instance == USART1)
    {
        Protocol_Parse_Byte(uart_rx_byte);

        // 重新开启下一字节接收
        HAL_UART_Receive_IT(&huart1, &uart_rx_byte, 1);
    }
}

HAL_UART_Receive_IT() 只接收一次，收到后要在回调里再次开启下一次接收。

按字节解析数据帧

cpp 复制代码

void Protocol_Parse_Byte(uint8_t byte)
{
    switch (frame_index)
    {
        case 0:
            if (byte == FRAME_HEAD_1)
            {
                frame_buf[frame_index++] = byte;
            }
            break;

        case 1:
            if (byte == FRAME_HEAD_2)
            {
                frame_buf[frame_index++] = byte;
            }
            else
            {
                frame_index = 0;
            }
            break;

        default:
            frame_buf[frame_index++] = byte;

            if (frame_index >= FRAME_LEN)
            {
                Protocol_Parse_Frame(frame_buf);
                frame_index = 0;
            }
            break;
    }
}

先找 0xAA

再找 0x55

找到帧头后继续接收后面的数据

收满 11 个字节后解析整帧

按字节解析数据帧

cpp 复制代码

void Protocol_Parse_Frame(uint8_t *frame)
{
    if (frame[0] != FRAME_HEAD_1 || frame[1] != FRAME_HEAD_2)
    {
        return;
    }

    uint8_t len = frame[2];
    uint8_t cmd = frame[3];

    if (len != 0x07)
    {
        return;
    }

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

    if (checksum != frame[10])
    {
        return;
    }

    if (cmd == CMD_VEL)
    {
        int16_t vx_data = (int16_t)((uint16_t)frame[4] | ((uint16_t)frame[5] << 8));
        int16_t vy_data = (int16_t)((uint16_t)frame[6] | ((uint16_t)frame[7] << 8));
        int16_t wz_data = (int16_t)((uint16_t)frame[8] | ((uint16_t)frame[9] << 8));

        target_vx = vx_data / 1000.0f;
        target_vy = vy_data / 1000.0f;
        target_wz = wz_data / 1000.0f;

        last_cmd_time_ms = HAL_GetTick();
    }
}

最关键的是这三句：

cpp 复制代码

int16_t vx_data = (int16_t)((uint16_t)frame[4] | ((uint16_t)frame[5] << 8));
int16_t vy_data = (int16_t)((uint16_t)frame[6] | ((uint16_t)frame[7] << 8));
int16_t wz_data = (int16_t)((uint16_t)frame[8] | ((uint16_t)frame[9] << 8));

底盘控制任务中使用：

cpp 复制代码

void Chassis_Control_Task(void)
{
    float vx = target_vx;
    float vy = target_vy;
    float wz = target_wz;

    // 1. 麦克纳姆轮逆运动学解算
    // vx, vy, wz → 四个轮子的目标速度

    // 2. 读取 C620 反馈的实际转速

    // 3. 四轮 PID 计算

    // 4. 通过 CAN 发送电流指令给 C620
}

通信超时保护

cpp 复制代码

void Chassis_Safety_Check(void)
{
    if (HAL_GetTick() - last_cmd_time_ms > 500)
    {
        target_vx = 0.0f;
        target_vy = 0.0f;
        target_wz = 0.0f;
    }
}

500 ms 没收到新的速度指令

→ 判断上位机通信超时

→ 目标速度清零

→ 底盘停车

串口通信节点

命令字如何决定的？

因为串口通信节点是从cmd_vel节点读取的数据所以，调用的是cmd_vel------callback函数

这个函数会固定填写命令字 = 0x01 速度控制。

如果是robot_modo节点，那么就是调用cmd_vel------callback函数。这个函数会固定填写命令字=0x02模式切换。