第七天：越疆小车的建图和导航，修改相关参数使得建图无重影无空缺更完整，使用python代码实现自动导航功能

119 阅读 0 评论 79 点赞

我是靠谱客的博主优美航空，这篇文章主要介绍第七天：越疆小车的建图和导航，修改相关参数使得建图无重影无空缺更完整，使用python代码实现自动导航功能，现在分享给大家，希望可以做个参考。

文章目录

- - 一、使用gmapping 算法构建地图
  - - Slam 算法介绍
    - - HectorSLAM
      - Gmapping
      - KartoSLAM
      - Cartographer
      - 步骤如下（参考官方给出的步骤，我会注释需要注意的地方）：
  - 二、gmmaping算法相关参数解释
  - 三、navigation 自主导航
  - 四、总结

这一篇的建图是后面功能实现的基础，图建不好，会严重影响后面的导航和抓取，所以建图尽量准确，能够省去很多麻烦事。

一、使用gmapping 算法构建地图

这里的gmapping算法只做了解，可以不用去深入学习算法本身的知识，有兴趣或者专业的可以深入学习，因为已经写好了launch文件，可以直接调用完成建图工作。

Gmapping 是一种高效的 Rao-Blakwellized 粒子滤波器，用于根据激光雷达测距数据来生成 2D栅格地图，ROS gmapping 是 OpenSLAM 下的 GMapping 的再封装移植版本。

Slam 算法介绍

SLAM 的英文全称是 Simultaneous Localization and Mapping，中文称作同时定位与地图创
建,它主要用于解决机器人在未知环境运动时的定位和地图构建问题。SLAM 有很多种分类方法。按照传感器的不同，可以分为基于激光雷达的 2D/3D SLAM、基于深度相机的 RGBD SLAM、基于视觉传感器的 visual SLAM（以下简称 vSLAM），我们主要使用的是基于激光雷达的 2D SLAM，根据算法的不同，激光 2D SLAM 又可以分为：
基于粒子滤波的 slam 算法，常用的有 HectorSLAM，Gmapping 算法
基于图优化的 slam 算法，常用的有 KartoSLAM，Cartographer 算法

HectorSLAM

HectorSLAM 是一种结合了鲁棒性较好的扫描匹方法 2D SLAM 方法和使用惯性传感系统的导航技术。
优点：不依赖里程计，适应无人机及地面不平坦的区域
缺点：对传感器性能要求非常高，激光雷达必须是高频率(40HZ 以上)且测量噪声很小的

Gmapping

Gmapping 是一种基于激光的 SLAM 算法，它已经集成在 ROS 中，是移动机器人中使用最多的SLAM 算法。这个算法是由 Grisetti 等人提出的一种基于 Rao-Blackwellized 的粒子滤波 SLAM 方法。
优点：对激光雷达频率要求低、鲁棒性高，能有效利用了车轮里程计信息，在构建小场景地图时，速度快，计算量小
缺点：严重依赖里程计，无法适应无人机及地面不平坦的区域，无回环（激光 SLAM 很难做回环检测），大的场景，粒子较多的情况下，特别消耗资源。

KartoSLAM

KartoSLAM 是基于图优化的方法，用高度优化和非迭代 cholesky 矩阵进行稀疏系统解耦作为解．图优化方法利用图的均值表示地图，每个节点表示机器人轨迹的一个位置点和传感器测量数据集，箭头的指向的连接表示连续机器人位置点的运动，每个新节点加入，地图就会依据空间中的节点箭头的约束进行计算更新．
优点：对传感器要求低，相对 gmapping 算法，它能构建大场景地图，有回环检测功能
缺点：计算量大，内存需求很大，稳定性不够，构建大地图时，可能会导致算法程序死掉

Cartographer

cartographer 是 Google 的实时室内建图项目，它也是基于图优化的 SLAM 方法，可以生成分辨率为 5cm 的 2D 格网地图。获得的每一帧 laser scan 数据，利用 scan match 在最佳估计位置处插入子图（submap）中，且 scan matching 只跟当前 submap 有关。在生成一个 submap 后，会进行一次局部的回环（loop close），利用分支定位和预先计算的网格，所有 submap 完成后，会进行全局的回环，相对 KartoSLAM，Cartographer 采取优化库的是 google 的 ceres 构建 problem 优化。
优点：适用大场景建图，能进行闭环检测，稳定性好
缺点：计算量大，内存需求很大

步骤如下（参考官方给出的步骤，我会注释需要注意的地方）：

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
第一步：
参考第三天那篇博文，实现虚拟机和小车的远程连接工作；

第二步：
使用远程连接工具xshell或者直接虚拟机ssh，进入小车的命令行中，输入如下指令，启动小车的建图：
roslaunch dashgo_nav gmapping_imu.launch #启动建图 launch

第三步：
在电脑中直接打开 rviz，观察地图：
export ROS_MASTER_URI=http://192.168.31.200:11311 #这个IP和端口是小车固定的，根据相应的官方文件进行修改
roslaunch dashgo_rviz view_navigation.launch #启动rviz

第四步
再在xshell中打开一个连接或者虚拟机中打开新的命令行，连接上小车，输入如下指令，就可以开始对小车控制
rosrun dashgo_tools teleop_twist_keyboard.py #启动键盘控制移动
更加提示可以看到可以控制八个方向
u	i	o
j	k	l
m	,	.
然后就控制小车四面八方的跑动，开始建图，建图实时效果可以根据rviz进行监控，如果看懂有些地方效果不好，可以多跑一下。

第五步
建图完成，就需要保存当前地图，再次打开一个窗口，输入以下指令：
roscd dashgo_nav/maps 		#进入地图目录
rosrun map_server map_saver –f eai_map_imu 	#保存地图为 eai_map_imu

这样一张地图就建出来了，不需要我们写什么，相关功能已经写好封装在lanunch文件中了，
可以查找原文件进行学习优化，我们所要做的就是根据实际情况修改参数值。具体操作如下。

二、gmmaping算法相关参数解释

复制代码

<launch>
 <arg name="scan_topic" default="scan" ></arg> //指定雷达消息主题为/scan
 <arg name="base_frame" default="base_footprint"></arg> //指定底盘坐标系
 <arg name="odom_frame" default="odom_combined"></arg> //指定里程计坐标系
 <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" respawn="true" >
 <param name="base_frame" value="$(arg base_frame)"/>
 <param name="odom_frame" value="$(arg odom_frame)"/>
 <param name="map_update_interval" value="1"/> //地图更新的时间间隔
 <param name="maxUrange" value="8.0"/> //雷达可用的最大有效测距值，
//一般情况下设置 maxUrange < 雷达的现实实际测距值 <= maxRange
 <param name="maxRange" value="10.0"/> //maxRange 是雷达的理论最大测距值
120
 <param name="sigma" value="0.05"/> //endpoint 匹配标准差
 <param name="kernelSize" value="3"/> //最多使用内核数
 <param name="lstep" value="0.05"/> //优化机器人移动的初始值（距离）
 <param name="astep" value="0.05"/> //优化 optimize 机器人移动的初始值（角度）
 <param name="iterations" value="5"/> //扫描匹配器的迭代次数
 <param name="lsigma" value="0.075"/> //用于计算的波束可能性的 sigma
 <param name="ogain" value="3.0"/> //在评估可能性时使用的增益，用于平滑重采样效果
 <param name="lskip" value="0"/> //为 0,表示所有的激光都处理
 <param name="minimumScore" value="30"/> //最小匹配得分，这个参数很重要，它决定了对激光的一个置信度，
//越高说明对激光匹配算法的要求越高，激光的匹配也越容易失败而
//转去使用里程计数据，而设的太低又会使地图中出现大量噪声，
//所以需要权衡调整
 <param name="srr" value="0.01"/> //运动模型的噪声参数
 <param name="srt" value="0.02"/> //运动模型的噪声参数
 <param name="str" value="0.01"/> //运动模型的噪声参数
 <param name="stt" value="0.02"/> //运动模型的噪声参数
 <param name="linearUpdate" value="0.05"/> //机器人移动 linearUpdate 距离，进行 scanmatch
 <param name="angularUpdate" value="0.0436"/> //机器人选装 angularUpdate 角度，进行 scanmatch 
 <param name="temporalUpdate" value="-1.0"/> //如果上次扫描处理的时间早于更新时间（秒），则处理扫描。
//小于零的值将关闭基于时间的更新
 <param name="resampleThreshold" value="0.5"/> //基于 Neff 的重采样阈值
 <param name="particles" value="50"/> //粒子个数,用于粒子滤波算法
 <param name="xmin" value="-1.0"/> //map 初始化的大小
 <param name="ymin" value="-1.0"/>
 <param name="xmax" value="1.0"/>
 <param name="ymax" value="1.0"/>
 <param name="delta" value="0.05"/> //地图的分辨率
 <param name="llsamplerange" value="0.01"/> //可能性的平移采样范围
 <param name="llsamplestep" value="0.01"/> //可能性的平移采样步骤
 <param name="lasamplerange" value="0.005"/> //角度采样范围的可能性
 <param name="lasamplestep" value="0.005"/> //角度采样步骤的可能性
 <remap from="scan" to="$(arg scan_topic)"></remap>
 </node>
</launch>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
<launch>
 <arg name="scan_topic" default="scan" /> //指定雷达消息主题为/scan
 <arg name="base_frame" default="base_footprint"/> //指定底盘坐标系
 <arg name="odom_frame" default="odom_combined"/> //指定里程计坐标系
 <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" respawn="true" >
 <param name="base_frame" value="$(arg base_frame)"/>
 <param name="odom_frame" value="$(arg odom_frame)"/>
 <param name="map_update_interval" value="1"/> //地图更新的时间间隔
 <param name="maxUrange" value="8.0"/> //雷达可用的最大有效测距值，
//一般情况下设置 maxUrange < 雷达的现实实际测距值 <= maxRange
 <param name="maxRange" value="10.0"/> //maxRange 是雷达的理论最大测距值
120
 <param name="sigma" value="0.05"/> //endpoint 匹配标准差
 <param name="kernelSize" value="3"/> //最多使用内核数
 <param name="lstep" value="0.05"/> //优化机器人移动的初始值（距离）
 <param name="astep" value="0.05"/> //优化 optimize 机器人移动的初始值（角度）
 <param name="iterations" value="5"/> //扫描匹配器的迭代次数
 <param name="lsigma" value="0.075"/> //用于计算的波束可能性的 sigma
 <param name="ogain" value="3.0"/> //在评估可能性时使用的增益，用于平滑重采样效果
 <param name="lskip" value="0"/> //为 0,表示所有的激光都处理
 <param name="minimumScore" value="30"/> //最小匹配得分，这个参数很重要，它决定了对激光的一个置信度，
//越高说明对激光匹配算法的要求越高，激光的匹配也越容易失败而
//转去使用里程计数据，而设的太低又会使地图中出现大量噪声，
//所以需要权衡调整
 <param name="srr" value="0.01"/> //运动模型的噪声参数
 <param name="srt" value="0.02"/> //运动模型的噪声参数
 <param name="str" value="0.01"/> //运动模型的噪声参数
 <param name="stt" value="0.02"/> //运动模型的噪声参数
 <param name="linearUpdate" value="0.05"/> //机器人移动 linearUpdate 距离，进行 scanmatch
 <param name="angularUpdate" value="0.0436"/> //机器人选装 angularUpdate 角度，进行 scanmatch 
 <param name="temporalUpdate" value="-1.0"/> //如果上次扫描处理的时间早于更新时间（秒），则处理扫描。
//小于零的值将关闭基于时间的更新
 <param name="resampleThreshold" value="0.5"/> //基于 Neff 的重采样阈值
 <param name="particles" value="50"/> //粒子个数,用于粒子滤波算法
 <param name="xmin" value="-1.0"/> //map 初始化的大小
 <param name="ymin" value="-1.0"/>
 <param name="xmax" value="1.0"/>
 <param name="ymax" value="1.0"/>
 <param name="delta" value="0.05"/> //地图的分辨率
 <param name="llsamplerange" value="0.01"/> //可能性的平移采样范围
 <param name="llsamplestep" value="0.01"/> //可能性的平移采样步骤
 <param name="lasamplerange" value="0.005"/> //角度采样范围的可能性
 <param name="lasamplestep" value="0.005"/> //角度采样步骤的可能性
 <remap from="scan" to="$(arg scan_topic)"/>
 </node>
</launch>

主要可以修改以下两个参数，优化我们的建图，至于其它参数的额详解可以自行网上搜索学习。
name=“minimumScore” value=“30”//最小匹配得分，这个参数很重要，它决定了对激光的一个置信度，
name=“particles” value=“50” //粒子个数,用于粒子滤波算法

三、navigation 自主导航

这里就不做详细解释了，直接上代码，具体可以参看相关注释。

复制代码

先简单说一下这里的思路。
首先我们要明白要去哪些点，根据比赛要求，是15个快递的，然后是10个投递目标点，以及1个起始点。
所以需要确定的是26个点。但是这个点属实有点多，不可能一个个去测，所以首先可以确定10个投递点，
这10个点事一定要测试的，根据前面的博文，测得10个点以后，只需要修改程序中对应的数组中10个省份。
加上起始点，11个点是一定要测试的。其次是邮件的抓取点，由于要求是邮件基本均匀摆放，
根据场地和数量可以算出邮件中心之间间距15CM，所以这11个点我们只需要改变Y轴坐标，
进行累加，其它的值包括X轴，Z轴，以及另外四个位姿值不变。
但是这里可能会出现误差，所以我们导航到下一个点是，可能邮件并不在我们的镜头中，所以需要进行调整。
我的调整方案是，左右依次旋转一定角度，在导航误差不大的情况下，就可以正常找到邮件并识别，
当然我也提到是误差不大，误差大了相当与直接放弃抓取，去往下一个，由于赛题是对抗赛，所以放弃抓取未尝不可，
可以更快的进入下一个邮件的识别抓取。
其次，我这个方案还有一个比较大的问题就是有点浪费时间，我是左右分别等待了10~15S左右，
要等待镜头和识别稳定下来，所以万一这一次完全没识别失败了，那么可能就会浪费30s。
这里我也简单的做了一个假设方案，我们可以采取识别，使用颜色分割，上面提到邮件不显示完全，
就无法正常识别，往往我们可以在图像中找到对应的黄色区域（赛题中做了要求，背景为蓝色，邮件为黄色）
然后我们可以根据黄色区域在我们画面的位置，来控制机械臂往对应方向移动。
但是我们最终没有去实现，一个是比较麻烦，得去做图像这一块的处理，加上机械臂的运动范围有限，可能目的范围会超出机械臂的范围，
而且这个范围是不好确定的，具体的可以参考越疆机械臂具体资料。
其次是再进行图像处理可能又会增加硬件的压力，耗时会大大加大，毕竟是部署在单片机上，性能远不比电脑（文字识别用的最小模型，帧率上都比较低，具体后面说）
所以综上所述，还是采取左右固定移动的方式，希望建图和导航不会出现较大的误差！

正式上代码（我不列出全部代码，以防影响大家，有疑问可交流，主要介绍一下各个功能函数）

'''
-------------------------------------------------------------------------------------------------
功能：导航停止
参数：无
讲解：
导航结束停止或者出现异常停止
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def shutdown():
    rospy.loginfo("Stopping the robot...")
    # Cancel any active goals
    move_base.cancel_goal()
    rospy.sleep(2)
    # Stop the robot
    cmd_vel_pub.publish(Twist())
    rospy.sleep(1)
'''
-------------------------------------------------------------------------------------------------
功能：读取导航点
参数：txt文件名
讲解：
将10个导航点存储在txt中，第一个是起始点，剩余十个对应邮递箱的点，其顺序和
代码数组中的10个省份对应，这样方便修改和写入，不改动代码
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
def read_goals(filename):
    # 定义导航点信息
    global nav_pointcnt
    quaternions = []		#坐标点
    Position = []			#位姿点
    data = []
    # 读入txt数据
    with open(filename) as f:
        for line in f.readlines():
            temp = line.split()
            data.append(temp)
    # 把数据解析存入目标点列表
    for n in data:
        x, y, z = float(n[0]), float(n[1]), float(n[2])
        x1, y1, z1, w = float(n[3]), float(n[4]), float(n[5]), float(n[6])
        data1 = Point(x, y, z)
        data2 = Quaternion(x1, y1, z1, w)
        Position.append(data1)
        quaternions.append(data2)
    nav_pointcnt = len(Position)
    for i in range(0, nav_pointcnt):
        waypoints.append(Pose(Position[i], quaternions[i])) #pose功能函数自带，使用即可
    print"read goals finish!!"
    return nav_pointcnt

'''
-------------------------------------------------------------------------------------------------
功能：导航移动
参数：目标点
讲解：
使用move_base.send_goal(goal)函数，实现导航功能，这里的自动导航已经用此函数进行封装了，所以我们直接使用就行
至于实现方法有兴趣可以找到源文件学习，这里不做讲解
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def move(goal):
    # 发送移动目标点
    move_base.send_goal(goal)
    #等待超时，单位 秒
    finished_within_time = move_base.wait_for_result(rospy.Duration(70))
    # 如果超时取消本目标点的导航，进入下一步
    if not finished_within_time:
        move_base.cancel_goal()
        rospy.loginfo("Timed out achieving goal")
    else:
        # 否则导航完成，获取状态
        state = move_base.get_state()
        # 如果返回GoalStatus.SUCCEEDED，则导航成功
        if state == GoalStatus.SUCCEEDED:
            state = True
            rospy.loginfo("Goal succeeded!")
            return state

'''
-------------------------------------------------------------------------------------------------
功能：机械臂状态回调函数
参数：状态值
讲解：
本函数主要接收抓取结束时和投递结束时的状态，以此来判断是移动向投递箱还是分拣台
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def dobot_statecallback(data):
    global dobot_grap_state, dobot_deliver_state, move_state, box_index
    try:
        dobot_grap_state = data.dobot_grap_flag
        dobot_deliver_state = data.dobot_deliver_flag
        box_index = data.deliver_box_index
        move_state = True
        #print("dobot_grap_state: ", dobot_grap_state,"deliver_state: ", dobot_deliver_state)
        print("box_index: ", box_index, "move_state:", move_state)
        return dobot_deliver_state, dobot_deliver_state, move_state, box_index
    except:
        print("receive dobot_state error")

'''
-------------------------------------------------------------------------------------------------
功能：导航主函数
参数：无
讲解：
只展示初始化和话题的接收和创建，只有后面的导航逻辑这里省略，主要是更加上面的回调函数判断过程，然后执行相应的操作
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def main():
    global dobot_grap_state, dobot_deliver_state ,move_state, box_index, 
        nav_grap_state, nav_deliver_state, nav_pointcnt
    pointcnt = 0
    para_move = 0.13			#邮件移动的常数值
    rospy.init_node('send_goal', anonymous=False)
    rospy.on_shutdown(shutdown)
    rospy.loginfo("Waiting for move_base action server...")
    pub_nav_state = rospy.Publisher('/navigational_state', navigational_state, queue_size=10)  # 发布数据
    rospy.Subscriber('/dobot_state', dobot_state, dobot_statecallback)
    # 等待服务器
    move_base.wait_for_server(rospy.Duration(20))
    rospy.loginfo("Connected to move base server")
    nav_pointcnt = read_goals("/home/eaibot/moveit_ws/goals.txt")

rate = rospy.Rate(10)

以下代码省略，主要就是调用上面的函数，判断小车的移动逻辑。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
先简单说一下这里的思路。
首先我们要明白要去哪些点，根据比赛要求，是15个快递的，然后是10个投递目标点，以及1个起始点。
所以需要确定的是26个点。但是这个点属实有点多，不可能一个个去测，所以首先可以确定10个投递点，
这10个点事一定要测试的，根据前面的博文，测得10个点以后，只需要修改程序中对应的数组中10个省份。
加上起始点，11个点是一定要测试的。其次是邮件的抓取点，由于要求是邮件基本均匀摆放，
根据场地和数量可以算出邮件中心之间间距15CM，所以这11个点我们只需要改变Y轴坐标，
进行累加，其它的值包括X轴，Z轴，以及另外四个位姿值不变。
但是这里可能会出现误差，所以我们导航到下一个点是，可能邮件并不在我们的镜头中，所以需要进行调整。
我的调整方案是，左右依次旋转一定角度，在导航误差不大的情况下，就可以正常找到邮件并识别，
当然我也提到是误差不大，误差大了相当与直接放弃抓取，去往下一个，由于赛题是对抗赛，所以放弃抓取未尝不可，
可以更快的进入下一个邮件的识别抓取。
其次，我这个方案还有一个比较大的问题就是有点浪费时间，我是左右分别等待了10~15S左右，
要等待镜头和识别稳定下来，所以万一这一次完全没识别失败了，那么可能就会浪费30s。
这里我也简单的做了一个假设方案，我们可以采取识别，使用颜色分割，上面提到邮件不显示完全，
就无法正常识别，往往我们可以在图像中找到对应的黄色区域（赛题中做了要求，背景为蓝色，邮件为黄色）
然后我们可以根据黄色区域在我们画面的位置，来控制机械臂往对应方向移动。
但是我们最终没有去实现，一个是比较麻烦，得去做图像这一块的处理，加上机械臂的运动范围有限，可能目的范围会超出机械臂的范围，
而且这个范围是不好确定的，具体的可以参考越疆机械臂具体资料。
其次是再进行图像处理可能又会增加硬件的压力，耗时会大大加大，毕竟是部署在单片机上，性能远不比电脑（文字识别用的最小模型，帧率上都比较低，具体后面说）
所以综上所述，还是采取左右固定移动的方式，希望建图和导航不会出现较大的误差！

正式上代码（我不列出全部代码，以防影响大家，有疑问可交流，主要介绍一下各个功能函数）

'''
-------------------------------------------------------------------------------------------------
功能：导航停止
参数：无
讲解：
导航结束停止或者出现异常停止
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def shutdown():
    rospy.loginfo("Stopping the robot...")
    # Cancel any active goals
    move_base.cancel_goal()
    rospy.sleep(2)
    # Stop the robot
    cmd_vel_pub.publish(Twist())
    rospy.sleep(1)
'''
-------------------------------------------------------------------------------------------------
功能：读取导航点
参数：txt文件名
讲解：
将10个导航点存储在txt中，第一个是起始点，剩余十个对应邮递箱的点，其顺序和
代码数组中的10个省份对应，这样方便修改和写入，不改动代码
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
def read_goals(filename):
    # 定义导航点信息
    global nav_pointcnt
    quaternions = []		#坐标点
    Position = []			#位姿点
    data = []
    # 读入txt数据
    with open(filename) as f:
        for line in f.readlines():
            temp = line.split()
            data.append(temp)
    # 把数据解析存入目标点列表
    for n in data:
        x, y, z = float(n[0]), float(n[1]), float(n[2])
        x1, y1, z1, w = float(n[3]), float(n[4]), float(n[5]), float(n[6])
        data1 = Point(x, y, z)
        data2 = Quaternion(x1, y1, z1, w)
        Position.append(data1)
        quaternions.append(data2)
    nav_pointcnt = len(Position)
    for i in range(0, nav_pointcnt):
        waypoints.append(Pose(Position[i], quaternions[i])) #pose功能函数自带，使用即可
    print"read goals finish!!"
    return nav_pointcnt



'''
-------------------------------------------------------------------------------------------------
功能：导航移动
参数：目标点
讲解：
使用move_base.send_goal(goal)函数，实现导航功能，这里的自动导航已经用此函数进行封装了，所以我们直接使用就行
至于实现方法有兴趣可以找到源文件学习，这里不做讲解
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def move(goal):
    # 发送移动目标点
    move_base.send_goal(goal)
    #等待超时，单位 秒
    finished_within_time = move_base.wait_for_result(rospy.Duration(70))
    # 如果超时取消本目标点的导航，进入下一步
    if not finished_within_time:
        move_base.cancel_goal()
        rospy.loginfo("Timed out achieving goal")
    else:
        # 否则导航完成，获取状态
        state = move_base.get_state()
        # 如果返回GoalStatus.SUCCEEDED，则导航成功
        if state == GoalStatus.SUCCEEDED:
            state = True
            rospy.loginfo("Goal succeeded!")
            return state


'''
-------------------------------------------------------------------------------------------------
功能：机械臂状态回调函数
参数：状态值
讲解：
本函数主要接收抓取结束时和投递结束时的状态，以此来判断是移动向投递箱还是分拣台
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def dobot_statecallback(data):
    global dobot_grap_state, dobot_deliver_state, move_state, box_index
    try:
        dobot_grap_state = data.dobot_grap_flag
        dobot_deliver_state = data.dobot_deliver_flag
        box_index = data.deliver_box_index
        move_state = True
        #print("dobot_grap_state: ", dobot_grap_state,"deliver_state: ", dobot_deliver_state)
        print("box_index: ", box_index, "move_state:", move_state)
        return dobot_deliver_state, dobot_deliver_state, move_state, box_index
    except:
        print("receive dobot_state error")

'''
-------------------------------------------------------------------------------------------------
功能：导航主函数
参数：无
讲解：
只展示初始化和话题的接收和创建，只有后面的导航逻辑这里省略，主要是更加上面的回调函数判断过程，然后执行相应的操作
作者：瓜洲
-------------------------------------------------------------------------------------------------
'''
def main():
    global dobot_grap_state, dobot_deliver_state ,move_state, box_index, 
        nav_grap_state, nav_deliver_state, nav_pointcnt
    pointcnt = 0
    para_move = 0.13			#邮件移动的常数值
    rospy.init_node('send_goal', anonymous=False)
    rospy.on_shutdown(shutdown)
    rospy.loginfo("Waiting for move_base action server...")
    pub_nav_state = rospy.Publisher('/navigational_state', navigational_state, queue_size=10)  # 发布数据
    rospy.Subscriber('/dobot_state', dobot_state, dobot_statecallback)
    # 等待服务器
    move_base.wait_for_server(rospy.Duration(20))
    rospy.loginfo("Connected to move base server")
    nav_pointcnt = read_goals("/home/eaibot/moveit_ws/goals.txt")

    rate = rospy.Rate(10)

以下代码省略，主要就是调用上面的函数，判断小车的移动逻辑。

四、总结

这一篇的核心在导航这一块，但是建图却是关键，图建不好就会严重影响导航的准确率，也就对全过程产生影响，所以一点要注意建图的方式方法。其次是检查小车的硬件，因为我遇到了小伙伴，他们建图的时候始终有缺陷，地图建出来严重变形，有比较大的发散，应该是激光雷达上翘了，最终连续厂家换了一台就好了。
其次就是虽然涉及到建图算法和导航算法，但是根据上面的内容，我们并没有去深究它，因为已经做了封装，直接拿来用就可以。当然要改进其性能，那肯定要熟读学习；
最后是可以参考我上面提到的一些问题，自己做一些改进，毕竟本人所学尚浅，还有很大的进步学习空间。下一章分享文字识别相关知识。