Costmap 学习笔记

Costmap执行逻辑

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