在上一篇的博客中，我们一起学习了ROS定位于导航的总体框架，这一篇我们主要研究其中最重要的move_base包。

      在总体框架图中可以看到，move_base提供了ROS导航的配置、运行、交互接口，它主要包括两个部分：
      （1） 全局路径规划（global planner）：根据给定的目标位置进行总体路径的规划；
      （2） 本地实时规划（local planner）：根据附近的障碍物进行躲避路线规划。

一、数据结构

        ROS中定义了MoveBaseActionGoal数据结构来存储导航的目标位置数据，其中最重要的就是位置坐标（position）和方向（orientation）。

1. [plain] view plaincopy
   
2. rosmsg show MoveBaseActionGoal  
   
3.   
   
4. [move_base_msgs/MoveBaseActionGoal]:  
   
5. std_msgs/Header header  
   
6.   uint32 seq  
   
7.   time stamp  
   
8.   string frame_id  
   
9. actionlib_msgs/GoalID goal_id  
   
10.   time stamp  
    
11.   string id  
    
12. move_base_msgs/MoveBaseGoal goal  
    
13.   geometry_msgs/PoseStamped target_pose  
    
14.     std_msgs/Header header  
    
15.       uint32 seq  
    
16.       time stamp  
    
17.       string frame_id  
    
18.     geometry_msgs/Pose pose  
    
19.       geometry_msgs/Point position  
    
20.         float64 x  
    
21.         float64 y  
    
22.         float64 z  
    
23.       geometry_msgs/Quaternion orientation  
    
24.         float64 x  
    
25.         float64 y  
    
26.         float64 z  
    
27.         float64 w  

复制代码

二、配置文件

        move_base使用前需要配置一些参数：运行成本、机器人半径、到达目标位置的距离，机器人移动的速度，这些参数都在rbx1_nav包的以下几个配置文件中：
        • base_local_planner_params.yaml
        • costmap_common_params.yaml
        • global_costmap_params.yaml
        • local_costmap_params.yaml

三、全局路径规划（global planner）

        在ROS的导航中，首先会通过全局路径规划，计算出机器人到目标位置的全局路线。这一功能是navfn这个包实现的。
        navfn通过Dijkstra最优路径的算法，计算costmap上的最小花费路径，作为机器人的全局路线。将来在算法上应该还会加入A*算法。
        具体见：http://www.ros.org/wiki/navfn?distro=fuerte

四、本地实时规划（local planner）

        本地的实时规划是利用base_local_planner包实现的。该包使用Trajectory Rollout 和Dynamic Window approaches算法计算机器人每个周期内应该行驶的速度和角度（dx，dy，dtheta velocities）。

    base_local_planner这个包通过地图数据，通过算法搜索到达目标的多条路经，利用一些评价标准（是否会撞击障碍物，所需要的时间等等）选取最优的路径，并且计算所需要的实时速度和角度。
其中，Trajectory Rollout 和Dynamic Window approaches算法的主要思路如下：
      （1） 采样机器人当前的状态（dx,dy,dtheta）；
      （2） 针对每个采样的速度，计算机器人以该速度行驶一段时间后的状态，得出一条行驶的路线。
      （3） 利用一些评价标准为多条路线打分。
      （4） 根据打分，选择最优路径。
      （5） 重复上面过程。
      具体参见：http://www.ros.org/wiki/base_local_planner?distro=groovy

五、ArbotiX仿真——手动设定目标

        在这一步，我们暂时使用空白地图（blank_map.pgm），就在空地上进行无障碍仿真。
        首先运行ArbotiX节点，并且加载机器人的URDF文件。

1. [plain] view plaincopy
   
2. roslaunch rbx1_bringup fake_turtlebot.launch  

复制代码

然后运行move_base和加载空白地图的launch文件（fake_move_base_blank_map.launch）：

1. [plain] view plaincopy
   
2. roslaunch rbx1_nav fake_move_base_blank_map.launch  
   

复制代码

该文件的具体内容如下：

1. [plain] view plaincopy
   
2. <launch>  
   
3.   <!-- Run the map server with a blank map -->  
   
4.   <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>  
   
5.       
   
6.   <include file="$(find rbx1_nav)/launch/fake_move_base.launch" />  
   
7.   
   
8.   <!-- Run a static transform between /odom and /map -->  
   
9.   <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />  
   
10.   
    
11. </launch>  

复制代码

     其中调用了fake_move_base.launch文件，是运行move_base节点并进行参数配置。

        然后调用rviz就可以看到机器人了。

1. [plain] view plaincopy
   
2. rosrun rviz rviz -d `rospack find rbx1_nav`/nav_fuerte.vcg  

复制代码

我们先以1m的速度进行一下测试：

        让机器人前进一米：

1. [plain] view plaincopy
   
2. rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \  
   
3. '{ header: { frame_id: "base_link" }, pose: { position: { x: 1.0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'  

复制代码

让机器人后退一米，回到原来的位置：

1. [plain] view plaincopy
   
2. rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \  
   
3. '{ header: { frame_id: "map" }, pose: { position: { x: 0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'  

复制代码

在rviz中的轨迹图如下：

       在机器人移动过程中，有一条蓝色的线（被黄线挡住了）就是机器人的全局规划的路径；红色的箭头是实施规划的路线，会不断更新，有的时候会呈现很大的弧线，那是因为机器人在转向的过程中尽量希望保持平稳的角度。如果觉得路径规划的精度不够，可以修改配置文件中的pdist_scale参数进行修正。
       然后我们可以认为的确定目标位置，点击rviz上方的2D Nav Goal按键，然后左键选择目标位置，机器人就开始自动导航了。

六、ArbotiX仿真——带有障碍物的路径规划

        首先我们让机器人走一个正方形的路线。先通过上面的命令，让机器人回到原始位置（0，0，0），然后按reset按键，把所有的箭头清除。接着运行走正方形路径的代码：

1. [plain] view plaincopy
   
2. rosrun rbx1_nav move_base_square.py  

复制代码

在rviz中可以看到：

       四个顶角的粉色圆盘就是我们设定的位置，正方形比较规则，可见定位还是比较准确的。然我们先来分析一下走正方形路线的代码：

1. [python] view plaincopy
   
2. #!/usr/bin/env python  
   
3. import roslib; roslib.load_manifest('rbx1_nav')  
   
4. import rospy  
   
5. import actionlib  
   
6. from actionlib_msgs.msg import *  
   
7. from geometry_msgs.msg import Pose, Point, Quaternion, Twist  
   
8. from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
   
9. from tf.transformations import quaternion_from_euler  
   
10. from visualization_msgs.msg import Marker  
    
11. from math import radians, pi  
    
12.   
    
13. class MoveBaseSquare():  
    
14.     def __init__(self):  
    
15.         rospy.init_node('nav_test', anonymous=False)  
    
16.          
    
17.         rospy.on_shutdown(self.shutdown)  
    
18.          
    
19.         # How big is the square we want the robot to navigate?  
    
20.         # 设定正方形的尺寸，默认是一米  
    
21.         square_size = rospy.get_param("~square_size", 1.0) # meters  
    
22.          
    
23.         # Create a list to hold the target quaternions (orientations)  
    
24.         # 创建一个列表，保存目标的角度数据  
    
25.         quaternions = list()  
    
26.          
    
27.         # First define the corner orientations as Euler angles  
    
28.         # 定义四个顶角处机器人的方向角度（Euler angles:http://zh.wikipedia.org/wiki/%E6%AC%A7%E6%8B%89%E8%A7%92)  
    
29.         euler_angles = (pi/2, pi, 3*pi/2, 0)  
    
30.          
    
31.         # Then convert the angles to quaternions  
    
32.         # 将上面的Euler angles转换成Quaternion的格式  
    
33.         for angle in euler_angles:  
    
34.             q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')  
    
35.             q = Quaternion(*q_angle)  
    
36.             quaternions.append(q)  
    
37.          
    
38.         # Create a list to hold the waypoint poses  
    
39.         # 创建一个列表存储导航点的位置  
    
40.         waypoints = list()  
    
41.          
    
42.         # Append each of the four waypoints to the list.  Each waypoint  
    
43.         # is a pose consisting of a position and orientation in the map frame.  
    
44.         # 创建四个导航点的位置（角度和坐标位置）  
    
45.         waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0]))  
    
46.         waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1]))  
    
47.         waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2]))  
    
48.         waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))  
    
49.          
    
50.         # Initialize the visualization markers for RViz  
    
51.         # 初始化可视化标记  
    
52.         self.init_markers()  
    
53.          
    
54.         # Set a visualization marker at each waypoint   
    
55.         # 给每个定点的导航点一个可视化标记(就是rviz中看到的粉色圆盘标记)  
    
56.         for waypoint in waypoints:            
    
57.             p = Point()  
    
58.             p = waypoint.position  
    
59.             self.markers.points.append(p)  
    
60.               
    
61.         # Publisher to manually control the robot (e.g. to stop it)  
    
62.         # 发布TWist消息控制机器人  
    
63.         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)  
    
64.          
    
65.         # Subscribe to the move_base action server  
    
66.         # 订阅move_base服务器的消息  
    
67.         self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  
    
68.          
    
69.         rospy.loginfo("Waiting for move_base action server...")  
    
70.          
    
71.         # Wait 60 seconds for the action server to become available  
    
72.         # 等待move_base服务器建立  
    
73.         self.move_base.wait_for_server(rospy.Duration(60))  
    
74.          
    
75.         rospy.loginfo("Connected to move base server")  
    
76.         rospy.loginfo("Starting navigation test")  
    
77.          
    
78.         # Initialize a counter to track waypoints  
    
79.         # 初始化一个计数器，记录到达的顶点号  
    
80.         i = 0  
    
81.          
    
82.         # Cycle through the four waypoints  
    
83.         # 主循环,环绕通过四个定点  
    
84.         while i < 4 and not rospy.is_shutdown():  
    
85.             # Update the marker display  
    
86.             # 发布标记指示四个目标的位置，每个周期发布一起，确保标记可见  
    
87.             self.marker_pub.publish(self.markers)  
    
88.               
    
89.             # Intialize the waypoint goal  
    
90.             # 初始化goal为MoveBaseGoal类型  
    
91.             goal = MoveBaseGoal()  
    
92.               
    
93.             # Use the map frame to define goal poses  
    
94.             # 使用map的frame定义goal的frame id  
    
95.             goal.target_pose.header.frame_id = 'map'  
    
96.               
    
97.             # Set the time stamp to "now"  
    
98.             # 设置时间戳  
    
99.             goal.target_pose.header.stamp = rospy.Time.now()  
    
100.               
     
101.             # Set the goal pose to the i-th waypoint  
     
102.             # 设置目标位置是当前第几个导航点  
     
103.             goal.target_pose.pose = waypoints[i]  
     
104.               
     
105.             # Start the robot moving toward the goal  
     
106.             # 机器人移动  
     
107.             self.move(goal)  
     
108.               
     
109.             i += 1  
     
110.          
     
111.     def move(self, goal):  
     
112.             # Send the goal pose to the MoveBaseAction server  
     
113.             # 把目标位置发送给MoveBaseAction的服务器  
     
114.             self.move_base.send_goal(goal)  
     
115.               
     
116.             # Allow 1 minute to get there  
     
117.             # 设定1分钟的时间限制  
     
118.             finished_within_time = self.move_base.wait_for_result(rospy.Duration(60))   
     
119.               
     
120.             # If we don't get there in time, abort the goal  
     
121.             # 如果一分钟之内没有到达，放弃目标  
     
122.             if not finished_within_time:  
     
123.                 self.move_base.cancel_goal()  
     
124.                 rospy.loginfo("Timed out achieving goal")  
     
125.             else:  
     
126.                 # We made it!  
     
127.                 state = self.move_base.get_state()  
     
128.                 if state == GoalStatus.SUCCEEDED:  
     
129.                     rospy.loginfo("Goal succeeded!")  
     
130.                      
     
131.     def init_markers(self):  
     
132.         # Set up our waypoint markers  
     
133.         # 设置标记的尺寸  
     
134.         marker_scale = 0.2  
     
135.         marker_lifetime = 0 # 0 is forever  
     
136.         marker_ns = 'waypoints'  
     
137.         marker_id = 0  
     
138.         marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}  
     
139.          
     
140.         # Define a marker publisher.  
     
141.         # 定义一个标记的发布者  
     
142.         self.marker_pub = rospy.Publisher('waypoint_markers', Marker)  
     
143.          
     
144.         # Initialize the marker points list.  
     
145.         # 初始化标记点的列表  
     
146.         self.markers = Marker()  
     
147.         self.markers.ns = marker_ns  
     
148.         self.markers.id = marker_id  
     
149.         self.markers.type = Marker.SPHERE_LIST  
     
150.         self.markers.action = Marker.ADD  
     
151.         self.markers.lifetime = rospy.Duration(marker_lifetime)  
     
152.         self.markers.scale.x = marker_scale  
     
153.         self.markers.scale.y = marker_scale  
     
154.         self.markers.color.r = marker_color['r']  
     
155.         self.markers.color.g = marker_color['g']  
     
156.         self.markers.color.b = marker_color['b']  
     
157.         self.markers.color.a = marker_color['a']  
     
158.          
     
159.         self.markers.header.frame_id = 'map'  
     
160.         self.markers.header.stamp = rospy.Time.now()  
     
161.         self.markers.points = list()  
     
162.   
     
163.     def shutdown(self):  
     
164.         rospy.loginfo("Stopping the robot...")  
     
165.         # Cancel any active goals  
     
166.         self.move_base.cancel_goal()  
     
167.         rospy.sleep(2)  
     
168.         # Stop the robot  
     
169.         self.cmd_vel_pub.publish(Twist())  
     
170.         rospy.sleep(1)  
     
171.   
     
172. if __name__ == '__main__':  
     
173.     try:  
     
174.         MoveBaseSquare()  
     
175.     except rospy.ROSInterruptException:  
     
176.         rospy.loginfo("Navigation test finished.")  

复制代码

       但是，在实际情况中，往往需要让机器人自动躲避障碍物。move_base包的一个强大的功能就是可以在全局规划的过程中自动躲避障碍物，而不影响全局路径。障碍物可以是静态的（比如墙、桌子等），也可以是动态的（比如人走过）。
       现在我们尝试在之前的正方形路径中加入障碍物。把之前运行fake_move_base_blank_map.launch的中断Ctrl-C掉，然后运行：

1. [plain] view plaincopy
   
2. roslaunch rbx1_nav fake_move_base_obstacle.launch  

复制代码

  然后就会看到在rviz中出现了障碍物。然后在运行之前走正方形路线的代码：

1. [plain] view plaincopy
   
2. rosrun rbx1_nav move_base_square.py   

复制代码

   这回我们可以看到，在全局路径规划的时候，机器人已经将障碍物绕过去了，下过如下图：

       在上图中，黑色的线是障碍物，周围浅色的椭圆形是根据配置文件中的inflation_radius参数计算出来的安全缓冲区。全局规划的路径基本已经是最短路径了。在仿真的过程中也可以动态重配置那四个配置文件，修改仿真参数。