ROS——下位机驱动节点：读取并发布单片机IMU、编码器速度数据（python）

最新推荐文章于 2024-05-17 11:18:21 发布

Irving.Gao

最新推荐文章于 2024-05-17 11:18:21 发布

阅读量3.1k

点赞数 8

分类专栏：智能机器人开发 ROS

本文链接：https://blog.csdn.net/qq_45779334/article/details/113563618

版权

ROS 同时被 2 个专栏收录

30 篇文章 18 订阅

订阅专栏

智能机器人开发

9 篇文章 39 订阅

订阅专栏

文章目录

因为下位机是拿micropython写的，所以需要自己写一下上位机ros驱动节点，同时也顺便熟悉一下ROS的驱动部分。

ROS下位机驱动节点

节点功能

ROS驱动节点包括以下三个功能：

1.获取串口数据

2.发布ROS节点数据

IMU数据
角速度、线速度数据

3.接收ROS节点数据

目标角速度和线速度数据

接收节点我们将使用键盘控制的方式控制小车运动。

驱动代码

1.IMU数据发布

在这里我的IMU的9轴数据已经在下位机处理成标准格式数据，并以元组格式向上位机发送：

#micropython下位机发送的是元组格式
（imu.accel.xyz, imu.gyro.xyz, imu.mag.xyz）
((0.03735352, 0.967041, 0.2568359), (0.09160305, 1.114504, -0.4656488), (19.85156, -0.3644531, 4.905469))

通信部分大家按照自己的上下位机情况自行改写即可。

节点代码编写

import rospy
import serial
from sensor_msgs.msg import Imu, MagneticField

if __name__ == '__main__':
    # 创建节点
    rospy.init_node('my_driver_node')

    # 串口创建
    ser = serial.Serial(port='/dev/ttyACM0', baudrate=9600)
    
    # 创建publisher
    imu_pub = rospy.Publisher('imu', Imu, queue_size=10)
    mag_pub = rospy.Publisher('mag', MagneticField, queue_size=10)

    # 和下位机进行通讯
    while not rospy.is_shutdown():
        # 阻塞式的函数
        # 读取并解码数据，变成元组
        data = ser.readline()
        data = data.decode("utf-8")
        data = str(data)[1:-3] # 将无关字符去除 “\n” 和 b'' 去除
        try:
            data = eval(data) # 转化回元组形式
        except SyntaxError:
            data = None

        if data != None:
            imu_data = data[1]
            # 采用IMU类型发布数据
            imu = Imu()
            imu.linear_acceleration.x = imu_data[0][0]
            imu.linear_acceleration.y = imu_data[0][1]
            imu.linear_acceleration.z = imu_data[0][2]
            imu.angular_velocity.x = imu_data[1][0]
            imu.angular_velocity.y = imu_data[1][1]
            imu.angular_velocity.z = imu_data[1][2]
            imu_pub.publish(imu)
			
            mag = MagneticField()
            mag.magnetic_field.x = imu_data[2][0]
            mag.magnetic_field.y = imu_data[2][1]
            mag.magnetic_field.z = imu_data[2][2]
            mag_pub.publish(mag)

    rospy.spin()

查看发布数据

执行driver_node节点：

# 给串口赋权限
chmod 777 /dev/ttyACM0
source /devel/setup.bash
rosrun driver driver_node.py

再打印一下imu发布的数据：

rostopic echo imu

在这里插入图片描述

2.角速度、线速度数据发布

在这里我的下位机本来是直接把编码器值传到上位机，但是为了上位机方便起见，直接在下位机处理成双轮的速度，然后再发到上位机，数据格式同样是元组：

#micropython下位机发送的是元组格式
（speed_A, speed_B）
(0.0, 0.0)

通信部分大家按照自己的上下位机情况自行改写即可。

将双轮速度数据处理为角速度和线速度数据

首先明确差分模型的机器人始终做的是以R为半径的圆弧运动。如下图所示，机器人的线速度V、角速度ω，左右轮速用VL和VR表示，用D表示轮间距，D=2d，右轮到旋转中心的距离为L。
图源https://blog.csdn.net/zhao_ke_xue/article/details/108425922?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.control

ROS端给机器人底盘（STM32单片机端）发送的是机器人要达到的线速度V和角速度ω，而我们底盘控制板需要的是左右轮速VL和VR来进行速度控制。所以他们的关系如下：
在这里插入图片描述

用python代码实现：

# 已知量，单位m
v = 0 # 机器人的线速度
w = 0 # 机器人的角速度
v_l = 0 # 左轮速度
v_r = 0 # 右轮速度
D = 0.2 # 轮间距
d = D/2

# 速度、角速度、左右轮速的关系式
v_l = v + wd
v_r = v - wd
v = (v_l + v_r)/2 
w = (v_l - v_r)/D

在知道了转换关系式后，我们就可以把角速度和线速度发布出去了。

节点代码编写

#!/usr/bin/env python
# coding:utf-8

import rospy
import serial
from sensor_msgs.msg import Imu, MagneticField
from geometry_msgs.msg import Twist

if __name__ == '__main__':
    # 参数初始化，单位m
    v = 0 # 机器人的线速度
    w = 0 # 机器人的角速度
    v_l = 0 # 左轮速度
    v_r = 0 # 右轮速度
    D = 0.2 # 轮间距
    d = D/2
    
    # 创建节点
    rospy.init_node('my_driver_node')

    # 串口通信
    ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200)

    twist_pub = rospy.Publisher('get_vel', Twist, queue_size=10)

    # 和下位机进行通讯
    while not rospy.is_shutdown():
        # 阻塞式的函数
        # 读取并解码数据，变成元组
        data = ser.readline()
        data = data.decode("utf-8")
        data = str(data)[1:-3] # 将无关字符去除 “\n” 和 b'' 去除
        try:
            data = eval(data) # 转化回元组形式
        except SyntaxError:
            data = None
        
        if data != None:
            # 分别提取出速度和imu数据
            speed_data = data[0]
            print(data)
            try:
                # 发布角速度线速度数据
                twist = Twist()
                v_l = speed_data[0] # 获取左轮速度
                v_r = speed_data[1] # 获取右轮速度
                twist.linear.x =(v_l + v_r)/2
                twist.linear.y = 0
                twist.linear.z = 0
                twist.angular.x = 0
                twist.angular.y = 0
                twist.angular.z = (v_l - v_r)/D
                twist_pub.publish(twist)

            except TypeError:
                pass    
    
    rospy.spin()

查看发布数据

执行driver_node节点：

# 给串口赋权限
chmod 777 /dev/ttyACM0
source /devel/setup.bash
rosrun driver driver_node.py

再打印一下twist发布的数据：

rostopic echo twist

在这里插入图片描述

driver_node.py节点完整代码

#!/usr/bin/env python
# coding:utf-8

import rospy
import serial
from sensor_msgs.msg import Imu, MagneticField
from geometry_msgs.msg import Twist

if __name__ == '__main__':
    # 参数初始化，单位m
    v = 0 # 机器人的线速度
    w = 0 # 机器人的角速度
    v_l = 0 # 左轮速度
    v_r = 0 # 右轮速度
    D = 0.2 # 轮间距
    d = D/2
    
    # 创建节点
    rospy.init_node('my_driver_node')

    # 串口通信
    ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200)
    
    # imu数据发布者
    imu_pub = rospy.Publisher('imu', Imu, queue_size=10)
    mag_pub = rospy.Publisher('mag', MagneticField, queue_size=10)

    twist_pub = rospy.Publisher('get_vel', Twist, queue_size=10)


    # 控制指令
    # rospy.Subscriber("cmd_vel", Twist, callback, Socket)

    # 和下位机进行通讯
    while not rospy.is_shutdown():
        # 阻塞式的函数
        # 读取并解码数据，变成元组
        data = ser.readline()
        data = data.decode("utf-8")
        data = str(data)[1:-3] # 将无关字符去除 “\n” 和 b'' 去除
        try:
            data = eval(data) # 转化回元组形式
        except SyntaxError:
            data = None
        
        if data != None:
            # 分别提取出速度和imu数据
            speed_data = data[0]
            imu_data = data[1]
            print(data)
            try:
                # 发布imu数据
                imu = Imu()
                imu.linear_acceleration.x = imu_data[0][0]
                imu.linear_acceleration.y = imu_data[0][1]
                imu.linear_acceleration.z = imu_data[0][2]
                imu.angular_velocity.x = imu_data[1][0]
                imu.angular_velocity.y = imu_data[1][1]
                imu.angular_velocity.z = imu_data[1][2]
                imu_pub.publish(imu)

                mag = MagneticField()
                mag.magnetic_field.x = imu_data[2][0]
                mag.magnetic_field.y = imu_data[2][1]
                mag.magnetic_field.z = imu_data[2][2]
                mag_pub.publish(mag)
            
                # 发布角速度线速度数据
                twist = Twist()
                v_l = speed_data[0] # 左轮速度
                v_r = speed_data[1] # 右轮速度
                twist.linear.x =(v_l + v_r)/2
                twist.linear.y = 0
                twist.linear.z = 0
                twist.angular.x = 0
                twist.angular.y = 0
                twist.angular.z = (v_l - v_r)/D
                twist_pub.publish(twist)

            except TypeError:
                pass    
    
    rospy.spin()

参考文章：

Irving.Gao

关注

8
点赞
踩
68

收藏

觉得还不错? 一键收藏
0
评论
ROS——下位机驱动节点：读取并发布单片机IMU、编码器速度数据（python）

因为下位机是拿micropython写的，所以现有的驱动节点我都用不了，需要自己写一下，同时也顺便熟悉一下ROS的驱动部分。文章目录ROS下位机驱动节点节点功能获取串口数据发布数据接收数据驱动代码IMU数据发布代码编写查看发布数据速度数据发布ROS下位机驱动节点节点功能ROS驱动节点包括以下两个功能：获取串口数据发布数据IMU数据编码器数据同时，如果可以也可以再将编码器数据转换为里程计(odom)数据里程计数据接收数据传给下位机控制机器人的速度驱动代码IMU数据发.
复制链接

扫一扫