SLAM组合拳之四—如何在ROS中使用路径规划（一）
RViz可视化
显示Costmap代价地图
-
点击左下角的add，选择Map类型
-
topic选择/move_base/global_costmap/costmap，即为全局代价地图
- topic选择/move_base/local_costmap/costmap，即为局部代价地图
显示Plans规划结果
-
点击左下角的add，选择Path类型
-
topic选择/move_base/NavfnROS/plan，即为全局规划结果
-
topic选择/move_base/TrajectoryPlannerROS/local_plan，即为局部规划结果
2D工具
-
2D Pose Estimate可以提供初始位姿
-
2D Nav Goal可以提供导航目标点
这里两个工具在RViz的工具栏里，如果没有可以点击工具栏右侧的加号将其显示出来，并且此时Global Options中的Fixed Frame必须为map不然工具无法生效。
move_base节点
它的功能主要是把机器人从当前位置移动到目标位置。其本质上是一个SimpleActionServer的实现，即所谓的action（ROS三大通讯方式之一）的服务器，它使用geometry_msgs/PoseStamped格式的消息，因此我们可以使用SimpleActionClient向move_base发送目标点。
同时move_base提供了一个话题move_base/goal作为导航堆栈的输入，我们可以简单地将目标点发布至这个话题即可触发整个move_base的工作链。
查看topic列表会发现，move_base和其他action一样也提供了五个话题用于获取过程数据以及发送取消目标和获取结果。
-
move_base/goal (move_base_msgs/MoveBaseActionGoal)
-
move_base/cancel (actionlib_msgs/GoalID)
-
move_base/feedback (move_base_msgs/MoveBaseActionFeedback)
-
move_base/status (actionlib_msgs/GoalStatusArray)
-
move_base/result (move_base_msgs/MoveBaseActionResult)
使用RViz的2D Nav Goal工具发布一个目标位姿，其本质就是向move_base/goal主题发布了一条消息，下面我们监听move_base的这几个话题来看看在发布一个目标后发生了什么。
使用这种方式进行导航时，机器人会在规划生成后开始运动，如果你只是想查看规划结果又不想移动机器人，那么可以使用move_base提供的/make_plan这个服务接口，它可以只生成规划但不移动机器人。
现在我们知道，当move_base收到一个目标姿势, 它会链接到 global planner（全局规划器）, local planner（局部规划器）, recovery behaviors（恢复行为）以及costmaps（代价地图）等组件,并 生成一个消息类型为 geometry_msgs/Twist的速度命令输出 , 将其发送到 /cmd_vel 主题来移动机器人。
全局规划器
全局规划器就是在运动之前，根据接收到的目标位姿基于global_costmap提供的全局地图调用对应的路径规划算法生成一条从当前位置到目标位置的路径，这里的global_costmap就是根据之前加载的map.pgm数据而生成的，也就是说它在规划的过程中并未获取当前传感器的数据，这和局部规划器是有区别的，后面会讲到。
我们可以用第一节中使用的现实世界中导航的例子来说，这个全局规划器就相当于根据预先采集的地图数据，计算一条从当前位置到目标位置的可行（或最优）路径，但它不会考虑路况，也不会帮你避开拥堵路段，因为它的数据来源只有地图，并没有实时路况数据。
当前ROS中提供了三个全局规划器的插件：navfn，carrot_planner以及global_planner。其中navfn是最常见的也是默认的全局规划器，使用的是Dijkstra's算法来计算初始位置和目标位置之间的最短路径。carrot_planner的适用性不强，一般在某些特定的场景较为有效（比如让机器人移动到离障碍物尽可能近的场景）。global_planner可以说是navfn的升级版本，虽然navfn内置有Dijkstra's和A*的两种算法的实现，但是在早期版本中A*算法的实现有些bug未修复，故认为navfn使用的是Dijkstra's算法，而在之后版本的升级中为了兼容老版本，所以保留了navfn但也推出了global_planner，global_planner既提供了Dijkstra's和A*算法的实现，还支持自定义的全局规划器插件，可以说比navfn更为灵活。
这里选择global_planner做为全局规划器，对于global_planner的具体参数，这里创建一个global_planner_params.yaml文件，用于管理相关参数，完整参数介绍可参见：http://wiki.ros.org/global_planner
GlobalPlanner:
allow_unknown: false #默认true，是否允许路径穿过未知区域
default_tolerance: 0.2 #默认0.0，目标容差
visualize_potential: false #默认false，是否显示从PointCloud2计算得到的势区域
use_dijkstra: true #默认true，true表示使用dijkstra's否则使用A*
use_quadratic: true #默认true，true表示使用二次函数近似函数
use_grid_path: false #默认false，true表示使路径沿栅格边界生成，否则使用梯度下降算法
old_navfn_behavior: false #默认false，是否复制navfn规划器的结果
lethal_cost: 253 #默认253，致命代价值
neutral_cost: 50 #默认50，中等代价值
cost_factor: 3.0 #默认3.0，代价因子
publish_potential: true #默认true，是否发布costmap的势函数
orientation_mode: 0 #默认0，设置点的方向
orientation_window_size: 1 #默认1，根据orientation_mode指定的位置积分确定窗口方向
全局代价地图
在这节最后，我们来研究一下全局代价地图，前面我们有稍微提到但未展开，代价地图是代表网格中机器人安全单元格的一种地图。通常代价地图中的值是二进制的，表示自由空间或机器人会碰撞的位置。代价地图中的每个单元格都有一个范围为{0,255}的整数值。在这个范围内有几个特定值较为常用，如下所示：
-
255 (NO_INFORMATION): 保留给没有足够信息的单元格。
-
254 (LETHAL_OBSTACLE: 表示在此单元为会发生碰撞的障碍物。
-
253 (INSCRIBED_INFLATED_OBSTACLE): 表示没有障碍物，但将机器人的中心移动到此位置将导致碰撞。
-
0 (FREE_SPACE): 没有障碍物并且将机器人的中心移动到这个位置不会导致碰撞的单元格。
这里一共有两种代价地图：global_costmap全局代价地图 （基于静态地图生成，即pgm地图），local_costmap局部代价地图（基于传感器的实时数据生成），而上面的全局规划器就是基于全局代价地图来做的路径规划。
代价地图参数在三个不同文件中进行定义:
-
一个YAML文件设定全局代价地图的参数，我们把这个文件命名为 global_costmap_params.yaml。
-
一个YAML文件设定局部代价地图的参数，我们把这个文件命名为 local_costmap_params.yaml。
-
一个YAML文件为全局代价地图和局部代价地图共同设定参数，我们把这个文件命名为 common_costmap_params.yaml。
这里我们重点看一下global_costmap_params.yaml的内容。
global_frame: map #全局基坐标系
static_map: true #是否使用静态地图来初始化代价地图
rolling_window: false #是否使用代价地图的滚动窗口版本
plugins:
- {name: static, type: "costmap_2d::StaticLayer"} #静态地图
- {name: inflation, type: "costmap_2d::InflationLayer"} #膨胀障碍物层
- {name: obstacles, type: "costmap_2d::VoxelLayer"} #障碍物层
这里展开一下plugins的内容，这是一个带有名称和类型字段的字典，用于为代价地图添加图层，图层类似于一类参数的集合，例如静态地图、感知障碍物和膨胀被分成不同的层。这些层在 common_costmap_parameters.yaml 文件中定义,然后被添加到 local_costmap_params.yaml 和 global_costmap_params.yaml 文件中。
转载自 https://mp.weixin.qq.com/s/r_EgdPscMlGpuVSDlJxrzw