2.5.标定结果测试

本节将对上节标定的运动参数进行多方位测试，将所有参数放到launch文件中进行修改调试。待参数满足要求精度后，即可作为底盘的驱动程序。

将所有参数放到launch文件中

修改trd_driver.py文件，将所有参数用ROS的get_param()方法获取：

# 使用ROS param设定参数值
self.linear_coef = rospy.get_param('~linear_coef', default=100.0)
self.angular_coef = rospy.get_param('~angular_coef', default=10.0)

self.wheel_diameter = rospy.get_param('~wheel_diameter', default=0.08)
self.base_width = rospy.get_param('~base_width', default=0.21)
self.encoder_ticks_per_rev = rospy.get_param('~encoder_ticks_per_rev', default=1980)

修改后内容如下：

#!/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

        # 使用ROS param设定参数值
        self.linear_coef = rospy.get_param('~linear_coef', default=100.0)
        self.angular_coef = rospy.get_param('~angular_coef', default=10.0)

        self.wheel_diameter = rospy.get_param('~wheel_diameter', default=0.08)
        self.base_width = rospy.get_param('~base_width', default=0.21)
        self.encoder_ticks_per_rev = rospy.get_param('~encoder_ticks_per_rev', default=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()

        self.is_first_time = True
        self.encoder1_offset = 0
        self.encoder2_offset = 0

    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)

            if self.is_first_time:
                self.encoder1_offset = self.encoder1
                self.encoder2_offset = self.encoder2
                self.is_first_time = False
            self.encoder1 -= self.encoder1_offset
            self.encoder2 -= self.encoder2_offset
            print('encoder-offset:', 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.launch文件，加入对应参数数值。 注意，此处底盘的轮径为0.08米，轮距为0.21米，编码器为1980线，控制速度线速度系数和角速度系数分别为100.0和10.0。

<?xml version="1.0"?>
<launch>
    <arg name="serialport" default="/dev/ttyUSB0" />
    <arg name="baudrate" default="38400" />
    <arg name="linear_coef" default="100.0" />
    <arg name="angular_coef" default="10.0" />
    <arg name="wheel_diameter" default="0.08" />
    <arg name="base_width" default="0.21" />
    <arg name="encoder_ticks_per_rev" default="1980" />

    <node name="trd_driver" pkg="trd_driver" type="trd_driver.py">
        <param name="serialport" value="$(arg serialport)" />
        <param name="baudrate" value="$(arg baudrate)" />
        <param name="wheel_diameter" value="$(arg wheel_diameter)" />
        <param name="base_width" value="$(arg base_width)" />
        <param name="encoder_ticks_per_rev" value="$(arg encoder_ticks_per_rev)" />
        <param name="linear_coef" value="$(arg linear_coef)" />
        <param name="angular_coef" value="$(arg angular_coef)" />
    </node>
</launch>

测试参数是否满足精度

启动驱动节点和键盘控制节点，遥控底盘运动，并查看/odom主题内容。

(远程终端)$ roslaunch trd_driver trd_driver.launch

(远程终端)$ rosrun trd_driver keyboard_teleop.py

(远程终端)$ rostopic echo /odom

观察/odom中的Position位置读数以及速度Twist读数，看看发现什么问题？如何调整参数进行校准？

如何测试，可以确保上述参数是准确的？

最终参数：

<arg name="linear_coef" default="100.0" />
<arg name="angular_coef" default="10.0" />
<arg name="wheel_diameter" default="0.08" />
<arg name="base_width" default="0.21" />
<arg name="encoder_ticks_per_rev" default="1980" />

何解决usb断线重连问题

由于底盘运行过程中会出现颠簸，难免导致USB接口出现断开的情况。因此需要在代码中加入重连机制。

提示 在TrdDriver类中定重连成员方法：

def reconnect(self):
    print('{} reconnecting...'.format(time.time()))
    while True:
        try:
            self.ser = serial.Serial(self.serialport, self.baudrate)
        except:
            time.sleep(1)
            print('{} reconnecting...'.format(time.time()))
        else:
            print('{} reconnected!'.format(time.time()))
            break

提示 发送指令出现异常时重连：

def send(self, cmd):
    while True:
        try:
            self.ser.write(cmd)
            break
        except serial.serialutil.SerialException:
            self.reconnect()

提示 接收指令出现异常时重连：

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

采用汤尼机器人官方trbase包

至此，差速电机驱动包的开发完成。汤尼机器人官方包 trbase 中包含了所有开发所用到的包，不仅包括trd_driver电机驱动包， 还包含传感器驱动、所有底盘建图及导航包，可以下载此包进行部署编译。

将trd_driver包备份到其它位置，将trbase包克隆到本地~/catkin_ws/src下。

$ cd ~/catkin_ws/src
$ mv trd_driver ~/Documents/
$ git clone https://github.com/TonyRobotics/trbase

如果未联网，可暂时将树莓派下~/catkin_ws工作区中的trbase包复制过来：

$ cd ~/catkin_ws/src
$ mv trd_driver ~/Documents/
$ rsync -avz pi@192.168.5.108:/home/pi/catkin_ws/src/trbase ./

然后部署到树莓派的 tutorial_ws 工作区下：

$ cd ~/catkin_ws
$ rsync -avz --delete --exclude="*.swp" src pi@192.168.5.108:/home/pi/tutorial_ws/

编译运行即可。