ROS2与STM32通信详解

介绍ROS2与STM32之间的通信实现，包括硬件连接、协议设计、代码实现等。

1. 硬件连接方案

1.1 物理连接方式

方案1：USB转TTL（推荐）
ROS2主机(USB) ↔ USB转TTL模块 ↔ STM32(UART引脚)

方案2：直接USB（如果STM32支持USB CDC）
ROS2主机(USB) ↔ STM32(USB接口)

方案3：无线通信
ROS2主机 ↔ WiFi/蓝牙模块 ↔ STM32(UART)

1.2 引脚连接示例（USB转TTL）

USB转TTL模块    STM32F103
    TX    →    PA3 (USART2_RX)
    RX    →    PA2 (USART2_TX)
    GND   →    GND
    5V/3.3V →  VCC (根据STM32电压)

2. 通信协议设计

2.1 数据帧格式设计

采用简单的文本协议，便于调试：

命令帧（ROS2 → STM32）

格式：CMD,<类型>,<参数1>,<参数2>,...,<校验和>\n
示例：CMD,VEL,0.2,0.5,0x3A\n

数据帧（STM32 → ROS2）

格式：DATA,<类型>,<参数1>,<参数2>,...,<校验和>\n 、
示例：DATA,ODOM,1.5,2.3,0.78,0x4B\n

2.2 消息类型定义

// 命令类型
#define MSG_CMD_VEL       "VEL"    // 速度控制
#define MSG_CMD_LED       "LED"    // LED控制
#define MSG_CMD_PID       "PID"    // PID参数设置

// 数据类型  
#define MSG_DATA_ODOM     "ODOM"   // 里程计数据
#define MSG_DATA_IMU      "IMU"    // IMU数据
#define MSG_DATA_STATUS   "STATUS" // 系统状态

2.3 校验和算法

简单的异或校验：

uint8_t calculate_checksum(const char* data, int len) {
    uint8_t checksum = 0;
    for(int i = 0; i < len; i++) {
        checksum ^= data[i];
    }
    return checksum;
}

3. STM32端实现

3.1 硬件初始化（HAL库）

// UART初始化
void MX_USART2_UART_Init(void) {
    huart2.Instance = USART2;
    huart2.Init.BaudRate = 115200;
    huart2.Init.WordLength = UART_WORDLENGTH_8B;
    huart2.Init.StopBits = UART_STOPBITS_1;
    huart2.Init.Parity = UART_PARITY_NONE;
    huart2.Init.Mode = UART_MODE_TX_RX;
    huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
    huart2.Init.OverSampling = UART_OVERSAMPLING_16;
    HAL_UART_Init(&huart2);
    
    // 启用串口中断
    HAL_UART_Receive_IT(&huart2, &rx_buffer, 1);
}

// 串口中断回调
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
    if(huart->Instance == USART2) {
        uart_rx_handler(rx_buffer);
        HAL_UART_Receive_IT(&huart2, &rx_buffer, 1);
    }
}

3.2 协议解析状态机

typedef enum {
    STATE_IDLE,
    STATE_RECEIVING,
    STATE_COMPLETE,
    STATE_ERROR
} uart_state_t;

typedef struct {
    uart_state_t state;
    char buffer[256];
    uint16_t index;
    uint8_t checksum;
} uart_parser_t;

void uart_rx_handler(uint8_t data) {
    static uart_parser_t parser = {0};
    
    switch(parser.state) {
        case STATE_IDLE:
            if(data == 'C' || data == 'D') { // 命令或数据起始
                parser.state = STATE_RECEIVING;
                parser.index = 0;
                parser.checksum = 0;
                parser.buffer[parser.index++] = data;
            }
            break;
            
        case STATE_RECEIVING:
            parser.buffer[parser.index++] = data;
            parser.checksum ^= data;
            
            if(data == '\n') { // 帧结束
                parser.buffer[parser.index] = '\0';
                parser.state = STATE_COMPLETE;
                process_received_frame(parser.buffer, parser.index, parser.checksum);
                parser.state = STATE_IDLE;
            }
            else if(parser.index >= sizeof(parser.buffer) - 1) {
                parser.state = STATE_ERROR;
            }
            break;
            
        default:
            parser.state = STATE_IDLE;
            break;
    }
}

3.3 命令处理函数

void process_received_frame(char* frame, int len, uint8_t recv_checksum) {
    // 验证校验和
    uint8_t calc_checksum = calculate_checksum(frame, len-1); // 排除\n
    if(calc_checksum != recv_checksum) {
        send_response("ERROR,CHECKSUM");
        return;
    }
    
    char* token = strtok(frame, ",");
    
    if(strcmp(token, "CMD") == 0) {
        token = strtok(NULL, ",");
        
        if(strcmp(token, "VEL") == 0) {
            // 解析速度命令: CMD,VEL,linear,angular
            float linear = atof(strtok(NULL, ","));
            float angular = atof(strtok(NULL, ","));
            
            motor_control(linear, angular);
            send_response("OK,VEL");
        }
        else if(strcmp(token, "LED") == 0) {
            // LED控制命令
            uint8_t led_state = atoi(strtok(NULL, ","));
            HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, led_state);
            send_response("OK,LED");
        }
    }
}

3.4 数据发送函数

void send_odometry_data(float x, float y, float theta, float vx, float vw) {
    char buffer[128];
    uint8_t checksum = 0;
    
    // 格式化数据
    int len = snprintf(buffer, sizeof(buffer)-10, "DATA,ODOM,%.3f,%.3f,%.3f,%.3f,%.3f", 
                      x, y, theta, vx, vw);
    
    // 计算校验和
    for(int i = 0; i < len; i++) {
        checksum ^= buffer[i];
    }
    
    // 添加校验和和换行
    len += snprintf(buffer + len, sizeof(buffer) - len, ",0x%02X\n", checksum);
    
    // 发送数据
    HAL_UART_Transmit(&huart2, (uint8_t*)buffer, len, 100);
}

void send_response(const char* response) {
    char buffer[64];
    uint8_t checksum = 0;
    int len = snprintf(buffer, sizeof(buffer)-10, "RESP,%s", response);
    
    for(int i = 0; i < len; i++) {
        checksum ^= buffer[i];
    }
    
    len += snprintf(buffer + len, sizeof(buffer) - len, ",0x%02X\n", checksum);
    HAL_UART_Transmit(&huart2, (uint8_t*)buffer, len, 100);
}

4. ROS2端实现

4.1 完整的STM32驱动节点

#!/usr/bin/env python3
import serial
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
import threading
import time

class STM32Driver(Node):
    def __init__(self):
        super().__init__('stm32_driver')
        
        # 参数配置
        self.declare_parameter('serial_port', '/dev/ttyUSB0')
        self.declare_parameter('baudrate', 115200)
        self.declare_parameter('timeout', 1.0)
        
        serial_port = self.get_parameter('serial_port').value
        baudrate = self.get_parameter('baudrate').value
        timeout = self.get_parameter('timeout').value
        
        # 串口初始化
        try:
            self.serial = serial.Serial(
                port=serial_port,
                baudrate=baudrate,
                timeout=timeout
            )
            self.get_logger().info(f'Connected to STM32 on {serial_port}')
        except Exception as e:
            self.get_logger().error(f'Failed to open serial port: {e}')
            return
        
        # 发布者
        self.odom_pub = self.create_publisher(Odometry, 'odom', 10)
        self.imu_pub = self.create_publisher(Imu, 'imu', 10)
        
        # 订阅者
        self.cmd_vel_sub = self.create_subscription(
            Twist, 'cmd_vel', self.cmd_vel_callback, 10)
        
        # TF广播
        self.odom_broadcaster = tf2_ros.TransformBroadcaster(self)
        
        # 串口读取线程
        self.read_thread = threading.Thread(target=self.serial_read_loop)
        self.read_thread.daemon = True
        self.read_thread.start()
        
        # 定时发送状态查询
        self.timer = self.create_timer(0.1, self.query_status)
        
        self.get_logger().info('STM32 driver node started')
    
    def cmd_vel_callback(self, msg):
        """处理速度命令"""
        # 限制速度范围
        linear_x = max(min(msg.linear.x, 1.0), -1.0)
        angular_z = max(min(msg.angular.z, 3.14), -3.14)
        
        # 发送命令到STM32
        cmd = f"CMD,VEL,{linear_x:.3f},{angular_z:.3f}"
        self.send_command(cmd)
    
    def send_command(self, command):
        """发送命令到STM32"""
        try:
            # 计算校验和
            checksum = 0
            for c in command:
                checksum ^= ord(c)
            
            # 构建完整命令
            full_cmd = f"{command},0x{checksum:02X}\n"
            self.serial.write(full_cmd.encode())
            
        except Exception as e:
            self.get_logger().error(f'Failed to send command: {e}')
    
    def query_status(self):
        """查询STM32状态"""
        self.send_command("CMD,STATUS")
    
    def serial_read_loop(self):
        """串口读取线程"""
        buffer = ""
        while rclpy.ok():
            try:
                if self.serial.in_waiting > 0:
                    data = self.serial.read(self.serial.in_waiting).decode()
                    buffer += data
                    
                    # 处理完整的数据行
                    while '\n' in buffer:
                        line, buffer = buffer.split('\n', 1)
                        line = line.strip()
                        if line:
                            self.process_serial_data(line)
                
                time.sleep(0.01)
                
            except Exception as e:
                self.get_logger().error(f'Serial read error: {e}')
                time.sleep(1)
    
    def process_serial_data(self, data):
        """处理从STM32接收的数据"""
        try:
            parts = data.split(',')
            if len(parts) < 3:
                return
            
            msg_type = parts[0]
            data_type = parts[1]
            
            # 验证校验和
            if parts[-1].startswith('0x'):
                recv_checksum = int(parts[-1][2:], 16)
                data_without_checksum = ','.join(parts[:-1])
                
                calc_checksum = 0
                for c in data_without_checksum:
                    calc_checksum ^= ord(c)
                
                if calc_checksum != recv_checksum:
                    self.get_logger().warn(f'Checksum error: {data}')
                    return
            
            if msg_type == "DATA":
                if data_type == "ODOM" and len(parts) >= 7:
                    self.publish_odometry(float(parts[2]), float(parts[3]), 
                                         float(parts[4]), float(parts[5]), float(parts[6]))
                
                elif data_type == "IMU" and len(parts) >= 9:
                    self.publish_imu(float(parts[2]), float(parts[3]), float(parts[4]),
                                   float(parts[5]), float(parts[6]), float(parts[7]),
                                   float(parts[8]))
            
            elif msg_type == "RESP":
                self.get_logger().info(f'STM32 response: {data_type}')
                
        except Exception as e:
            self.get_logger().error(f'Data processing error: {e}')
    
    def publish_odometry(self, x, y, theta, vx, vw):
        """发布里程计数据"""
        odom_msg = Odometry()
        odom_msg.header.stamp = self.get_clock().now().to_msg()
        odom_msg.header.frame_id = "odom"
        odom_msg.child_frame_id = "base_footprint"
        
        # 位置
        odom_msg.pose.pose.position.x = x
        odom_msg.pose.pose.position.y = y
        
        # 方向（四元数）
        import math
        from geometry_msgs.msg import Quaternion
        cy = math.cos(theta * 0.5)
        sy = math.sin(theta * 0.5)
        odom_msg.pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=sy, w=cy)
        
        # 速度
        odom_msg.twist.twist.linear.x = vx
        odom_msg.twist.twist.angular.z = vw
        
        self.odom_pub.publish(odom_msg)
        
        # 发布TF变换
        self.publish_odom_tf(x, y, theta)
    
    def publish_odom_tf(self, x, y, theta):
        """发布odom到base_footprint的TF变换"""
        from geometry_msgs.msg import TransformStamped
        import math
        
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = "odom"
        t.child_frame_id = "base_footprint"
        
        t.transform.translation.x = x
        t.transform.translation.y = y
        t.transform.translation.z = 0.0
        
        cy = math.cos(theta * 0.5)
        sy = math.sin(theta * 0.5)
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = sy
        t.transform.rotation.w = cy
        
        self.odom_broadcaster.sendTransform(t)
    
    def publish_imu(self, ax, ay, az, gx, gy, gz, yaw):
        """发布IMU数据"""
        imu_msg = Imu()
        imu_msg.header.stamp = self.get_clock().now().to_msg()
        imu_msg.header.frame_id = "imu_link"
        
        # 线性加速度
        imu_msg.linear_acceleration.x = ax
        imu_msg.linear_acceleration.y = ay
        imu_msg.linear_acceleration.z = az
        
        # 角速度
        imu_msg.angular_velocity.x = gx
        imu_msg.angular_velocity.y = gy
        imu_msg.angular_velocity.z = gz
        
        self.imu_pub.publish(imu_msg)

def main():
    rclpy.init()
    node = STM32Driver()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

4.2 启动文件配置

创建stm32_bringup.launch.py：

from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    return LaunchDescription([
        # STM32驱动节点
        Node(
            package='stm32_driver',
            executable='stm32_driver_node',
            name='stm32_driver',
            parameters=[{
                'serial_port': '/dev/ttyUSB0',
                'baudrate': 115200,
                'timeout': 1.0
            }]
        ),
        
        # 激光雷达节点
        Node(
            package='sllidar_ros2',
            executable='sllidar_node',
            name='sllidar_node',
            parameters=[{
                'serial_port': '/dev/ttyUSB1',  # 假设雷达在另一个USB口
                'baudrate': 256000
            }]
        ),
        
        # 静态TF：base_footprint → laser
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments=['0.1', '0', '0.2', '0', '0', '0', 'base_footprint', 'laser']
        ),
        
        # 静态TF：base_footprint → imu_link  
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments=['0', '0', '0.1', '0', '0', '0', 'base_footprint', 'imu_link']
        )
    ])

5. 调试和测试

5.1 测试步骤

1. 硬件测试：用串口调试工具验证STM32通信

2. 协议测试：手动发送命令测试协议解析

3. 节点测试：单独测试STM32驱动节点

4. 集成测试：与导航栈集成测试

5.2 调试工具

# 查看串口设备
ls /dev/ttyUSB*

# 测试串口通信（安装minicom）
sudo apt install minicom
minicom -D /dev/ttyUSB0 -b 115200

# 查看ROS2话题
ros2 topic list
ros2 topic echo /odom
ros2 topic pub /cmd_vel geometry_msgs/Twist ...