Costmap执行逻辑
- 定义
layered_costmap_
(LayeredCostmap
),最终的地图。- 成员变量:
Costmap
- 成员变量:地图子层
- 成员变量:
layered_costmap_
根据自定义加载子地图层。- 各层初始化主地图
- 各层进行初始化。
- 静态层接受静态地图
- 障碍物层加载障碍物缓冲数据
- 膨胀层初始化计算成本的过程变量
- 通过创建新的线程(
mapUpdateLoop
)进入地图更新循环。- 更新地图
- 各层(根据位置)更新地图边界
- 静态层根据接收地图大小计算出尺寸
- 障碍物层根据障碍位置计算出地图大小
- 膨胀层在上面的基础上再扩大一个膨胀半径的大小
- 重置
master
地图(layered_costmap_
地图设置成默认值) - 各层计算
Cost
- 静态层根据接收到的地图以取最大值或覆盖的方式更新
master
地图 - 障碍物根据障碍位置更新
master
地图 - 膨胀层根据障碍物节点利用有序
map
,通过每次查找最近节点计算Cost
并更新master
地图
- 静态层根据接收到的地图以取最大值或覆盖的方式更新
- 各层(根据位置)更新地图边界
- 获取最终地图的边界,并发布地图
- 更新地图