【ROS2】MOMO的鱼香ROS2(七)ROS2进阶篇——从驱动IMU开始前篇:串口数据读取

引言

笔者跟着鱼香ROS的ROS2学习之旅
学习参考:
【ROS2机器人入门到实战】
笔者的学习目录

  1. MOMO的鱼香ROS2(一)ROS2入门篇——从Ubuntu操作系统开启
  2. MOMO的鱼香ROS2(二)ROS2入门篇——ROS2初体验
  3. MOMO的鱼香ROS2(三)ROS2入门篇——ROS2第一个节点
  4. MOMO的鱼香ROS2(四)ROS2入门篇——ROS2节点通信之话题与服务
  5. MOMO的鱼香ROS2(五)ROS2入门篇——ROS2接口与自定义
  6. MOMO的鱼香ROS2(六)ROS2入门篇——ROS2通信之参数与动作

前沿

笔者使用的IMU型号及相关信息如下:

Yesense元生创新 YIS506姿态传感器
通信接口:RS232转USB
其他牌子的IMU数据读取流程类似,无非就是协议定义不同
在这里插入图片描述
首先,先使用产品的上位机(Windows,官网下载或购买产品时附带),对IMU的串口连接信息进行查看,并采集一些数据,便于后续代码接收IMU数据进行校对。

1. 连接上位机测试
Windows电脑连接设备,再双击运行上位机
在这里插入图片描述
在这里插入图片描述
经过上位机测试,确定以下信息:

串口号:COM7
波特率:460800
采集数据如下:
在这里插入图片描述

2. 根据产品手册明确相关协议的定义

该设备私有输出协议采用固定的格式,帧结构包含五部分:帧头、帧序号、数据长度、 数据域、校验码,如下表所示,且字节序全部采用小段模式。

在这里插入图片描述
IMU数据的ID定义如下(具体解析的数据顺序要根据实际测试,手册仅供参考):
在这里插入图片描述
手册上的其他信息后续使用时再说明

1 串口接收IMU数据

环境:
Ubuntu 18.04
ROS2 eloquent

将设备连接到电脑

Ubuntu下的串口助手cutecom
下载:sudo apt-get install cutecom 打开:sudo cutecom

在这里插入图片描述
在这里插入图片描述

1.查看映射端口
ls /dev/ttyUSB*
2.更改端口的权限
sudo chmod 777 /dev/ttyUSB0
sudo usermod -aG dialout username[修改]
3.安装python的serial库
sudo apt install python3-serial

运行如下脚本测试IMU是否正常采集数据

"""
2024.03.15
author:alian
IMU测试
1.查看映射端口
ls /dev/ttyUSB*
2.更改端口的权限
sudo chmod 777 /dev/ttyUSB0
sudo usermod -aG dialout username[修改]
"""

import serial
import serial
from tkinter import *
import struct
import codecs

def Test_IMU():
    ser = serial.Serial(port = '/dev/ttyUSB0',      # 将 'COM7' 替换成实际的串口号
                           baudrate=460800)  # 460800 是波特率

    # 判断串口是否打开成功
    if ser.isOpen():
        print("open success")
    else:
        print("open failed")
    while True:
        count = ser.inWaiting()
        if count>0:
            data = ser.read(count)
            print(data)
        else:  # 重新打开
            ser = serial.Serial('/dev/ttyUSB0', 460800)
        
if __name__ == '__main__':
	Test_IMU()

终端显示如下,返回的原始数据为16进制数值的字节字符串,即b'...',则测试成功!
在这里插入图片描述

2 Python解析IMU数据

2.1 协议头判断

  1. IMU原始数据以高频率不断发送,首先要找到协议头的位置,根据用户手册协议头定义为'0x59 0x53'共占2个字节,在字节字符串中的显示为'YS'
  2. 协议头判断成功后,根据手册,数据长度在第5个字节的位置,运行如下脚本,输出为72
  3. 结合手册说明,协议固定帧结构包含五部分:帧头2字节、帧序号2字节、数据长度1字节、 数据域72字节、校验1+1字节,得到一条完整的协议的长度为79
def Read_IMUdata():
    # 定义解析头
    PROTOCOL_FIRST_BYTE = 0x59
    PROTOCOL_SECOND_BYTE = 0x53
    my_imu = serial.Serial('/dev/ttyUSB0', 460800)  # 
    # 判断串口是否打开成功
    if my_imu.isOpen():
        print("open success1111")
    else:
        print("open failed")
    
    while True:
        count = my_imu.inWaiting()
        if count > 0:
            print('开始解析数据')
            data = my_imu.read(count)
            if data[0]==0x59 and data[1]==0x53:
                print('解析头校验成功:',data[0] == PROTOCOL_FIRST_BYTE and data[1] == PROTOCOL_SECOND_BYTE)   
                header = struct.unpack('HHB', data[:5])  # 解包协议头部分:帧头、帧序号、数据长度
                payload_len = header[2]  # 返回数据长度  
                print('数据长度:',payload_len)       
           # 重新打开串口
          else: 
          my_imu = serial.Serial('/dev/ttyUSB0', 460800)

2.2 IMU各类数据解析

在官方协议文档中说明如下:
在这里插入图片描述
在这里插入图片描述

内容拆解:

  1. 首先判断数据ID,如加速度的ID为0x10,占1个字节
  2. 数据长度值,如加速度为12,占1个字节
  3. 数据域,占12个字节,每四个字节组成一个 int 型(有符号)数据,12/4=3正好对应加速度x,y,z
  4. 解算公式:小端模式,系数因子为0.000001
# 定义一个字节字符串,假设是小端模式存储的
imu_ori = b'\x3B\x21\xFE\xFF'
# 将字节字符串转换为小端模式存储的整数
imu_data = int.from_bytes(imu_ori, byteorder='little', signed=True)
# 打印结果
print("原始字节字符串:", imu_ori)
print("转换后的整数:", imu_data)

综上,完整的数据解析代码如下:

class IMUInfo:  # 接收数据初始化
    def __init__(self):
        # 加速度
        self.accel_x = 0.0
        self.accel_y = 0.0
        self.accel_z = 0.0
        # 角速度
        self.angle_x = 0.0
        self.angle_y = 0.0
        self.angle_z = 0.0
        # 欧拉角
        self.pitch = 0.0
        self.roll = 0.0
        self.yaw = 0.0
        # 四元数
        self.quaternion_data0 = 0.0
        self.quaternion_data1 = 0.0
        self.quaternion_data2 = 0.0
        self.quaternion_data3 = 0.0
        # 采样时间戳
        self.sample_timestamp = 0
        # 同步输出时间戳
        self.data_ready_timestamp = 0
def Read_IMUdata():
    # 定义解析头
    PROTOCOL_FIRST_BYTE = 0x59
    PROTOCOL_SECOND_BYTE = 0x53
    # 定义数据因子
    fac1 = 0.000001
    fac2 = 0.001
    my_imu = serial.Serial('/dev/ttyUSB0', 460800)  # 
    imu_data = IMUInfo()  # 数据初始化
    # 判断串口是否打开成功
    if my_imu.isOpen():
        print("open success1111")
    else:
        print("open failed")
    
    while True:
        count = my_imu.inWaiting()
        if count > 0:
            # 发送静态零偏校准指令
            my_imu.write(codecs.decode('59534d12005001b06a','hex'))
            data = my_imu.read(79)
            print('开始解析数据')
            if data[0]==0x59 and data[1]==0x53:
                print('解析头校验:',data[0] == PROTOCOL_FIRST_BYTE and data[1] == PROTOCOL_SECOND_BYTE)
                header = struct.unpack('HHB', data[:5])  # 解包协议头部分:帧头、帧序号、数据长度
                payload_len = header[2]  # 返回数据长度

                # 2.分析数据域
                if payload_len>0:
                    # 解析加速度
                    if data[5]==0x10 and data[6]==12:
                        # 每四个字节组成一个int型(有符号)数据,小端模式(最低有效字节排在最前面),然后乘以系数
                        imu_data.accel_x = int.from_bytes(data[7:11],byteorder='little',signed = True)*fac1
                        imu_data.accel_y = int.from_bytes(data[11:15],byteorder='little',signed = True)*fac1
                        imu_data.accel_z = int.from_bytes(data[15:19],byteorder='little',signed = True)*fac1

                    # 解析角速度
                    if data[19]==0x20 and data[20]==12:
                        imu_data.angle_x = int.from_bytes(data[21:25],byteorder='little',signed = True)*fac1
                        imu_data.angle_y = int.from_bytes(data[25:29],byteorder='little',signed = True)*fac1
                        imu_data.angle_z = int.from_bytes(data[29:33],byteorder='little',signed = True)*fac1
                    # 解析欧拉角
                    if data[33]==0x40 and data[34]==12:
                        imu_data.pitch = int.from_bytes(data[35:39],byteorder='little',signed = True)*fac1
                        imu_data.roll = int.from_bytes(data[39:43],byteorder='little',signed = True)*fac1
                        imu_data.yaw = int.from_bytes(data[43:47],byteorder='little',signed = True)*fac1
                    # 解析四元数
                    if data[47]==0x41 and data[48]==16:
                        imu_data.quaternion_data0 = int.from_bytes(data[49:53],byteorder='little',signed = True)*fac1
                        imu_data.quaternion_data1 = int.from_bytes(data[53:57],byteorder='little',signed = True)*fac1
                        imu_data.quaternion_data2 = int.from_bytes(data[57:61],byteorder='little',signed = True)*fac1
                        imu_data.quaternion_data3 = int.from_bytes(data[61:65],byteorder='little',signed = True)*fac1
                    # 解析采样时间戳
                    if data[65]==0x51 and data[66]==4:
                        imu_data.sample_timestamp = int.from_bytes(data[67:71],byteorder='little',signed = False)
                    # 解析采样时间戳
                    if data[71]==0x52 and data[72]==4:
                        imu_data.data_ready_timestamp = int.from_bytes(data[73:77],byteorder='little',signed = False)
                    
                    print('加速度:',imu_data.accel_x,imu_data.accel_y,imu_data.accel_z)
                    print('角速度:',imu_data.angle_x,imu_data.angle_y,imu_data.angle_z)
                    print('欧拉角:',imu_data.pitch,imu_data.roll,imu_data.yaw)
                    print('时间戳:',imu_data.sample_timestamp,imu_data.data_ready_timestamp)
            
            # 当协议头判断失败,则重新打开串口
            else: my_imu = serial.Serial('/dev/ttyUSB0', 460800)
if __name__ == '__main__':
    # 串口数据读取
    Read_IMUdata()

3 ROS2发布IMU数据

笔者整理了ROS2编写节点7步程序,如下:

  1. 创建工作空间
    mkdir -p ROS_WS/colcon_ws/src
    cd ROS_WS/colcon_ws/src
  2. 创建功能包
    ros2 pkg create imu_py[功能包名称] --build-type ament_python --dependencies rclpy
  3. 创建节点
    touch imu_py/imu_py /node_imu.py
  4. 编写节点脚本程序如下:
    (1) 导入库文件
    (2) 初始化客户端库
    (3) 新建节点
    (4) spin循环节点
    (5) 关闭客户端库shutdown
  5. 修改 setup.py
    node_imu = imu_py.node_imu:main
    node_imu:节点名称
    imu_py.node_imu:main:【软件包】.【执行文件.py】:【执行函数mian】
  6. 编译运行节点
    cd ROS_WS/colcon_ws
    colcon build --packages-select imu_py --symlink-install
    source install/setup.bash
  7. 运行节点
    ros2 run imu_py node_imu

其中节点脚本如下:

# -*- coding: utf-8 -*-
# 1. 导入库文件
import rclpy
from rclpy.node import Node

# imu接收数据类型
class Node_imu(Node):

    def __init__(self,name):
        super().__init__(name)  # 继承父类,初始化名称
        self.get_logger().info("大家好,我是%s!" % name)
      
def main(args=None):
    """
    ros2运行该节点的入口函数
    编写ROS2节点的一般步骤
    1. 导入库文件
    2. 初始化客户端库
    3. 新建节点对象
    4. spin循环节点
    5. 关闭客户端库
    """
    rclpy.init(args=args)  # 2. 初始化客户端库
    node = IMUPublisher("imu_publisher")  # 3. 新建节点对象
    rclpy.spin(node)  #  4. spin循环节点,保持节点运行,检测是否收到退出指令(Ctrl+C)
    rclpy.shutdown()  # 5. 关闭客户端库

3.1 创建节点

下面正式开始编写程序!

步骤1-2如上
步骤3:创建IMU发布者节点
touch imu_py /imu_publisher.py
将上述代码在ROS2发布节点的时间回调函数中引用即可,如下:

# -*- coding: utf-8 -*-
"""
2024.03.15
author:alian
function:ROS2发布IMU
1.查看映射端口
ls /dev/ttyUSB*
2.更改端口的权限
sudo chmod 777 /dev/ttyUSB0
sudo usermod -aG dialout username[修改]
"""
import rclpy
from rclpy.node import Node
# 话题接口
from sensor_msgs.msg import Imu  # imu接口
from sensor_msgs.msg import LaserScan # 激光雷达接口
from sensor_msgs.msg import Image  # 相机接口
# Usart Library
import serial
import struct
import codecs

# imu接收数据类型
class IMUPublisher(Node):

    def __init__(self,name):
        super().__init__(name)  # 继承父类,初始化名称
        self.get_logger().info("大家好,我是%s!" % name)
        # 初始化发布者,传入参数:消息类型、话题名称、缓冲区大小
        self.imu_publisher = self.create_publisher(Imu, 'alian_IMU', 10)  # 创建发布imu数据的发布者到话题:imu_data上

        # 串口初始化
        self.IMU_Usart = serial.Serial(
            port='/dev/ttyUSB0',  # 串口
            baudrate=460800
        )
        # 接收数据初始化----------------------------------------------------------
        # 加速度
        self.accel_x = 0.0
        self.accel_y = 0.0
        self.accel_z = 0.0
        # 角速度
        self.angle_x = 0.0
        self.angle_y = 0.0
        self.angle_z = 0.0
        # 欧拉角
        self.pitch = 0.0
        self.roll = 0.0
        self.yaw = 0.0
        # 四元数
        self.quaternion_data0 = 0.0
        self.quaternion_data1 = 0.0
        self.quaternion_data2 = 0.0
        self.quaternion_data3 = 0.0
        # 采样时间戳
        self.sample_timestamp = 0
        # 同步输出时间戳
        self.data_ready_timestamp = 0

        # 判断串口是否打开成功
        if self.IMU_Usart.isOpen():
            print("open success1111")
        else:
            print("open failed")

        # 回调函数返回周期
        # 定时器:一定的时间间隔(time_period)触发回调函数(self.timer_callback)
        time_period = 0.001
        self.timer = self.create_timer(time_period, self.timer_callback)

    def timer_callback(self):
        """
        定时器回调函数
        """
        # ----读取IMU的内部数据-----------------------------------
        try:            
            self.Read_IMUdata()
            # 发布sensor_msgs/Imu 数据类型
            imu_data = Imu()
            imu_data.header.frame_id = "IMU"
            imu_data.header.stamp = self.get_clock().now().to_msg()
            imu_data.linear_acceleration.x = self.accel_x
            imu_data.linear_acceleration.y = self.accel_y
            imu_data.linear_acceleration.z = self.accel_z
            imu_data.angular_velocity.x = self.angle_x  # unit transfer to rad/s
            imu_data.angular_velocity.y = self.angle_y
            imu_data.angular_velocity.z = self.angle_z
            imu_data.orientation.x = self.quaternion_data0
            imu_data.orientation.y = self.quaternion_data1
            imu_data.orientation.z = self.quaternion_data2
            imu_data.orientation.w = self.quaternion_data3
            self.imu_publisher.publish(imu_data)  # 发布imu的数据
            self.get_logger().info(f'发布了指令:{imu_data.header.frame_id}')    #打印一下发布的数据
            # --------------------------------------------------------
            print('加速度:',self.accel_x,self.accel_y,self.accel_z)
            print('角速度:',self.angle_x,self.angle_y,self.angle_z)
            print('欧拉角:',self.pitch,self.roll,self.yaw)
            print('时间戳:',self.sample_timestamp,self.data_ready_timestamp)

        except KeyboardInterrupt:
            if serial != None:
                print("close serial port")
                self.IMU_Usart.close()

    def Send_ReadCommand(self):
        '''
        Author: alian
        Time: 2024.03.15
        description: 发送读取IMU内部数据指令
        IMU型号:YIS506, 波特率为:460800
        协议长度:93
        '''
        # 静态零偏校准指令发送
        self.IMU_Usart.write(codecs.decode('59534d12005001b06a','hex'))                           #发送

    def Read_IMUdata(self):
        # 定义解析头
        PROTOCOL_FIRST_BYTE = 0x59
        PROTOCOL_SECOND_BYTE = 0x53
        # 定义数据因子
        fac1 = 0.000001
        count = self.IMU_Usart.inWaiting()
        if count > 0:
            # 发送静态零偏校准指令
            self.IMU_Usart.write(codecs.decode('59534d12005001b06a','hex'))
            data = self.IMU_Usart.read(79)
            print('开始解析数据')
            if data[0]==0x59 and data[1]==0x53:
                print('解析头校验:',data[0] == PROTOCOL_FIRST_BYTE and data[1] == PROTOCOL_SECOND_BYTE)
                header = struct.unpack('HHB', data[:5])  # 解包协议头部分:帧头、帧序号、数据长度
                payload_len = header[2]  # 返回数据长度

                # 2.分析数据域
                if payload_len>0:
                    # 解析加速度
                    if data[5]==0x10 and data[6]==12:
                        # 每四个字节组成一个int型(有符号)数据,小端模式(最低有效字节排在最前面),然后乘以系数
                        self.accel_x = int.from_bytes(data[7:11],byteorder='little',signed = True)*fac1
                        self.accel_y = int.from_bytes(data[11:15],byteorder='little',signed = True)*fac1
                        self.accel_z = int.from_bytes(data[15:19],byteorder='little',signed = True)*fac1

                    # 解析角速度
                    if data[19]==0x20 and data[20]==12:
                        self.angle_x = int.from_bytes(data[21:25],byteorder='little',signed = True)*fac1
                        self.angle_y = int.from_bytes(data[25:29],byteorder='little',signed = True)*fac1
                        self.angle_z = int.from_bytes(data[29:33],byteorder='little',signed = True)*fac1
                    # 解析欧拉角
                    if data[33]==0x40 and data[34]==12:
                        self.pitch = int.from_bytes(data[35:39],byteorder='little',signed = True)*fac1
                        self.roll = int.from_bytes(data[39:43],byteorder='little',signed = True)*fac1
                        self.yaw = int.from_bytes(data[43:47],byteorder='little',signed = True)*fac1
                    # 解析四元数
                    if data[47]==0x41 and data[48]==16:
                        self.quaternion_data0 = int.from_bytes(data[49:53],byteorder='little',signed = True)*fac1
                        self.quaternion_data1 = int.from_bytes(data[53:57],byteorder='little',signed = True)*fac1
                        self.quaternion_data2 = int.from_bytes(data[57:61],byteorder='little',signed = True)*fac1
                        self.quaternion_data3 = int.from_bytes(data[61:65],byteorder='little',signed = True)*fac1
                    # 解析采样时间戳
                    if data[65]==0x51 and data[66]==4:
                        self.sample_timestamp = int.from_bytes(data[67:71],byteorder='little',signed = False)
                    # 解析采样时间戳
                    if data[71]==0x52 and data[72]==4:
                        self.data_ready_timestamp = int.from_bytes(data[73:77],byteorder='little',signed = False)
 
            # 重新打开串口
            else: self.IMU_Usart = serial.Serial('/dev/ttyUSB0', 460800)

def main(args=None):
    """
    ros2运行该节点的入口函数
    编写ROS2节点的一般步骤
    1. 导入库文件
    2. 初始化客户端库
    3. 新建节点对象
    4. spin循环节点
    5. 关闭客户端库
    """
    rclpy.init(args=args)  # 初始化rclpy
    node = IMUPublisher("imu_publisher")  # 新建一个节点
    rclpy.spin(node)  # 保持节点运行,检测是否收到退出指令(Ctrl+C)
    rclpy.shutdown()  # 关闭rclpy

3.2 修改setup.py

from setuptools import setup
import os 
import glob

package_name = 'imu_py'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='ll',
    maintainer_email='ll@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            "node_imu=imu_py.node_imu:main",
            "imu_publisher=imu_py.imu_publisher:main"  # 在这修改哦!可以发布多个节点
        ],
    },
)

3.3 编译+配置+运行

终端1:

# 编译运行节点
cd ROS_WS/colcon_ws
colcon build --packages-select imu_py --symlink-install
source install/setup.bash
# 运行节点
ros2 run imu_py imu_publisher

在这里插入图片描述
终端输出对比上位机采集的数据

综上,完成IMU串口数据的读取,不同IMU,解析步骤大同小异

评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值