连载文章，长期更新，欢迎关注：
扫描二维码关注公众号，回复： 14638343 查看本文章
其实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版本的智能。
源码仓库
-
Gitee下载（国内访问速度快）：gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation