文章目录
- ROS下位机驱动节点
- 节点功能
- 1.获取串口数据
- 2.发布ROS节点数据
- 3.接收ROS节点数据
- 驱动代码
- 1.IMU数据发布
- 节点代码编写
- 查看发布数据
- 2.角速度、线速度数据发布
- 将双轮速度数据处理为角速度和线速度数据
- 节点代码编写
- 查看发布数据
- driver_node.py节点完整代码
因为下位机是拿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。
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()
参考文章：
- 读取编码器的计数脉冲转化成ROS的odom数据
- ROS机器人里程计模型
- ROS小车记录系列（二）IMU采集、过滤，与odom数据融合，发布新的odom话题
- ROS订阅/cmd_vel话题，转化成移动机器人左右轮的转速
