2.4.将程序放到底盘上以及运动模型的标定¶
本节介绍如何将电脑上的程序放到底盘的板卡操作系统上运行，并通过远程进行开发及调试，多个ROS主机的网络配置以及通信。然后在上一节实现的差速控制模型的基础上，介绍如何进行底盘运动的标定，让底盘按照指定的线速度及角速度进行准确的运动，以及对里程进行精确的计算。
网络架构¶
上一节中，通过笔记本电脑的USB口直接跟底盘控制器进行串口通信，发送并接受指令。架构如下：
在实际使用过程中，是不可能让笔记本拖着线跟底盘一起跑的，所以在底盘上安装一个嵌入式板卡–树莓派（Raspberry Pi 3）。 与我们笔记本电脑X86架构不同，这是一款基于ARM架构的板卡，也装有Ubuntu操作系统，并安装了ROS。 处理能力也比较强悍，1G内存，4核处理器。与X86架构相比优势是低功耗，体积小巧，方便安装在底盘上。
在开发和使用过程中，将笔记本电脑跟树莓派嵌入式板卡通过网络连接起来，进行远程控制和调试，架构如下：
远程开发模式¶
安装上图网络连接好后，在笔记本电脑上，连接到名为”Venus-05”的路由器上，密码为：12345678。
连接后，执行：
$ ifconfig
即可看到本机的IP地址，例如，此处我的笔记本电脑显示如下信息：
wlp3s0 Link encap:Ethernet HWaddr b8:ee:65:28:f3:ae
inet addr:192.168.5.100 Bcast:192.168.5.255 Mask:255.255.255.0
可见IP地址为192.168.5.100。接下来可使用nmap指令列出路由器所在网段下连接的所有设备的IP：
$ nmap -sP 192.168.5.0/24
如果提示 找不到nmap指令 ，则需要先安装nmap。联网后，执行以下指令：
$ sudo apt install nmap
会得到以下输出：
Starting Nmap 7.01 ( https://nmap.org ) at 2019-01-15 14:03 CST
Nmap scan report for 192.168.5.1
Host is up (0.048s latency).
Nmap scan report for 192.168.5.100
Host is up (0.00086s latency).
Nmap scan report for 192.168.5.108
Host is up (0.011s latency).
Nmap done: 256 IP addresses (3 hosts up) scanned in 16.81 seconds
此时会列出树莓派的IP地址为192.168.5.108，此时可通过SSH进行连接（登录用户名为：pi，密码为：1）。并在板卡上建立一个名为tutorial_ws的空文件夹。
$ ssh pi@192.168.5.108
(远程终端)$ mkdir tutorial_ws
首次连接时，会出现询问：
The authenticity of host '192.168.5.108 (192.168.5.108)' can't be established.
ECDSA key fingerprint is SHA256:EfOywkQj6mliIyg8KumdsZP/paXBmmdj00/NxPX0h6Y.
Are you sure you want to continue connecting (yes/no)?
则输入 yes 并回车确定。然后输入登录密码。
在多数情况下，我们可以在笔记本电脑上进行开发，然后使用rsync指令将结果部署到底盘板卡上。 例如在笔记本电脑的catkin_ws路径下，将src文件夹(包含了所有的ROS包源代码)部署到远程板卡的tutorial_ws目录下。
$ cd ~/catkin_ws
$ rsync -avz --delete --exclude="*.swp" src pi@192.168.5.108:/home/pi/tutorial_ws/
这段指令的意思将本机的当前路径的src文件夹，拷贝到远程板卡的/home/pi/tutorial_ws文件夹下。 使用rsync而不是scp的好处是，rsync只会拷贝差异，当只改变个别文件时，只会将改变的文件拷贝到远程，而不是全部拷贝。 这里–delete选项是指本地删除某个文件，同步后远程也会删除此文件，–exclude是指不会拷贝后缀名为swp的文件(swp为临时打开文件)。
部署完成后，可在远程板卡的tutorial_ws路径下执行catkin_make进行编译构建。
(远程终端)$ cd ~/tutorial_ws
(远程终端)$ catkin_make
利用树莓派板卡控制底盘运动¶
将源代码部署到远程板卡，构建成功后，还需要将工作区导出到环境变量，方法与在笔记本电脑相同，即：
(远程终端)$ echo "source ~/tutorial_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 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()
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()
硬件参数的测量¶
在位置解算时，需要知道轮子的直径，以及两个轮子之间的距离。 在代码中分别对应wheel_diameter和base_width参数。 测量后，轮子的直径为8cm，两轮子距离为21cm。修改以下代码：
self.wheel_diameter = 0.08
self.base_width = 0.21
另外，根据编码器型号，我们可以查到其每转1圈会有1980个脉冲，修改以下代码：
self.encoder_ticks_per_rev = 1980
底盘线速度标定¶
- 首先查看底盘行走一段距离，编码器计算的到的/odom里程数据准不准，如何查看？
- 记下实际运行距离，以及里程计输出的距离，需要修改那些参数使得两个距离一致？
- 如何标定线速度控制系数？
底盘角速度标定¶
- 参考线速度标定步骤，如何标定角速度控制系数？(提示：/odom里程消息中有解算得到的速度信息)
综合测试¶
- 如何测试标定结果是否准确？