引言
笔者跟着鱼香ROS的ROS2学习之旅
学习参考:
【ROS2机器人入门到实战】
笔者的学习目录
- MOMO的鱼香ROS2(一)ROS2入门篇——从Ubuntu操作系统开启
- MOMO的鱼香ROS2(二)ROS2入门篇——ROS2初体验
- MOMO的鱼香ROS2(三)ROS2入门篇——ROS2第一个节点
- MOMO的鱼香ROS2(四)ROS2入门篇——ROS2节点通信之话题与服务
- MOMO的鱼香ROS2(五)ROS2入门篇——ROS2接口与自定义
- 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 协议头判断
- IMU原始数据以高频率不断发送,首先要找到协议头的位置,根据用户手册协议头定义为
'0x59 0x53'
共占2个字节,在字节字符串中的显示为'YS'
- 协议头判断成功后,根据手册,数据长度在第5个字节的位置,运行如下脚本,输出为
72
- 结合手册说明,协议固定帧结构包含五部分:帧头
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各类数据解析
在官方协议文档中说明如下:
内容拆解:
- 首先判断数据ID,如加速度的ID为0x10,占1个字节
- 数据长度值,如加速度为12,占1个字节
- 数据域,占12个字节,每四个字节组成一个 int 型(有符号)数据,12/4=3正好对应加速度x,y,z
- 解算公式:小端模式,系数因子为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步程序,如下:
- 创建工作空间
mkdir -p ROS_WS/colcon_ws/src
cd ROS_WS/colcon_ws/src- 创建功能包
ros2 pkg create imu_py[功能包名称] --build-type ament_python --dependencies rclpy- 创建节点
touch imu_py/imu_py /node_imu.py- 编写节点脚本程序如下:
(1) 导入库文件
(2) 初始化客户端库
(3) 新建节点
(4) spin循环节点
(5) 关闭客户端库shutdown- 修改 setup.py
node_imu = imu_py.node_imu:main
node_imu:节点名称
imu_py.node_imu:main:【软件包】.【执行文件.py】:【执行函数mian】- 编译运行节点
cd ROS_WS/colcon_ws
colcon build --packages-select imu_py --symlink-install
source install/setup.bash- 运行节点
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,解析步骤大同小异