ROS探索总结（十三）（十四）（十五）——导航与定位框架 move_base（路径规划） amcl（导航与定位）

ROS探索总结（十三）——导航与定位框架

导航与定位是机器人研究中的重要部分。
        一般机器人在陌生的环境下需要使用激光传感器（或者深度传感器转换成激光数据），先进行地图建模，然后在根据建立的地图进行导航、定位。在ROS中也有很多完善的包可以直接使用。
        在ROS中，进行导航需要使用到的三个包是：
      （1） move_base：根据参照的消息进行路径规划，使移动机器人到达指定的位置；
      （2） gmapping：根据激光数据（或者深度数据模拟的激光数据）建立地图；
      （3） amcl：根据已经有的地图进行定位。

       参考链接：http://www.ros.org/wiki/navigation/Tutorials/RobotSetup

                         http://www.ros.org/wiki/navigation/Tutorials

       整体导航包的格局如下图所示：

        其中白色框内的是ROS已经为我们准备好的必须使用的组件，灰色框内的是ROS中可选的组件，蓝色的是用户需要提供的机器人平台上的组件。

1、sensor transforms

        其中涉及到使用tf进行传感器坐标的转换，因为我们使用的机器人的控制中心不一定是在传感器上，所以要把传感器的数据转换成在控制中心上的坐标信息。如下图所示，传感器获取的数据是在base_laser的坐标系统中的，但是我们控制的时候是以base_link为中心，所以要根据两者的位置关心进行坐标的变换。

        变换的过程不需要我们自己处理，只需要将base_laser和base_link两者之间的位置关系告诉tf，就可以自动转换了。具体的实现可以参见：http://blog.csdn.net/hcx25909/article/details/9255001。

2、sensor sources

        这里是机器人导航传感器数据输入，一般只有两种：
      （1） 激光传感器数据
      （2） 点云数据
      具体实现见：http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Sensors

3、odometry source

        机器人的导航需要输入里程计的数据（tf，nav_msgs/Odometry_message），具体实现见：
        http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom

4、base controller

        在导航过程中，该部分负责将之前得出来的数据转封装成具体的线速度和转向角度信息（Twist），并且发布给硬件平台。

5、map_server

       在导航过程中，地图并不是必须的，此时相当于是在一个无限大的空地上进行导航，并没有任何障碍物。但是考虑到实际情况，在我们使用导航的过程中还是要一个地图的。
       具体实现见：http://www.ros.org/wiki/slam_gmapping/Tutorials/MappingFromLoggedData
       在ROS的导航中，costmap_2d这个包主要负责根据传感器的信息建立和更新二维或三维的地图。ROS的地图（costmap）采用网格（grid）的形式，每个网格的值从0~255，分为三种状态：占用（有障碍物）、无用（空闲的）、未知。具体状态和值的对应关系如下：

        

        上图共分为五个部分：（下面的红色框图是机器人的轮廓，旁边的黑框是上图的映射位置）
      （1） Lethal（致命的）:机器人的中心与该网格的中心重合，此时机器人必然与障碍物冲突。
      （2） Inscribed（内切）：网格的外切圆与机器人的轮廓内切，此时机器人也必然与障碍物冲突。
      （3） Possibly circumscribed（外切）：网格的外切圆与机器人的轮廓外切，此时机器人相当于靠在障碍物附近，所以不一定冲突。
      （4） Freespace（自由空间）：没有障碍物的空间。
      （5） Unknown（未知）：未知的空间。
       具体可见：http://www.ros.org/wiki/costmap_2d

----------------------------------------------------------------

ROS探索总结（十四）——move_base（路径规划）

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

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

一、数据结构

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

[plain]  view plain  copy

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

[plain]  view plain  copy

1. roslaunch rbx1_bringup fake_turtlebot.launch  

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

[plain]  view plain  copy

1. roslaunch rbx1_nav fake_move_base_blank_map.launch  

        该文件的具体内容如下：

[plain]  view plain  copy

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

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

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

[plain]  view plain  copy

1. rosrun rviz rviz -d `rospack find rbx1_nav`/nav_fuerte.vcg  

        

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

        让机器人前进一米：

[plain]  view plain  copy

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

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

[plain]  view plain  copy

1. rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \  
2. '{ 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按键，把所有的箭头清除。接着运行走正方形路径的代码：

[plain]  view plain  copy

1. rosrun rbx1_nav move_base_square.py  

        在rviz中可以看到：

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

[python]  view plain  copy

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

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

[plain]  view plain  copy

1. roslaunch rbx1_nav fake_move_base_obstacle.launch  

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

[plain]  view plain  copy

1. rosrun rbx1_nav move_base_square.py   

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

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

----------------------------------------------------------------

ROS探索总结（十五）——amcl（导航与定位）

在理解了move_base的基础上，我们开始机器人的定位与导航。gmaping包是用来生成地图的，需要使用实际的机器人获取激光或者深度数据，所以我们先在已有的地图上进行导航与定位的仿真。

        amcl是移动机器人二维环境下的概率定位系统。它实现了自适应（或kld采样）的蒙特卡罗定位方法，其中针对已有的地图使用粒子滤波器跟踪一个机器人的姿态。

一、测试

        首先运行机器人节点：

[plain]  view plain  copy

1. roslaunch rbx1_bringup fake_turtlebot.launch  

        然后运行amcl节点，使用测试地图：        

[plain]  view plain  copy

1. roslaunch rbx1_nav fake_amcl.launch map:=test_map.yaml  

        可以看一下fake_amcl.launch这个文件的内容：

[plain]  view plain  copy

1. <launch>  
2.   <!-- Set the name of the map yaml file: can be overridden on the command line. -->  
3.   <arg name="map" default="test_map.yaml" />  
4.   <!-- Run the map server with the desired map -->  
5.   <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/$(arg map)"/>  
6.   <!-- The move_base node -->  
7.   <include file="$(find rbx1_nav)/launch/fake_move_base.launch" />  
8.     
9.   <!-- Run fake localization compatible with AMCL output -->  
10.   <node pkg="fake_localization" type="fake_localization"  name="fake_localization" output="screen" />  
11.   <!-- For fake localization we need static transforms between /odom and /map and /map and /world -->  
12.   <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster"   
13. args="0 0 0 0 0 0 /odom /map 100" />  
14. </launch>  

         这个lanuch文件作用是加载地图，并且调用fake_move_base.launch文件打开move_base节点并加载配置文件，最后运行amcl。
         然后运行rviz：

[plain]  view plain  copy

1. rosrun rviz rviz -d `rospack find rbx1_nav`/nav_fuerte.vcg  

         这时在rvoiz中就应该显示出了地图和机器人：

        现在就可以通过rviz在地图上选择目标位置了，然后就会看到机器人自动规划出一条全局路径，并且导航前进：

二、自主导航 

        在实际应用中，我们往往希望机器人能够自主进行定位和导航，不需要认为的干预，这样才更智能化。在这一节的测试中，我们让目标点在地图中随机生成，然后机器人自动导航到达目标。
        这里运行的主要文件是：fake_nav_test.launch，让我们来看一下这个文件的内容：   

[plain]  view plain  copy

1. <launch>  
2.   
3.   <param name="use_sim_time" value="false" />  
4.   
5.   <!-- Start the ArbotiX controller -->  
6.   <include file="$(find rbx1_bringup)/launch/fake_turtlebot.launch" />  
7.   
8.   <!-- Run the map server with the desired map -->  
9.   <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/test_map.yaml"/>  
10.   
11.   <!-- The move_base node -->  
12.   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">  
13.     <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />  
14.     <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />  
15.     <rosparam file="$(find rbx1_nav)/config/fake/local_costmap_params.yaml" command="load" />  
16.     <rosparam file="$(find rbx1_nav)/config/fake/global_costmap_params.yaml" command="load" />  
17.     <rosparam file="$(find rbx1_nav)/config/fake/base_local_planner_params.yaml" command="load" />  
18.     <rosparam file="$(find rbx1_nav)/config/nav_test_params.yaml" command="load" />  
19.   </node>  
20.     
21.   <!-- Run fake localization compatible with AMCL output -->  
22.   <node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen" />  
23.     
24.   <!-- For fake localization we need static transform between /odom and /map -->  
25.   <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />  
26.     
27.   <!-- Start the navigation test -->  
28.   <node pkg="rbx1_nav" type="nav_test.py" name="nav_test" output="screen">  
29.     <param name="rest_time" value="1" />  
30.     <param name="fake_test" value="true" />  
31.   </node>  
32.     
33. </launch>  

        这个lanuch的功能比较多：
      （1） 加载机器人驱动
      （2） 加载地图
      （3） 启动move_base节点，并且加载配置文件
      （4） 运行amcl节点
      （5） 然后加载nav_test.py执行文件，进行随机导航
        相当于是把我们之前实验中的多个lanuch文件合成了一个文件。
        现在开始进行测试，先运行ROS：

[plain]  view plain  copy

1. roscore  

        然后我们运行一个监控的窗口，可以实时看到机器人发送的数据：

[plain]  view plain  copy

1. rxconsole  

        接着运行lanuch文件，并且在一个新的终端中打开rviz：

[plain]  view plain  copy

1. roslaunch rbx1_nav fake_nav_test.launch  
2. （打开新终端）  
3. rosrun rviz rviz -d `rospack find rbx1_nav`/nav_test_fuerte.vcg  

        好了，此时就看到了机器人已经放在地图当中了。然后我们点击rviz上的“2D Pose Estimate”按键，然后左键在机器人上单击，让绿色的箭头和黄色的箭头重合，机器人就开始随机选择目标导航了：

        

         在监控窗口中，我们可以看到机器人发送的状态信息：

        其中包括距离信息、状态信息、目标的编号、成功率和速度等信息。

三、导航代码分析

[plain]  view plain  copy

1. #!/usr/bin/env python  
2.   
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, PoseWithCovarianceStamped, Point, Quaternion, Twist  
8. from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
9. from random import sample  
10. from math import pow, sqrt  
11.   
12. class NavTest():  
13.     def __init__(self):  
14.         rospy.init_node('nav_test', anonymous=True)  
15.           
16.         rospy.on_shutdown(self.shutdown)  
17.           
18.         # How long in seconds should the robot pause at each location?  
19.         # 在每个目标位置暂停的时间  
20.         self.rest_time = rospy.get_param("~rest_time", 10)  
21.           
22.         # Are we running in the fake simulator?  
23.         # 是否仿真？  
24.         self.fake_test = rospy.get_param("~fake_test", False)  
25.           
26.         # Goal state return values  
27.         # 到达目标的状态  
28.         goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',   
29.                        'SUCCEEDED', 'ABORTED', 'REJECTED',  
30.                        'PREEMPTING', 'RECALLING', 'RECALLED',  
31.                        'LOST']  
32.           
33.         # Set up the goal locations. Poses are defined in the map frame.    
34.         # An easy way to find the pose coordinates is to point-and-click  
35.         # Nav Goals in RViz when running in the simulator.  
36.         # Pose coordinates are then displayed in the terminal  
37.         # that was used to launch RViz.  
38.         # 设置目标点的位置  
39.         # 如果想要获得某一点的坐标，在rviz中点击 2D Nav Goal 按键，然后单机地图中一点  
40.         # 在终端中就会看到坐标信息  
41.         locations = dict()  
42.           
43.         locations['hall_foyer'] = Pose(Point(0.643, 4.720, 0.000), Quaternion(0.000, 0.000, 0.223, 0.975))  
44.         locations['hall_kitchen'] = Pose(Point(-1.994, 4.382, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))  
45.         locations['hall_bedroom'] = Pose(Point(-3.719, 4.401, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680))  
46.         locations['living_room_1'] = Pose(Point(0.720, 2.229, 0.000), Quaternion(0.000, 0.000, 0.786, 0.618))  
47.         locations['living_room_2'] = Pose(Point(1.471, 1.007, 0.000), Quaternion(0.000, 0.000, 0.480, 0.877))  
48.         locations['dining_room_1'] = Pose(Point(-0.861, -0.019, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451))  
49.           
50.         # Publisher to manually control the robot (e.g. to stop it)  
51.         # 发布控制机器人的消息  
52.         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)  
53.           
54.         # Subscribe to the move_base action server  
55.         # 订阅move_base服务器的消息  
56.         self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  
57.           
58.         rospy.loginfo("Waiting for move_base action server...")  
59.           
60.         # Wait 60 seconds for the action server to become available  
61.         # 60s等待时间限制  
62.         self.move_base.wait_for_server(rospy.Duration(60))  
63.           
64.         rospy.loginfo("Connected to move base server")  
65.           
66.         # A variable to hold the initial pose of the robot to be set by   
67.         # the user in RViz  
68.         # 保存机器人的在rviz中的初始位置  
69.         initial_pose = PoseWithCovarianceStamped()  
70.           
71.         # Variables to keep track of success rate, running time,  
72.         # and distance traveled  
73.         # 保存成功率、运行时间、和距离的变量  
74.         n_locations = len(locations)  
75.         n_goals = 0  
76.         n_successes = 0  
77.         i = n_locations  
78.         distance_traveled = 0  
79.         start_time = rospy.Time.now()  
80.         running_time = 0  
81.         location = ""  
82.         last_location = ""  
83.           
84.         # Get the initial pose from the user  
85.         # 获取初始位置(仿真中可以不需要)  
86.         rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")  
87.         rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)  
88.         self.last_location = Pose()  
89.         rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)  
90.           
91.         # Make sure we have the initial pose  
92.         # 确保有初始位置  
93.         while initial_pose.header.stamp == "":  
94.             rospy.sleep(1)  
95.               
96.         rospy.loginfo("Starting navigation test")  
97.           
98.         # Begin the main loop and run through a sequence of locations  
99.         # 开始主循环，随机导航  
100.         while not rospy.is_shutdown():  
101.             # If we've gone through the current sequence,  
102.             # start with a new random sequence  
103.             # 如果已经走完了所有点，再重新开始排序  
104.             if i == n_locations:  
105.                 i = 0  
106.                 sequence = sample(locations, n_locations)  
107.                 # Skip over first location if it is the same as  
108.                 # the last location  
109.                 # 如果最后一个点和第一个点相同，则跳过  
110.                 if sequence[0] == last_location:  
111.                     i = 1  
112.               
113.             # Get the next location in the current sequence  
114.             # 在当前的排序中获取下一个目标点  
115.             location = sequence[i]  
116.                           
117.             # Keep track of the distance traveled.  
118.             # Use updated initial pose if available.  
119.             # 跟踪形式距离  
120.             # 使用更新的初始位置  
121.             if initial_pose.header.stamp == "":  
122.                 distance = sqrt(pow(locations[location].position.x -   
123.                                     locations[last_location].position.x, 2) +  
124.                                 pow(locations[location].position.y -   
125.                                     locations[last_location].position.y, 2))  
126.             else:  
127.                 rospy.loginfo("Updating current pose.")  
128.                 distance = sqrt(pow(locations[location].position.x -   
129.                                     initial_pose.pose.pose.position.x, 2) +  
130.                                 pow(locations[location].position.y -   
131.                                     initial_pose.pose.pose.position.y, 2))  
132.                 initial_pose.header.stamp = ""  
133.               
134.             # Store the last location for distance calculations  
135.             # 存储上一次的位置，计算距离  
136.             last_location = location  
137.               
138.             # Increment the counters  
139.             # 计数器加1  
140.             i += 1  
141.             n_goals += 1  
142.           
143.             # Set up the next goal location  
144.             # 设定下一个目标点  
145.             self.goal = MoveBaseGoal()  
146.             self.goal.target_pose.pose = locations[location]  
147.             self.goal.target_pose.header.frame_id = 'map'  
148.             self.goal.target_pose.header.stamp = rospy.Time.now()  
149.               
150.             # Let the user know where the robot is going next  
151.             # 让用户知道下一个位置  
152.             rospy.loginfo("Going to: " + str(location))  
153.               
154.             # Start the robot toward the next location  
155.             # 向下一个位置进发  
156.             self.move_base.send_goal(self.goal)  
157.               
158.             # Allow 5 minutes to get there  
159.             # 五分钟时间限制  
160.             finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))   
161.               
162.             # Check for success or failure  
163.             # 查看是否成功到达  
164.             if not finished_within_time:  
165.                 self.move_base.cancel_goal()  
166.                 rospy.loginfo("Timed out achieving goal")  
167.             else:  
168.                 state = self.move_base.get_state()  
169.                 if state == GoalStatus.SUCCEEDED:  
170.                     rospy.loginfo("Goal succeeded!")  
171.                     n_successes += 1  
172.                     distance_traveled += distance  
173.                     rospy.loginfo("State:" + str(state))  
174.                 else:  
175.                   rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))  
176.               
177.             # How long have we been running?  
178.             # 运行所用时间  
179.             running_time = rospy.Time.now() - start_time  
180.             running_time = running_time.secs / 60.0  
181.               
182.             # Print a summary success/failure, distance traveled and time elapsed  
183.             # 输出本次导航的所有信息  
184.             rospy.loginfo("Success so far: " + str(n_successes) + "/" +   
185.                           str(n_goals) + " = " +   
186.                           str(100 * n_successes/n_goals) + "%")  
187.             rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +   
188.                           " min Distance: " + str(trunc(distance_traveled, 1)) + " m")  
189.             rospy.sleep(self.rest_time)  
190.               
191.     def update_initial_pose(self, initial_pose):  
192.         self.initial_pose = initial_pose  
193.   
194.     def shutdown(self):  
195.         rospy.loginfo("Stopping the robot...")  
196.         self.move_base.cancel_goal()  
197.         rospy.sleep(2)  
198.         self.cmd_vel_pub.publish(Twist())  
199.         rospy.sleep(1)  
200.         
201. def trunc(f, n):  
202.     # Truncates/pads a float f to n decimal places without rounding  
203.     slen = len('%.*f' % (n, f))  
204.     return float(str(f)[:slen])  
205.   
206. if __name__ == '__main__':  
207.     try:  
208.         NavTest()  
209.         rospy.spin()  
210.     except rospy.ROSInterruptException:  
211.         rospy.loginfo("AMCL navigation test finished.")  

----------------------------------------------------------------

欢迎大家转载我的文章。

转载请注明：转自古-月

http://blog.csdn.net/hcx25909

欢迎继续关注我的博客