课程3.将程序放到底盘上以及运动模型的标定 - TonyRobotics/trbase GitHub Wiki

本节介绍如何将电脑上的程序放到底盘的板卡操作系统上运行，并通过远程进行开发及调试，多个ROS主机的网络配置以及通信。然后在上一节实现的差速控制模型的基础上，介绍如何进行底盘运动的标定，让底盘按照指定的线速度及角速度进行准确的运动，以及对里程进行精确的计算。

网络架构

上一节中，通过笔记本电脑的USB口直接跟底盘控制器进行串口通信，发送并接受指令。架构如下：

在实际使用过程中，是不可能让笔记本拖着线跟底盘一起跑的，所以在底盘上安装一个嵌入式板卡--Odroid XU4。与我们笔记本电脑X86架构不同，这是一款基于ARM架构的板卡，也装有Ubuntu操作系统，并安装了ROS。处理能力也比较强悍，2G内存，8核处理器。与X86架构相比优势是低功耗，体积小巧，方便安装在底盘上。

在开发和使用过程中，将笔记本电脑跟ARM板卡通过网络连接起来，进行远程控制和调试，架构如下：

远程开发模式

安装上图网络连接好后，在笔记本电脑上，连接到名为"TR"的路由器上，密码为：12345678。

连接后，执行：

$ ifconfig

即可看到本机的IP地址，例如，此处我的笔记本电脑显示如下信息：

wlp3s0    Link encap:Ethernet  HWaddr b8:ee:65:28:f3:ae  
          inet addr:192.168.5.101  Bcast:192.168.5.255  Mask:255.255.255.0

可见IP地址为192.168.5.101。接下来可使用nmap指令列出路由器所在网段下连接的所有设备的IP：

$ nmap -sP 192.168.5.0/24

会得到以下输出：

Nmap scan report for 192.168.5.1
Host is up (0.034s latency).
Nmap scan report for 192.168.5.101
Host is up (0.000046s latency).
Nmap scan report for 192.168.5.102
Host is up (0.039s latency).

此时会列出Odroid XU4的IP地址为192.168.5.102，此时可通过SSH进行连接。并在板卡上建立一个名为catkin_ws的空文件夹。

$ ssh [email protected]
(远程终端)$ mkdir catkin_ws

在多数情况下，我们可以在笔记本电脑上进行开发，然后使用rsync指令将结果部署到底盘板卡上。例如在笔记本电脑的catkin_ws路径下，将src文件夹(包含了所有的ROS包源代码)部署到远程板卡的catkin_ws目录下。

$ cd ~/catkin_ws
$ rsync -avz --delete --exclude="*.swp" src [email protected]:/home/odroid/catkin_ws/

这段指令的意思将本机的当前路径的src文件夹，拷贝到远程板卡的/home/odroid/catkin_ws文件夹下。使用rsync而不是scp的好处是，rsync只会拷贝差异，当只改变个别文件时，只会将改变的文件拷贝到远程，而不是全部拷贝。这里--delete选项是指本地删除某个文件，同步后远程也会删除此文件，--exclude是指不会拷贝后缀名为swp的文件(swp为临时打开文件)。

部署完成后，可在远程板卡的catkin_ws路径下执行catkin_make进行编译构建。

(远程终端)$ cd ~/catkin_ws
(远程终端)$ catkin_make

利用Odroid XU4板卡控制底盘运动

将源代码部署到远程板卡，构建成功后，还需要将工作区导出到环境变量，方法与在笔记本电脑相同，即：

(远程终端)$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
(远程终端)$ source ~/.bashrc

此时可在远程终端执行ROS节点。先启动roscore：

(远程终端)$ roscore

然后分别启动trd_driver.py底盘驱动节点，和keyboard_teleop.py键盘控制节点。

新打开终端，执行：

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

新打开终端，执行：

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

此时可通过i , j l k按键分别控制底盘前进、后退、左转、右转和停止。

此处，keyboard_teleop.py节点控制底盘运动，会发现速度从0增加到最大值时间较长，也就是加速较慢。仔细查看此节点代码，思考如何修改，可以使加速度较快，或瞬间加速到最大值。

控制延迟问题

此时遥控底盘运动，请思考当keyboard_teleop.py发送频率大于trd_driver循环频率是会发生什么情况？

当停止发送指令时，trd_driver订阅速度的话题队列为非空，会继续接收并执行队列中的消息。

可查看Subscriber方法中queue_size参数的定义。

@param queue_size: maximum number of messages to receive at
                   a time. This will generally be 1 or None (infinite,
                   default).

此时，需要将queue_size改为1，即可解决控制延迟问题。

编码器初始偏置问题

前提：除非重启运动控制板卡，否则编码器的读数是不会归零的。

这一前提会给调试带来不便，想想为什么。

下面我们在代码中加入两个变量来存储每次启动时的初始偏置，用读到的数值减去偏置即可得到相对编码器读数。

修改后的代码如下：

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

from threading import Lock
lock = Lock()

class TrdDriver():

    def __init__(self, serialport, baudrate):
        self.pub = rospy.Publisher('/odom', Odometry, 
                                    queue_size=100) 
        self.sub = rospy.Subscriber('/cmd_vel', Twist, 
                                    self.vel_callback, 
                                    queue_size=1)
        self.ser = serial.Serial(serialport, baudrate)

        self.wheel_diameter = 0.1
        self.base_width = 0.5
        self.encoder_ticks_per_rev = 4400

        self.first_flag = True
        self.encoder1_offset = 0
        self.encoder2_offset = 0
        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):
        #print('send cmd:', cmd)
        self.ser.write(cmd)

    def read_buffer(self):
        time.sleep(0.05)
        res = ''
        while self.ser.inWaiting() > 0:
            res += self.ser.read(1)
        res = bytearray(res)
        if res[0:2]=='?C' and res[-1]==13:
            self.encoder1 = int(res.split(':')[0].split('=')[1])
            self.encoder2 = -int(res.split(':')[1][:-1])
            if self.first_flag:
                self.encoder1_offset = self.encoder1
                self.encoder2_offset = self.encoder2
                self.first_flag = False
            self.encoder1 -= self.encoder1_offset
            self.encoder2 -= self.encoder2_offset
            print('Encoder {} {}'.format(self.encoder1, self.encoder2))
        return res

    def get_encoder(self):
        cmd = '?C\r'
        self.send(cmd)

    def set_speed(self, v1, v2):
        print('{} Set Speed: {} {}'.format(time.time(), v1, v2))
        cmd = '!M {} {}\r'.format(v1, v2)
        self.send(cmd)

    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

    def run(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            # 读编码器值
            lock.acquire()
            self.get_encoder()
            self.read_buffer()
            lock.release()
            # 更新计算并更新里程信息
            self.update_odom()
            # 发布里程信息
            self.pub.publish(self.odom)
            rate.sleep()

    def vel_callback(self,msg):
        lock.acquire()
        v1 = 400*msg.linear.x
        v1 += 100*msg.angular.z
        v2 = 400*msg.linear.x
        v2 -= 100*msg.angular.z
        self.set_speed(v1, -v2)
        self.read_buffer()
        lock.release()

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

硬件参数的测量

在位置解算时，需要知道轮子的直径，以及两个轮子之间的距离。在代码中分别对应wheel_diameter和base_width参数。测量后，轮子的直径为7cm，两轮子距离为40cm。修改以下代码：

        self.wheel_diameter = 0.07
        self.base_width = 0.4

另外，根据编码器型号k3808-1000bm-c526，我们可以查到其每转1圈会有4000个脉冲，修改以下代码：

        self.encoder_ticks_per_rev = 4000

底盘线速度标定

首先查看底盘行走一段距离，编码器计算的到的/odom里程数据准不准，思考如何查看？
记下实际运行距离，以及里程计输出的距离，思考需要修改那些参数使得两个距离一致？
思考如何标定线速度控制系数？

底盘角速度标定

参考线速度标定步骤，思考如何标定角速度控制系数？(提示：/odom里程消息中有解算得到的速度信息)

综合测试

思考如何测试标定结果是否准确？