ROS2串口通信实现飞控IMU消息与STM32点灯的简单节点控制

环境

树莓派4B
ArduCopter 4.0.7
Ubuntu mate 22.04
ROS2 Humble
(本人搭建所使用的软件硬件版本，仅供参考)

零：项目目标

在实际机器人项目中，机载电脑+单片机是一个常用的组合，二者能够实现优势互补，适用于各种应用场景。

本文打通了ROS2与stm32之间的串口通信链路，加之前面文章实现的Mavros数据读取，实际上实现了树莓派，单片机和飞控三者之间的相互通信。

预期实现的效果是，如果飞控的横滚角和俯仰角的绝对值均小于30°，则通过串口向stm32发送一个字符‘A’，stm32识别到以后，PC13引脚上接的板载LED被点亮，发出蓝光；否则，发送字符‘B’，stm32上的LED不亮。

一：单片机程序编写

在stm32中只需要接收并判断串口发来的字符，打开或关闭板载的LED灯即可，逻辑非常简单。

#include "stm32f10x.h"              
#include "Serial.h"
#include "Led.h"
#include "Delay.h"
uint8_t RxData;
int main(void)
{    
    Serial_Init();
    LED_Init();
    while (1)
    {
        if (Serial_GetRxFlag() == 1)
        {
            RxData = Serial_GetRxData();
            if(RxData=='A')
            {
                LED_ON();
            }
            else
            {    
                LED_OFF();
            }
        }
    }

}

完整的stm32工程可以通过

百度网盘获取

二：串口识别与匹配

首先将stm32连接上USB转ttl串口模块，再将USB转ttl串口模块插在树莓派任一空闲USB口。

在终端输入以下命令来查看端口号

ls -l /dev |grep ttyUSB

为什么要查看端口号？

在树莓派中，端口号与实际的USB端口并非固定的对应方式，而是按照先后顺序，从ttyUSB0，ttyUSB1……以此类推。所以如果同时插有多个USB转串口模块，就需要分别查询各个设备的端口号。

而这里只插了一个USB转串口模块，查看端口号是为了检测树莓派能不能识别到单片机的串口。

树莓派只为USB转串口设备分配端口号ttyUSBx，其它USB设备如键盘、鼠标等不会被分配端口号。

如果没有任何输出，很可能是被brltty（一个帮助盲人阅读的服务）占用了进程，直接卸载+重启，问题解决

解决Ubuntu22.04下找不到ttyUSB0_ttyusb0不存在-CSDN博客

sudo apt remove brltty
sudo reboot

也有可能是linux下驱动的问题，需要安装/更新CH340LINUX驱动，这是最万不得已的糟糕情况。所幸我遇到的问题属于前者。 

重新启动后再次输入

ls -l /dev |grep ttyUSB

可以看到ttyUSB0已经被识别到

crw-rw----  1 root dialout 188, 0 2月 28 17:21 ttyUSB0

这样就确定了单片机所连接的是ttyUSB0

三：pyserial介绍

由于ros2 c++中没有集成serial库，因此还需要自己下载serial源码进行编译安装。编译时还需要将serial文件夹放到工作空间下的src文件夹里一起编译。

但是使用python编写的ros2节点就避免了这些繁琐的步骤，只需要一条安装指令就可以在程序中自由使用serial库了。

sudo apt-get install python3-serial

四：节点程序编写

首先创建功能包

cd ~/colcon_ws/src
ros2 pkg create --build-type ament_python myserial_pkg

 在myserial_pkg中的myserial_pkg文件夹下新建一个myserial_node.py

这里我们在上一节中rpy_node.py的基础上进行了一些修改，并使用chatgpt对串口通信中的异常情况进行了处理。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
from rclpy.qos import QoSProfile, ReliabilityPolicy
import math
import serial

# 尝试打开串口的函数
def try_open_serial_port():
    try:
        # 配置串口参数并尝试打开串口
        serial_port = serial.Serial(
            port="/dev/ttyUSB0",          # 串口设备文件
            baudrate=115200,              # 波特率
            bytesize=serial.EIGHTBITS,    # 数据位
            parity=serial.PARITY_NONE,    # 校验位
            stopbits=serial.STOPBITS_ONE, # 停止位
            timeout=1                     # 读取超时时间
        )
        # 如果串口成功打开，返回串口对象
        if serial_port.is_open:
            print("Serial port opened successfully.")
            return serial_port
        else:
            # 如果串口没有打开，抛出异常
            raise serial.SerialException("Serial port is not open.")
    except serial.SerialException as e:
        # 打印串口打开失败的错误信息
        print(f"Unable to open serial port: {e}")
        return None

# 定义ROS2节点类
class MySerialNode(Node):
    def __init__(self):
        # 调用父类Node的构造函数，初始化节点
        super().__init__('myserial_node')
        # 创建IMU数据的订阅者，并设置回调函数和服务质量
        self.subscription = self.create_subscription(
            Imu,
            '/mavros/imu/data',
            self.listener_callback,
            QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)
        )
        # 避免订阅者未被使用警告
        self.subscription

    # IMU数据的回调函数
    def listener_callback(self, msg):
        # 从IMU消息中提取四元数分量
        x, y, z, w = msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w
        # 从四元数计算roll, pitch, yaw角度
        roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
        pitch = math.asin(2 * (w * y - z * x))
        yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
        # 将弧度转换为度
        roll_deg = math.degrees(roll)
        pitch_deg = math.degrees(pitch)
        yaw_deg = math.degrees(yaw)
        # 打印姿态角信息
        self.get_logger().info(f'Orientation: roll={roll_deg:.2f}, pitch={pitch_deg:.2f}, yaw={yaw_deg:.2f} degrees')
        # 根据roll和pitch的值发送不同的字符到串口
        if abs(pitch_deg) <= 30 and abs(roll_deg) <= 30:
            self.serial_write(b'A')  # roll和pitch均小于等于30度时发送'A'
        else:
            self.serial_write(b'B')  # 否则发送'B'

    # 串口写入数据的函数
    def serial_write(self, data):
        # 检查串口是否打开，如果打开则写入数据
        if self.serial_port and self.serial_port.is_open:
            self.serial_port.write(data)
        else:
            # 如果串口未打开，记录警告信息
            self.get_logger().warn("Serial port is not open for writing.")

# 程序入口函数
def main(args=None):
    # 初始化ROS2
    rclpy.init(args=args)
    # 创建节点对象
    node = MySerialNode()
    # 尝试打开串口，并在成功时保存串口对象
    node.serial_port = try_open_serial_port()
    # 如果串口成功打开，则开始节点的事件循环
    if node.serial_port:
        rclpy.spin(node)
    else:
        print("Cannot spin the node without a serial port.")
    # 销毁节点对象
    node.destroy_node()
    # 如果串口对象存在，则关闭串口
    if node.serial_port:
        node.serial_port.close()
        print("Serial port closed.")

# 判断是否是主程序运行，如果是则调用main函数
if __name__ == '__main__':
    main()

 在myserial_pkg功能包中，setup.py中的console_scripts的方括号里添加节点 

'console_scripts': [
            "myserial_node = myserial_pkg.myserial_node:main"
        ],

五：运行效果

 接下来编译： 

cd ~/colcon_ws #或者cd ..
colcon build

运行发布者mavros的apm.launch文件

sudo chmod 777 /dev/ttyAMA0
ros2 launch mavros apm.launch

 新打开一个终端运行myserial_node订阅者节点

ros2 service call /mavros/set_stream_rate mavros_msgs/StreamRate "{stream_id: 0, message_rate: 10, on_off: true}"
sudo chmod 777 /dev/ttyUSB0
ros2 run myserial_pkg myserial_node

运行效果：

ROS2节点myserial_node效果演示

作者：xehuosh