连载文章，长期更新，欢迎关注：
写在前面
第1章-ROS入门必备知识
第2章-C++编程范式
第3章-OpenCV图像处理
第4章-机器人传感器
第5章-机器人主机
第6章-机器人底盘
第7章-SLAM中的数学基础
第8章-激光SLAM系统
第9章-视觉SLAM系统
第10章-其他SLAM系统
第11章-自主导航中的数学基础
第12章-典型自主导航系统
第13章-机器人SLAM导航综合实战
13.1 运行机器人上的传感器
13.2 运行SLAM构建地图
13.3 运行自主导航
13.4 基于自主导航的应用
其实ros-navigation导航框架只是为我们提供了一个最基本的机器人自动导航接口，即从A点到B点的单点导航。然而在实际的应用中，机器人往往要完成复杂的任务，这些复杂的任务都是由一个个基本的任务组合而成的，这些基本任务一般以状态机的形式组合在一起。如图13-7为一个有限状态机（Finite State Machine，FSM）的例子，任何一个FSM都可以用状态转换图来描述，状态转换图中的节点表示FSM中的一个状态，有向加权边表示在输入条件时状态的转换关系。
图13-7 有限状态机
下面基于ros-navigation所提供的单点导航接口来实现一个简单的应用，即多目标点巡逻。这里采用Python来编写多目标点巡航的逻辑，用Python开发ROS节点的优点是简洁高效。写好的代码patrol_fsm.py见代码清单13-1所示，其中waypoints数组里面存放的是要巡航的各个目标点，大家可以根据自己的需要进行相应的替换和增减；with patrol代码块里面实现状态机的构建；最后调用状态机的执行函数，状态机就开始工作了，也就是开始执行巡航了。
代码清单13-1 多目标点巡逻程序patrol_fsm.py
1 #!/usr/bin/env python
2
3 import rospy
4 from smach import StateMachine
5 from smach_ros import SimpleActionState
6 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
7
8 waypoints =[
9 ['one', (-0.2, -2.1), (0.0, 0.0, 0.0, 1.0)],
10 ['two', (0.4, -1.3), (0.0, 0.0, -0.984047240305, 0.177907360295)],
11 ['three', (0.0, 0.0), (0.0, 0.0, 0.0, 1.0)]
12 ]
13
14 if __name__ == '__main__':
15 rospy.init_node('patrol')
16 patrol = StateMachine(['succeeded','aborted','preempted'])
17 with patrol:
18 for i,w in enumerate(waypoints):
19 goal_pose = MoveBaseGoal()
20 goal_pose.target_pose.header.frame_id = 'map'
21 goal_pose.target_pose.pose.position.x = w[1][0]
22 goal_pose.target_pose.pose.position.y = w[1][1]
23 goal_pose.target_pose.pose.position.z = 0.0
24 goal_pose.target_pose.pose.orientation.x = w[2][0]
25 goal_pose.target_pose.pose.orientation.y = w[2][1]
26 goal_pose.target_pose.pose.orientation.z = w[2][2]
27 goal_pose.target_pose.pose.orientation.w = w[2][3]
28
29 StateMachine.add(
30 w[0],
31 SimpleActionState('move_base', MoveBaseAction, goal=goal_pose),
32 transitions={'succeeded':waypoints[(i + 1) % len(waypoints)][0]}
33 )
34
35 patrol.execute()这里新建一个功能包patrol，将patrol_fsm.py存放到patrol/src/路径，然后就可以启动该节点程序进行多目标点巡逻了。
#启动多目标点巡逻
rosrun patrol patrol_fsm.py最后总结一下，机器人可以进行自动导航、人机对话、用机械臂抓取物体、物体识别等。将这些任务结合起来，利用基于深度学习或强化学习的推理机制能完成更为复杂和智能化的任务。如果说基于状态机的复杂任务调度是1.0版本的智能，那么基于深度学习或强化学习推理机制的复杂任务调度将是2.0版本的智能。
Github下载：github.com/xiihoo/Books_Robot_SLAM_Navigation
Gitee下载（国内访问速度快）：gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation
【1】 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.