转载请注明作者,谢谢
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的路径类型,计算路径