转载请注明作者，谢谢

MoveBace.cpp阅读笔记

开放的功能函数：

Action Server：

executeCb

Thread：

planThread

Sub CallBack ：

goalCB

Srv Callback：

planService

clearCostMapService

reconfigureCB

executeCb负责接收新目标点，并设置别是否进行全局规划的标志位（planThread函数中），之后具体的到点在executeCycle中进行

导航实际流程为：

进行全局路径规划，在进行局部路径规划，然后发布速度

全局路径规划在makePlan函数中，该函数中调用了planner_的makePlan和empty接口。

planner_为继承于BaseGlobalPlanner的实例，由pluginlib通过具体类的名字进行装载。

之后，调用tc_的setPlan接口，对局部路径规划器进行全局路径设置，然后，调用tc_的isReached接口进行判断，然后调用tc_的computeVelocityCommands接口，进行速度计算，然后进行速度下发。

tc_为继承于BaseLocalPlanner的实例，也是由pluginlinb通过具体类的名字进行装载。

下面带来两个问题，planner_怎么进行路径规划，以及tc_如何计算速度。

planner_在初始化时候，被塞入了planner_costmap_ros_

tc_在初始化时，被塞入了controller_costmap_ros_

在global planner的包中，注册了插件：global planner::GlobalPlanner

分层地图分析：

CostMap2D在外部被实例化的过程：

通过params读取plugins参数，遍历每个plugin，得到name和type，然后先声明一个Layer的指针，然后通过initialize函数初始化具体的Layer，其他的实例，比如StaticLayer都是继承于Layer的插件

StaticLayer首先继承于CostmapLayer，CostMapLayer继承于Layer和Costmap2D

在StaticLayer中，会申请接收maptopic，然后接收对应的地图信息，然后进行初始化继承于Costmap2D的成员变量costmap_，为char*类型，因此就可以进行占用栅格的初始化，maptopic通过参数指定，也就是外部指定接收哪副地图

其他所有的不同层地图同StaticLayer一样，为最终的具体的分层地图

每个具体的分层地图肯定都有updateCosts函数，用于更新他们的总包地图，而他们总包地图用于规划或者别的拥堵，总包地图的类型为LayerdCostmap

现有分层地图分析：

Static_layer: 继承于CostmapLayer

Obstacle_layer：继承于CostmapLayer

Footprint_layer：继承于Layer

Inflation_layer：继承于Layer

Voxel_layer：继承于Obstacle_Layer

分层地图使用总结：

最外层为Costmap2DROS, 成员变量layered_costmap_类型为LayeredCostmap

layered_costmap_中添加各层地图插件；

因此layered_costmap_为用于某一用途的总分层地图

在具体使用过程中，会调用Costmap2DROS的getCostmap接口，里面是返回了layered_costmap_的getCostmap接口，返回形式为Costmap2D的指针。在LayeredCostmap中，costmap_会被所有的添加过的plugin进行update，因此确实得知layered_costmap_确实为总cost地图，地图中的cost通过已经添加过得分层插件都分别进行了更新。

LayeredCostmap中的成员变量，Costmap2D*类型的costmap_，是实际被外部算法所使用的代价地图

全局规划模块分析

GlobalPlanner为BaseGlobalPlanner的插件

1、PlannerWithCostmap继承于GlobalPlanner，GlobalPlanner继承于BaseGlobalPlanner

接收goal，并且在poseCallback函数中，调用makePlan函数

2、costmap：

通过Costmap2DROS实例化，在Costmap2DROS的构造函数中，

new了LayeredCostmap类，并且进行了一系列初始化，

并且会进行Costmap2DPublisher的创建，

LayeredCostmap会通过plugin_loader进行多层地图的创建以及加载

进行footprint的接收

3、costmap和goal都塞在了GlobalPlanner中，costmap是初始化，goal是接收别的模块的发送

4、GlobalPlanner的构造函数中，存储外部new的costmap，并且进行类的Initialize，初始化函数中，进行全局规划算法的选择，dijkstra或者astar，存在成员变量planner_中，为在BaseGlobalPlanner中的成员变量

5、GlobalPlanner中的makePlan函数：进行规划，并且发布plan

6、makePlan解析：

a) 将start和goal世界坐标转化成costmap中的序号索引坐标worldToMap

b) 设置各种规划器尺寸 setSize（nx,ny）

c) 设置规划地图边框：outlineMap

d) 计算Potentials，根据具体路径规划算法比如AStar，Dijkstra进行

e) 根据初始位置，目标位置，potential，根据具体路径类型，继承于TraceBack的路径类型，计算路径