2.3.让底盘动起来！差速控制模型及串口驱动编写

本节将编写一个差速底盘的驱动程序，采用串口通信方式，主要实现以下功能：

• 订阅ROS其它节点发布的速度指令，控制底盘运行；
• 读取底盘两个轮子的编码器数据，并解算成底盘的位置坐标。

用Python写一个简单的ROS发布者节点

首先，在trd_driver包的src目录下编写一个用Python写的ROS节点，命名为trd_driver.py，此节点以10Hz的频率，将ROS中nav_msgs包下的Odometry类型消息发布到名为/odom的主题上。

Odometry消息类型定义可查看ROS官方文档http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html。

注意：需将trd_driver.py文件添加执行权限后才可运行：

$ roscd trd_driver/src/
$ chmod +x trd_driver.py

#!/usr/bin/python2

import rospy
from nav_msgs.msg import Odometry

if __name__=='__main__':
    rospy.init_node('trd_driver_node')
    odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        current_odom = Odometry()
        odom_pub.publish(current_odom)
        rate.sleep()

此时，先启动roscore，然后打开新的终端，执行：

$ cd ~/catkin_ws/src/trd_driver/src/
$ ./trd_driver.py

另一种执行方式是用rosrun指令，此命令可在任意路径下执行，无需进入到trd_driver.py所在路径。

$ rosrun trd_driver trd_driver.py

运行此节点后，利用rostopic命令查看节点发布的主题：

$ rostopic list
$ rostopic echo /odom

此时，可看到终端不停地打印出/odom主题，由于未对其赋值，所以默认值均为0。

在节点中添加速度订阅者，并接收键盘控制的速度指令

然后，在节点文件中加入一个订阅者，订阅名为/cmd_vel主题的消息，消息类型为geometry_msgs包下的Twist类型，为ROS中专门描述速度的消息。

Twist消息类型定义可查看ROS官方文档http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html。

#!/usr/bin/python2

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist

def vel_callback(msg):
    print(msg.linear.x, msg.angular.z)

if __name__=='__main__':
    rospy.init_node('trd_driver_node')
    odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
    vel_sub = rospy.Subscriber('/cmd_vel', Twist, vel_callback)
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        current_odom = Odometry()
        odom_pub.publish(current_odom)
        rate.sleep()

然后，在src目录下新建名为keyboard_teleop.py的ROS节点（记得修改执行权限），作用是将键盘控制信号发布到/cmd_vel速度主题。 （其源代码可到此处下载。)

trd_driver/src/keyboard_teleop.py

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty

msg = """
Control Your Turtlebot!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothly

CTRL-C to quit
"""

moveBindings = {
        'i':(1,0),
        'o':(1,-1),
        'j':(0,1),
        'l':(0,-1),
        'u':(1,1),
        ',':(-1,0),
        '.':(-1,1),
        'm':(-1,-1),
           }

speedBindings={
        'q':(1.1,1.1),
        'z':(.9,.9),
        'w':(1.1,1),
        'x':(.9,1),
        'e':(1,1.1),
        'c':(1,.9),
          }

def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

speed = 0.5
turn = 1

def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)

if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)
    
    rospy.init_node('teleop_key')
    pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)

    x = 0
    th = 0
    status = 0
    count = 0
    acc = 0.1
    target_speed = 0
    target_turn = 0
    control_speed = 0
    control_turn = 0
    try:
        print msg
        print vels(speed,turn)
        while(1):
            key = getKey()
            if key in moveBindings.keys():
                x = moveBindings[key][0]
                th = moveBindings[key][1]
                count = 0
            elif key in speedBindings.keys():
                speed = speed * speedBindings[key][0]
                turn = turn * speedBindings[key][1]
                count = 0

                print vels(speed,turn)
                if (status == 14):
                    print msg
                status = (status + 1) % 15
            elif key == ' ' or key == 'k' :
                x = 0
                th = 0
                control_speed = 0
                control_turn = 0
            else:
                count = count + 1
                if count > 4:
                    x = 0
                    th = 0
                if (key == '\x03'):
                    break

            target_speed = speed * x
            target_turn = turn * th

            if target_speed > control_speed:
                control_speed = min( target_speed, control_speed + 0.02 )
            elif target_speed < control_speed:
                control_speed = max( target_speed, control_speed - 0.02 )
            else:
                control_speed = target_speed

            if target_turn > control_turn:
                control_turn = min( target_turn, control_turn + 0.1 )
            elif target_turn < control_turn:
                control_turn = max( target_turn, control_turn - 0.1 )
            else:
                control_turn = target_turn

            twist = Twist()
            twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turn
            pub.publish(twist)

            #print("loop: {0}".format(count))
            #print("target: vx: {0}, wz: {1}".format(target_speed, target_turn))
            #print("publihsed: vx: {0}, wz: {1}".format(twist.linear.x, twist.angular.z))

    except:
        print e

    finally:
        twist = Twist()
        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
        pub.publish(twist)

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

在两个终端中分别启动trd_driver.py和keyboard_teleop.py，并通过键盘的i , j l键盘发布速度消息，观察trd_driver.py的输出。

$ rosrun trd_driver trd_driver.py

$ rosrun trd_driver keyboard_teleop.py

将上述代码封装到类中

为了便于下一步开发，将所有功能封装成类比较好。

#!/usr/bin/python2

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist

class TrdDriver():

    def __init__(self):
        self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
        self.vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.vel_callback)

    def run(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            current_odom = Odometry()
            self.odom_pub.publish(current_odom)
            rate.sleep()

    def vel_callback(self,msg):
        print(msg.linear.x, msg.angular.z)

if __name__=='__main__':
    rospy.init_node('trd_driver_node')
    trd_driver = TrdDriver()
    trd_driver.run()

在节点中加入串口驱动程序，控制底盘转动

TRD电机驱动板卡采用串口控制，其通信方式如下：

I.发送

字节	1	2	3	4	5	6	7
内容	0xea	0x05	0x7e	速度1	速度2	校验	0x0d

发送指令包含7个字节，头尾分别为0xea和0x0d，第4位、第5位分别表示两个电机的速度， 均用一个字节表示。数值为128时速度为零，大于128时电机正转，数值越大速度越快；小于 128时电机反转，数值越小速度越快。其中，第6位为前1~5位字节校验和（求异或）。

II.接收

每次发送速度控制指令后，电机驱动板卡都会回应以下信息。 接收到的数据包含23个字节，头尾分别为0xea和0x0d，4~21位描述了当前电机状态。 除了编码器均用一个字节来表示。两个电机编码器的数值分别用4个字节表示，为大端法表示的有符号整形。 其中，第20位为前1~19位字节校验和（求异或）。

字节	1	2	3	4	5	6	7	8	9
内容	0xea	0x13	0x7e	电压	电流1	电流2	错误码	速度1	速度2

10	11	12~15	16~19	20-21	22	23
实际速度1	实际速度2	编码器1(大端int)	编码器2(大端int)	未用	校验	0x0d

将底盘TRD电机驱动板卡的串口转USB线连接到电脑的USB接口，此时会出现”/dev/ttyUSB0”的串口设备。

$ ls /dev/ttyUSB*

在节点中，引入python的serial串口驱动包，打开”/dev/ttyUSB0”串口，波特率为38400。 然后根据上述通信协议，向电机驱动板卡发送速度控制指令。（可修改源码中的速度值改变电机转速）

#!/usr/bin/python2
# coding: utf-8

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist

import serial

class TrdDriver():

    def __init__(self):
        self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
        self.vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.vel_callback)
        self.ser = serial.Serial('/dev/ttyUSB0', 38400)

    def send(self, cmd):
        self.ser.write(cmd)

    def set_speed(self, v1, v2):
        cmd = [0xea, 0x05, 0x7e, v1, v2, 0x00, 0x0d]
        for i in range(len(cmd)-2):
            cmd[-2] ^= cmd[i]
        print('send cmd:', cmd)
        self.send(cmd)

    def run(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            current_odom = Odometry()
            self.odom_pub.publish(current_odom)
            rate.sleep()

    def vel_callback(self, msg):
        print(msg.linear.x, msg.angular.z)

if __name__=='__main__':
    rospy.init_node('trd_driver_node')
    trd_driver = TrdDriver()
    trd_driver.set_speed(128, 128)
    trd_driver.run()

将当前用户添加到dialout用户组

通常，普通用户对串口设备没有读写权限，如果存在这样的情况，一种方式是通过chmod指令修改串口权限，但这样每次重启系统要重新执行。

sudo chmod 775 /dev/ttyUSB0

要想一劳永逸，最好将当前用户添加到dialout用户组：

sudo usermod -a -G dialout $USER

利用键盘控制底盘运动

在订阅/cmd_vel的回调函数中，根据接收到的速度数据控制底盘运动。

其中，msg.linear.x表示前进或后退速度，单位为m/s。msg.angular.z表示旋转速度，单位为rad/s。

#!/usr/bin/python2
# coding: utf-8

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist

import serial
import time

class TrdDriver():

    def __init__(self):
        self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
        self.vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.vel_callback)
        self.ser = serial.Serial('/dev/ttyUSB0', 38400)
        self.v1 = 128
        self.v2 = 128
        self.linear_coef = 82.0
        self.angular_coef = 14.6

    def send(self, cmd):
        self.ser.write(cmd)

    def set_speed(self, v1, v2):
        cmd = [0xea, 0x05, 0x7e, v1, v2, 0x00, 0x0d]
        for i in range(len(cmd)-2):
            cmd[-2] ^= cmd[i]
        print('send cmd:', cmd)
        self.send(cmd)

    def read_buffer(self):
        result = ''
        while self.ser.inWaiting() > 0:
            result += self.ser.read(1)
        return result

    def run(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            self.set_speed(self.v1, self.v2)
            time.sleep(0.05)
            result = self.read_buffer()
            print('response:', result)
            current_odom = Odometry()
            self.odom_pub.publish(current_odom)
            rate.sleep()

    def vel_callback(self, msg):
        v1 = self.linear_coef * msg.linear.x
        v2 = self.linear_coef * msg.linear.x
        v1 -= self.angular_coef * msg.angular.z
        v2 += self.angular_coef * msg.angular.z
        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
        self.v1 = v1
        self.v2 = v2

if __name__=='__main__':
    rospy.init_node('trd_driver_node')
    trd_driver = TrdDriver()
    trd_driver.run()

运行脚本会打印如下消息：

('send cmd:', [234, 5, 126, 128, 128, 145, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x80\x80\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\r')
('send cmd:', [234, 5, 126, 129, 129, 145, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x81\x81\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\r')
('send cmd:', [234, 5, 126, 131, 131, 145, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x83\x83\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\r')
('send cmd:', [234, 5, 126, 132, 132, 145, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x84\x84\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\r')
('send cmd:', [234, 5, 126, 134, 134, 145, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x86\x86\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\r')
('send cmd:', [234, 5, 126, 134, 137, 158, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x86\x89\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x16\r')
('send cmd:', [234, 5, 126, 134, 140, 155, 13])
('response:', '\xea\x15~\x11\x00\t\x80\x86\x8c\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x13\r')

在两个终端中分别启动trd_driver.py和keyboard_teleop.py，并通过键盘的i , j l键盘发布速度消息，观察底盘运动情况。

$ rosrun trd_driver trd_driver.py

$ rosrun trd_driver keyboard_teleop.py

基于双轮差速模型的底盘里程计计算

差速模型示意图：

根据此模型，实现update_odom方法，可根据获取到的编码器值，计算底盘实时里程，并发布到/odom主题。

#!/usr/bin/python2
# coding: utf-8

import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
import tf

import serial
import time
import struct
import math

class TrdDriver():

    def __init__(self, serialport, baudrate):
        self.odom_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
        self.vel_sub = rospy.Subscriber('/cmd_vel', Twist, self.vel_callback)
        self.ser = serial.Serial(serialport, baudrate)
        self.v1 = 128
        self.v2 = 128
        self.linear_coef = 100.0
        self.angular_coef = 10.0

        self.wheel_diameter = 0.08
        self.base_width = 0.21
        self.encoder_ticks_per_rev = 1980

        self.encoder1 = 0
        self.encoder2 = 0
        self.encoder1_prev = 0
        self.encoder2_prev = 0

        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.time_prev = rospy.Time.now()

    def send(self, cmd):
        self.ser.write(cmd)

    def set_speed(self, v1, v2):
        cmd = [0xea, 0x05, 0x7e, v1, v2, 0x00, 0x0d]
        for i in range(len(cmd)-2):
            cmd[-2] ^= cmd[i]
        print('send cmd:', cmd)
        self.send(cmd)

    def read_buffer(self):
        result = ''
        while self.ser.inWaiting() > 0:
            result += self.ser.read(1)
        return result

    def run(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            self.set_speed(self.v1, self.v2)
            time.sleep(0.05)
            result = self.read_buffer()
            print('response:', result)

            #提取编码器数值
            self.encoder1, = struct.unpack('>i', bytearray(result[11:15]))
            self.encoder2, = struct.unpack('>i', bytearray(result[15:19]))
            print('encoder:', self.encoder1, self.encoder2)

            #计算新的里程计并发布
            self.update_odom()

    def vel_callback(self, msg):
        v1 = self.linear_coef * msg.linear.x
        v2 = self.linear_coef * msg.linear.x
        v1 -= self.angular_coef * msg.angular.z
        v2 += self.angular_coef * msg.angular.z
        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
        self.v1 = v1
        self.v2 = v2

    def update_odom(self):
        encoder1 = self.encoder1
        encoder2 = self.encoder2
        time_current = rospy.Time.now()
        time_elapsed = (time_current - self.time_prev).to_sec()
        self.time_prev = time_current
        dleft = math.pi * self.wheel_diameter * \
                (encoder1 - self.encoder1_prev) / self.encoder_ticks_per_rev
        dright = 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

        self.odom_pub.publish(self.odom)

if __name__=='__main__':
    rospy.init_node('trd_driver_node')
    serialport = rospy.get_param('~serialport', default='/dev/ttyUSB0')
    baudrate = rospy.get_param('~baudrate', default=38400)
    trd_driver = TrdDriver(serialport, baudrate)
    trd_driver.run()

在两个终端中分别启动trd_driver.py和keyboard_teleop.py，并通过键盘的i , j l键盘发布速度消息。 并查看/odom消息内容更新情况。

$ rosrun trd_driver trd_driver.py

$ rosrun trd_driver keyboard_teleop.py

$ rostopic echo /odom

用launch文件启动并加载参数

在trd_driver包下launch文件夹中，修改trd_driver.launch文件，将启动的节点改为 trd_driver.py 。

另外，可以将串口名和波特率传入节点。

<?xml version="1.0"?>
<launch>
    <node name="trd_driver" pkg="trd_driver" type="trd_driver.py">
        <param name="serialport" value="/dev/ttyUSB0" />
        <param name="baudrate" value="38400" />
    </node>
</launch>

考虑到trd_driver.launch文件可能会被传入参数，或被其它launch文件调用，所以最好采用arg标签指定。

<?xml version="1.0"?>
<launch>
    <arg name="serialport" default="/dev/ttyUSB0" />
    <arg name="baudrate"  default="38400" />
    <node name="trd_driver" pkg="trd_driver" type="trd_driver.py">
        <param name="serialport" value="$(arg serialport)" />
        <param name="baudrate" value="$(arg baudrate)" />
    </node>
</launch>