在制作ros小车时，上位机与下位之间的通信是非常重要的一部分，以下是阿贝尔机器人树莓派端的底层代码，添加了注释，供以后学习查阅，该程序需使用了多线程工作，有关python多线程详解的有关内容请参照此博客，ros小车的下位机代码还没有写出，等回学校有时间在研究。如有错误还请大佬指正，非常感谢！！！
#!/usr/bin/python2
import serial
import threading
import time
import struct
import math
import rospy
import tf
from geometry_msgs.msg import Twist, Quaternion
from nav_msgs.msg import Odometry
lock = threading.Lock() # 线程锁
#串口类， 自定义线程
class TrdSerial(threading.Thread):
def __init__(self, serialport_name, baudrate):
threading.Thread.__init__(self) # 重构run函数必须要写
# uper(MyThread, self).__init__() # 上面的代码也可换为次句
self._serialport_name = serialport_name
self._baudrate = baudrate
self._conn = serial.Serial(self._serialport_name, self._baudrate, timeout=1)
self._stop = False
self._first_time_flag = True
self._encoders_offset = [0, 0]
self._encoders = [0, 0]
self._voltage = 0
self._currents = [0,0]
self._error_code = 0
#重连
def reconnect(self, side):
print('{} {} reconnecting...'.format(side, time.time()))
while True:
try:
self._conn = serial.Serial(self._serialport_name, self._baudrate, timeout=1)
except:
time.sleep(1)
print('{} {} reconnecting...'.format(side, time.time()))
else:
print('{} {} reconnected!'.format(side, time.time()))
break
# 重构线程的run函数
def run(self):
msg = []
msg_len = 0
in_msg_flag = False
while not self._stop:
#读取单片机发送的一个字节
r = self.read(1)
if in_msg_flag:
#数据溢出，丢弃
if len(msg) > 30:
in_msg_flag = False
msg[:] = []
print('Overflow message: {}, discarded!'.format(msg))
#跳出while循环
continue
msg.append(r)
#当接收到第二个数据时
if len(msg)==2:
#数据的第二位为数据的总长度
msg_len = ord(r)
#当收到的数据位数 加上前两位 满足数据长度
if len(msg) == msg_len+2:
#数据以回车为结束
if r=='\r':
in_msg_flag = False
#接收数据结束，进行数据更新
self.update(msg)
msg[:] = []
#无效消息，丢弃
else:
print('Invalid message: {}, discarded!'.format(msg))
#如果单片机发送的是0xea
elif r=='\xea':
msg[:] = []
#msg的第一个数据，数据的头
msg.append(r)
in_msg_flag = True
#获得编码器数据
def get_encoders(self):
return (self._encoders[0], self._encoders[1])
def set_speed(self, v1, v2):
cmd = [0xea, 0x05, 0x7e, v1, v2, 0x00, 0x0d]
self.send(cmd)
def reset_encoder(self):
print('Reset encoder')
cmd = [0xea, 0x03, 0x35, 0x00, 0x0d]
self.send(cmd)
def reset_base(self):
print('Reset base')
cmd = [0xea, 0x03, 0x50, 0x00, 0x0d]
self.send(cmd)
time.sleep(3)
#启用超时？？？？？？？
def enable_timeout(self):
print('Enable timeout')
cmd = [0xea, 0x02, 0x39, 0x00, 0x0d]
self.send(cmd)
def update(self, msg):
#print('{} received message: {}'.format(time.time(), [hex(ord(m)) for m in msg]))
if len(msg) < 4:
print('Msg length < 4 : {}'.format(len(msg)))
#表示数据长度的位
if ord(msg[2]) == 0x7e:
if len(msg) != 23:
print('Msg length = {} not correct, expected: 23'.format(len(msg)))
#电压值
self._voltage = 0.2157*ord(msg[3])
#电流？？？？
self._currents = [0.0272*ord(msg[4]), 0.0272*ord(msg[5])]
#错误代码？？？？？
self._error_code = ord(msg[6])
if self._error_code != 0:
print('trd_driver Error! error code({})'.format(self._error_code))
#编码器数据，需要去看单片机上读取到的编码器数据 一个编码器四位
self._encoders[0], = struct.unpack('>i', bytearray(msg[11:15]))
self._encoders[1], = struct.unpack('>i', bytearray(msg[15:19]))
#取出第一次编码器的值
if self._first_time_flag:
self._encoders_offset[0] = self._encoders[0]
self._encoders_offset[1] = self._encoders[1]
self._first_time_flag = False
#去除第一次读到的编码器数值，排除干扰
self._encoders[0] -= self._encoders_offset[0]
self._encoders[1] -= self._encoders_offset[1]
#print('{} error:{} encoders: {}, {}V {}A {}A. set:{} {}, actual:{} {}'.format(
# time.time(), self._error_code, self._encoders, self._voltage, self._currents[0], self._currents[1],
# ord(msg[7]), ord(msg[8]), ord(msg[9]), ord(msg[10])))
else:
print('{} received message: {}'.format(time.time(), msg))
def stop(self):
print('stop')
self._stop = True
self._conn.close()
def send(self, cmd):
for i in range(len(cmd)-2):
#表示异或赋值，校验位
cmd[-2] ^= cmd[i]
while True:
try:
self._conn.write(cmd)
break
except serial.serialutil.SerialException:
self.reconnect('send')
def read(self, n):
r = ''
try:
r = self._conn.read(n)
except:
self.reconnect('read')
return r
#ROS有关的类，此处也是一个线程，但并没有自定义，可以把此类理解为一个主线程，而串口线程则是跟此线程并行的
class RosWraperTrd():
def __init__(self, node_name):
rospy.init_node(node_name,disable_signals=True)
#串口名称
self.serialport_name = rospy.get_param('~serialport_name', \
default='/dev/motor_trd')
#波特率
self.baudrate = rospy.get_param('~baudrate', default=38400)
#线性系数，直线运动比例系数
self.linear_coef = rospy.get_param('~linear_coef', default=82)
#角度系数，差速比例系数
self.angular_coef = rospy.get_param('~angular_coef', default=14.64)
#左轮系数？？？
self.left_coef = rospy.get_param('~left_coef', default=1)
#右轮系数？？？
self.right_coef = rospy.get_param('~right_coef', default =1)
#编码器没转一圈的脉冲数？？？ 这麽大？？？？？
self.encoder_ticks_per_rev = rospy.get_param('~encoder_ticks_per_rev', \
default=1600)
#车的宽度？？？？ 两轮的间距
self.base_width = rospy.get_param('~base_width', default=0.39)
#轮的直径
self.wheel_diameter = rospy.get_param('~wheel_diameter', default=0.125)
####发布里程计数据，gmapping订阅#####################
self.pub_odom = rospy.Publisher('/odom', Odometry, queue_size=10)
####订阅导航的底层控制数据，用于控制电机###############
self.sub_vel = rospy.Subscriber('/cmd_vel', Twist, self.vel_callback, queue_size=1)
#tf变换
self.tf_broadcaster = tf.TransformBroadcaster()
#编码器的什么数据？？？？？？ 上一次编码器的数据
self.encoder1_prev = 0
self.encoder2_prev = 0
#时间差
self.time_prev = rospy.Time.now() - rospy.Time.from_sec(1)
self.x = 0
self.y = 0
self.theta = 0
self.odom = Odometry()
self.odom.header.frame_id = 'odom'
self.odom.child_frame_id = 'base_link'
self.trd_serial = TrdSerial(self.serialport_name, self.baudrate)
self.trd_serial.reset_encoder()
#self.trd_serial.reset_base()
self.trd_serial.enable_timeout()
#打开串口线程，跟ros线程并行运行
self.trd_serial.start()
self.trd_serial.run()
self.vel1 = 0
self.vel2 = 0
self.vel_latest_time = 0
self.vel_timeout = 1
#电机控制回调
def vel_callback(self, vel_msg):
v1 = self.linear_coef * vel_msg.linear.x
v2 = self.linear_coef * vel_msg.linear.x
v1 -= self.angular_coef * vel_msg.angular.z
v2 += self.angular_coef * vel_msg.angular.z
v1 *= self.left_coef
v2 *= self.right_coef
v1 += 128
v2 += 128
v1 = int(v1) if v1<255 else 255
v2 = int(v2) if v2<255 else 255
v1 = int(v1) if v1>0 else 0
v2 = int(v2) if v2>0 else 0
#线程上锁，防止数据出错
lock.acquire()
self.vel1 = v1
self.vel2 = v2
#解锁线程
lock.release()
self.vel_latest_time = time.time()
def update(self):
#set speed 如果长时间没有产生速度回调
if time.time()-self.vel_latest_time > self.vel_timeout:
#print('Vel timeout, set to (0,0)')
lock.acquire()
self.vel1 = 128
self.vel2 = 128
lock.release()
#如果正常产生回调
self.trd_serial.set_speed(self.vel1, self.vel2)
#get encoders
(encoder1, encoder2) = self.trd_serial.get_encoders()
time_current = rospy.Time.now()
time_elapsed = (time_current - self.time_prev).to_sec()
self.time_prev = time_current
#里程计=轮周长×（脉冲数/一圈的脉冲数）
dleft = self.left_coef * math.pi * self.wheel_diameter * \
(encoder1 - self.encoder1_prev) / self.encoder_ticks_per_rev
dright = self.right_coef * math.pi * self.wheel_diameter * \
(encoder2 - self.encoder2_prev) / self.encoder_ticks_per_rev
self.encoder1_prev = encoder1
self.encoder2_prev = encoder2
#距离
d = (dleft + dright) / 2
#偏航角（近似值），两轮运动距离相差较小时成立 正切值近似，，，
dtheta = (dright - dleft) / self.base_width
#计算世界坐标下的车的运动
if d != 0:
#计算车当前坐标系下的坐标信息
dx = math.cos(dtheta) * d
dy = math.sin(dtheta) * d
#换算到里程计坐标下
self.x += dx*math.cos(self.theta)-dy*math.sin(self.theta)
self.y += dx*math.sin(self.theta)+dy*math.cos(self.theta)
#偏航角进行累加
self.theta += dtheta
self.odom.header.stamp = time_current
self.odom.pose.pose.position.x = self.x
self.odom.pose.pose.position.y = self.y
#四元数表示位姿
q = tf.transformations.quaternion_from_euler(0,0,self.theta)
self.odom.pose.pose.orientation.x = q[0]
self.odom.pose.pose.orientation.y = q[1]
self.odom.pose.pose.orientation.z = q[2]
self.odom.pose.pose.orientation.w = q[3]
#计算速度
self.odom.twist.twist.linear.x = d / time_elapsed
#计算角速度
self.odom.twist.twist.angular.z = dtheta / time_elapsed
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
try:
self.update()
#发布里程计信息，供导航包使用
self.pub_odom.publish(self.odom)
#发布坐标变换，odom->base_link，构建tf_tree
self.tf_broadcaster.sendTransform(
(self.x,self.y,0),
tf.transformations.quaternion_from_euler(0, 0, self.theta),
rospy.Time.now(),
'base_link',
'odom')
rate.sleep()
except KeyboardInterrupt:
print('exit.')
self.trd_serial.stop()
break
if __name__ == '__main__':
ros_wrapper_trd = RosWraperTrd('trd_driver')
ros_wrapper_trd.run()
