第十章 NavigationNavigation是机器人最基本的功能之一，ROS为我们提供了一整套Navigation的解决方案，包括全局与局部的路径规划、代价地图、异常行为恢复、地图服务器等等，这些开源工具包极大地减少了我们开发的工作量，任何一套移动机器人硬件平台经过这套方案就可以快速部署实现。10.1 Navigation Stack10.1.1 Navigation StackNavigation Stack是一个ROS的metapackage，里面包含了ROS在路径规划、定位、地图、异常行为恢复等方面的package，其中运行的算法都堪称经典。Navigation Stack的主要作用就是路径规划，通常是输入各传感器的数据，输出速度。一般我们的ROS都预装了Navigation。Navigation Stack的源代码位于https://github.com/ros-plannin
Navigation是机器人最基本的功能之一，ROS为我们提供了一整套Navigation的解决方案，包括全局与局部的路径规划、代价地图、异常行为恢复、地图服务器等等，这些开源工具包极大地减少了我们开发的工作量，任何一套移动机器人硬件平台经过这套方案就可以快速部署实现。
10.1 Navigation Stack
10.1.1 Navigation Stack
Navigation Stack是一个ROS的metapackage，里面包含了ROS在路径规划、定位、地图、异常行为恢复等方面的package，其中运行的算法都堪称经典。Navigation Stack的主要作用就是路径规划，通常是输入各传感器的数据，输出速度。一般我们的ROS都预装了Navigation。
Navigation Stack的源代码位于https://github.com/ros-planning/navigation，包括了以下几个package：
包名 功能 amcl 定位 fake_localization 定位 map_server 提供地图 move_base 路径规划节点 nav_core 路径规划的接口类，包括base_local_planner、base_global_planner和recovery_behavior三个接口 base_local_planner 实现了Trajectory Rollout和DWA两种局部规划算法 dwa_local_planner 重新实现了DWA局部规划算法 parrot_planner 实现了较简单的全局规划算法 navfn 实现了Dijkstra和A*全局规划算法 global_planner 重新实现了Dijkstra和A*全局规划算法 clear_costmap_recovery 实现了清除代价地图的恢复行为 rotate_recovery 实现了旋转的恢复行为 move_slow_and_clear 实现了缓慢移动的恢复行为 costmap_2d 二维代价地图 voxel_grid 三维小方块（体素？） robot_pose_ekf 机器人位姿的卡尔曼滤波
这么多package，你可能会觉得很乱，不过担心，在使用中其实还是比较简单的，我们接下来会对常用的主要功能进行介绍。
10.1.2 Navigation工作框架
机器人的自主导航功能基本全靠Navigation中的pacakge，来看这张图：
上图中位于导航功能正中心的是move_base节点，可以理解为一个强大的路径规划器，在实际的导航任务中，你只需要启动这一个node，并且给他提供数据，就可以规划出路径和速度。 move_base之所以能做到路径规划，是因为它包含了很多的插件，像图中的白色圆圈global_planner、local_planner、global_costmap、local_costmap、recovery_behaviors。这些插件用于负责一些更细微的任务：全局规划、局部规划、全局地图、局部地图、恢复行为。而每一个插件其实也都是一个package，放在Navigation Stack里。 关于move_base我们后面会进一步介绍，先来看看move_base外围有哪些输入输出。
输入
/tf:提要提供的tf包括map_frame、odom_frame、base_frame以及机器人各关节之间的完成的一棵tf树。/odom:里程计信息/scan或/pointcloud:传感器的输入信息，最常用的是激光雷达(sensor_msgs/LaserScan类型)，也有用点云数据(sensor_msgs/PointCloud)的。/map：地图，可以由SLAM程序来提供，也可以由map_server来指定已知地图。
以上四个Topic是必须持续提供给导航系统的，下面一个是可随时发布的topic：
move_base_simple/goal:目标点位置。
有几点需要注意：
1.move_base并不会去发布tf，因为对于路径规划问题来说，假设地图和位置都是已知的，定位和建图是其他节点的事情。 2.sensor_topics一般输入是激光雷达数据，但也有输入点云的情况。 3.图中map_server是灰色，代表可选，并不表示/map这个topic是可选，必须提供地图给move_base。
输出
/cmd_vel:geometry_msgs/Twist类型，为每一时刻规划的速度信息。
10.2 move_base
10.2.1 move_base与插件
move_base算得上是Navigation中的核心节点，之所以称之为核心，是因为它在导航的任务中处于支配地位，其他的一些package都是它的插件。 来看这张图 
move_base要运行起来，需要选择好插件，包括三种插件：base_local_planner、base_global_planner和recovery_behavior，这三种插件都得指定，否则系统会指定默认值。
Navigation为我们提供了不少候选的插件，可以在配置move_base时选择。
base_local_planner插件：
base_local_planner: 实现了Trajectory Rollout和DWA两种局部规划算法
dwa_local_planner: 实现了DWA局部规划算法，可以看作是base_local_planner的改进版本
base_global_planner插件：
parrot_planner: 实现了较简单的全局规划算法
navfn: 实现了Dijkstra和A*全局规划算法
global_planner: 重新实现了Dijkstra和A*全局规划算法,可以看作navfn的改进版
recovery_behavior插件：
clear_costmap_recovery: 实现了清除代价地图的恢复行为
rotate_recovery: 实现了旋转的恢复行为
move_slow_and_clear: 实现了缓慢移动的恢复行为
除了以上三个需要指定的插件外，还有一个costmap插件，该插件默认已经选择好，无法更改。
以上所有的插件都是继承于nav_core里的接口，nav_core属于一个接口package，它只定义了三种插件的规范，也可以说定义了三种接口类，然后分别由以上的插件来继承和实现这些接口。因此如果你要研究路径规划算法，不妨研究一下nav_core定义的路径规划工作流程，然后仿照dwa_local_planner或其他插件来实现。
除了以上三个需要指定的插件外，还有一个costmap插件，该插件默认已经选择好，默认即为costmap_2d，不可更改，但costmap_2d提供了不同的Layer可以供我们设置，在9.3节我们会进行介绍。
在这里插件的概念并不是我们抽象的描述，而是在ROS里catkin编译系统能够认出的，并且与其他节点能够耦合的C++库，插件是可以动态加载的类，也就是说插件不需要提前链接到ROS的程序上，只需在运行时加载插件就可以调用其中的功能。
具体关于插件的介绍，有兴趣请看http://wiki.ros.org/pluginlib，本书不做过多介绍。
10.2.2 插件选择(参数)
既然我们知道了move_base具体的一些插件，那如何来选择呢？其实非常简单。在move_base的参数设置里可以选择插件。 move_base的参数包括以下内容：
参数 默认值 功能 ~base_global_planner navfn/NavfnROS 设置全局规划器 ~base_local_planner base_local_planner/TrajectoryPlannerROS 设置局部规划器 ~recovery_behaviors [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}] 设置恢复行为 ... ... ...
除了这三个选择插件的参数，还有控制频率、误差等等参数。 具体请看http://wiki.ros.org/move_base介绍。 在ROS-Academy-for-Beginners的代码中的navigation_sim_demo例子中，由于要配置的参数太多，通常会将配置写在一个yaml文件中，我们用param/move_base_params.yaml来保存以上参数。而关于一些具体插件，比如dwa_local_planner则也会创建一个文件param/dwa_local_planner.yaml来保存它的设置。
10.2.3 Topic与Service
move_base输入输出的Topic在9.1节已经做了介绍，这里不再赘述。
move_base包含的Service包括：
make_plan: nav_msgs/GetPlan类型，请求为一个目标点，响应为规划的轨迹，但不执行该轨迹。
clear_unknown_space: std_srvs/Empty类型，允许用户清除未知区域地图。
clear_costmaps: std_srvs/Empty类型，允许用户清楚代价地图上的障碍物。
10.3 costmap
costmap是Navigation Stack里的代价地图，它其实也是move_base插件，本质上是C++的动态链接库，用过catkin_make之后生成.so文件，然后move_base在启动时会通过动态加载的方式调用其中的函数。
10.3.1 代价地图
之前我们在介绍SLAM时讲过ROS里的地图的概念，地图就是/map这个topic，它也是一张图片，一个像素代表了实际的一块面积，用灰度值来表示障碍物存在的可能性。然而在实际的导航任务中，光有一张地图是不够的，机器人需要能动态的把障碍物加入，或者清楚已经不存在的障碍物，有些时候还要在地图上标出危险区域，为路径规划提供更有用的信息。
因为导航的需要，所以出现了代价地图。你可以将代价地图理解为，在/map之上新加的另外几层地图，不仅包含了原始地图信息，还加入了其他辅助信息。
代价地图有一下特点： １．首先，代价地图有两张，一张是local_costmap，一张是global_costmap，分别用于局部路径规划器和全局路径规划器，而这两个costmap都默认并且只能选择costmap_2d作为插件。 ２. 无论是local_costmap还是global_costmap，都可以配置他们的Layer，可以选择多个层次。costmap的Layer包括以下几种：
Static Map Layer：静态地图层，通常都是SLAM建立完成的静态地图。
Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。
Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。
Other Layers：你还可以通过插件的形式自己实现costmap，目前已有
Social Costmap Layer、Range Sensor Layer等开源插件。
可以同时选择多个Layer并存。
10.3.2 地图插件的选择
与9.2节中move_base插件的配置类似，costmap配置也同样用yaml 来保存，其本质是维护在参数服务器上。由于costmap通常分为local和global的coastmap，我们习惯把两个代价地图分开。以ROS-Academy-for-Beginners为例，配置写在了param文件夹下的global_costmap_params.yaml和local_costmap_params.yaml里。 global_costmap_params.yaml:
global_costmap:
global_frame: /map
robot_base_frame: /base_footprint
update_frequency: 2.0
publish_frequency: 0.5
static_map: true
rolling_window: false
transform_tolerance: 0.5
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: voxel_layer, type: "costmap_2d::VoxelLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
local_costmap_params.yaml:
local_costmap:
global_frame: /map
robot_base_frame: /base_footprint
update_frequency: 5.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.05
origin_x: 5.0
origin_y: 0
transform_tolerance: 0.5
plugins:
- {name: voxel_layer, type: "costmap_2d::VoxelLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
在plugins一项中可以设置Layer的种类，可以多层叠加。在本例中，考虑到局部地图并不需要静态地图，而只考虑传感器感知到的障碍物，因此可以删去StaticLayer。
10.4 map_server & amcl
在某些固定场景下，我们已经知道了地图（无论通过SLAM还是测量），这样机器人每次启动最好就能直接加载已知地图，而是每次开机都重建。在这种情况下，就需要有一个节点来发布/map，提供场景信息了。
10.3.1 map_server
map_server是一个和地图相关的功能包，它可以将已知地图发布出来，供导航和其他功能使用，也可以保存SLAM建立的地图。
要让map_server发布/map，需要输入给它两个文件:
地图文件，通常为pgm格式；
地图的描述文件，通常为yaml格式
例如在ROS-Academy-for-Beginners里，我们提供了软件博物馆的地图文件，见slam_sim_demo/maps下：
Software_Museum.pgm
Software_Museum.yaml
image: Software_Museum.pgm #指定地图文件
resolution: 0.050000 #地图的分辨率 单位为m/pixel
origin: [-25.000000, -25.000000, 0.000000] #地图的原点
negate: 0 #0代表 白色为空闲 黑色为占据
occupied_thresh: 0.65 #当占据的概率大于0.65认为被占据
free_thresh: 0.196 #当占据的概率小于0.196认为无障碍
其中占据的概率 occ = (255-color_avg)/255.0， color_avg为RGB三个通道的平均值。
有了以上两个文件，你可以通过指令来加载这张地图,map_server相关命令如下:
map_server命令 作用 rosrun map_server map_server Software_Museum.yaml加载自定义的地图 rosrun map_server map_saver -f mymap保存当前地图为mymap.pgn和mymap.yaml
当我运行rosrun map_server map_server ***.yaml时，会有以下的通信接口：
Topic
通常我们是在launch文件中加载map_server，发布地图。而map_server发布的消息包括：
/map_metadata: 发布地图的描述信息
/map: 发布锁存的地图消息
Service
amcl的服务只有一个：
static_map: 用于请求和响应当前的静态地图。
Param
amcl有一个参数需要设置，就是发布地图的frame。
~frame_id: string类型，默认为map。 绑定发布的地图与tf中的哪个frame，通常就是map。
有两个概念不要搞混淆，map既是一个topic，也是一个frame，前者是topic通信方式中的一个话题，信息交互的频道，后者是tf中的一个坐标系，map_frame需要和其他的frame相连通。
10.3.2 amcl
Adaptive Mentcarto Localization(AMCL)，蒙特卡洛自适应定位是一种很常用的定位算法，它通过比较检测到的障碍物和已知地图来进行定位。
AMCL上的通信架构如上图所示，与之前SLAM的框架很像，最主要的区别是/map作为了输入，而不是输出，因为AMCL算法只负责定位，而不管建图。 
同时还有一点需要注意，AMCl定位会对里程计误差进行修正，修正的方法是把里程计误差加到map_frame和odom_frame之间，而odom_frame和base_frame之间是里程计的测量值，这个测量值并不会被修正。这一工程实现与之前gmapping、karto的做法是相同的。
演示截图
amcl算法演示效果图如下：
发表评论