racer多机探索代码解读——multi_map_manager.cpp【多图管理、子图融合】

一、概述

multi_map_manager.cpp是racer多机探索中特有的关于多图管理和子图融合的模块。若集群共含有3台无人机，则每个无人机维护4张子图【3张无人机子图、最后一张地面node(不发送chunk)】，每张子图包括两类信息：
1、本机多图中<某无人机>子图已经接受到chunk数据{每个chunk包含200个珊格体素，chunk是多机地图交换的基本单元}
2、本机多图中<某无人机>子图含有chunk的idx_list_信息
【chunk最小id (id对（buffer中相邻chunk的id差值大于1的前chunk的id、后chunk的id）) chunk最大id】
偶数索引起，相邻之间为已经完全接收到的chunk的id范围；奇数索引起，相邻之间为没有接受到的chunk的id范围

二、主要函数及作用

1、MultiMapManager::setMap

输入：sdf_map的this指针
作用：sdf map赋值

2、MultiMapManager::init

作用：1、launch参数读取；
     2、ros定时器、pub、订阅声明；
     3、本机多图、子图包围框初始化、chunk缓存数据及接受时刻初始化

3、MultiMapManager::updateMapChunk

输入：当前无人机的当前帧点云中的新发现的体素【可能free也可能占据】的一维数组地址
作用：
1、将本机当前帧点云中新发现体素存入chunk珊格缓存，并按照200个珊格地址为一组进行chunk拆解【一维地址集合、本机map所处chunk id、标志位等】并压入本机chunks集合
2、若此次为首次压入chunk，将本机map的idx_list_变为【1,本机map含有chunk的个数】；
3、adr_buffer_中已经插入到本机map中的新体素chunk删除

4、MultiMapManager::stampTimerCallback

定时器函数0.1s执行一次：1、本体无人机向其与无人机的进行chunk stamp的发布：本机id、发布时刻、本无人机保存的多图chunks{无人机+地面node}的idx_list
                     2、本体无人机子图中chunk的占据珊格的增量式发布

5、MultiMapManager::stampMsgCallback

回调函数：1、接受到某无人机发布的chunk stamp后，进行处理（对于同一架无人机的消息时间处理间隔需要大于0.3s）
        2、迭代本机5张子图『4个无人机+地面node』：对比本机<某无人机>子图拥有但是订阅消息多图同<某无人机>缺少的chunk的id范围，并进行chunk data的发布【按照chunk为单位发布】

6、MultiMapManager::findMissedChunkIds

idx索引从1开始
输入：本无人机的多图中某无人机子图的idx_list_[初始化：本机：【1，本机地图chunk个数】;其余无人机：【空】]  
     订阅消息【其余无人机发布】的同一无人机子图的idx_list_
作用：确定子图已经接受chunk的id范围与订阅消息未接受chunk的id范围的交叠部分：子图已经接受存在但是订阅消息未接受不存在chunk的id范围【即本子图可以发送出去的id范围】

7、MultiMapManager::findIntersect

输入： 区间1范围      区间2范围     交叠范围的下界     交叠范围的上界
作用： 确定两个区间的交叠部分【如果有交叠，则返回true；否则返回false】

8、MultiMapManager::sendChunks

输入：本无人机的多图中某无人机的id【从1开始】           
     订阅消息的无人机id【消息来源】          
     本无人机多图中某无人机有但是发布者中没有的chunk索引【从1开始】：初始化【本无人机本子图【1，本机地图chunk个数】;其余无人机：【空】】
作用：本无人机多图中某无人机子图有但是发布者中没有的chunk进行消息发布【无人机发布id、无人机接受id、无人机子图id、chunk在发布者子图中id、chunk地址数据、chunk占据数据】

9、MultiMapManager::getOccOfChunk

输入：本机本子图独有的某chunk的一维数组地址     本机本子图独有的某chunk的voxel_occ_【欲求】
作用：判断属于本chunk的珊格位置是否为占据状态：是返回1  否则为0

10、MultiMapManager::chunkCallback

回调函数：chunkdata的数据接受-----》将接受到chunk data插入到chunk_buffer_的<某无人机>buffer【chunk不存在重复，唯一的】（接受到的消息决定）、将【chunk的id，1】插入到buffer_map的<对应某无人机>位置【哈希map：不存在重复】
备注：0.1s的chundata数据处理定时器会进行数据清空

11、MultiMapManager::chunkTimerCallback

定时器函数0.1s执行一次：针对本无人机接收到的5张chunk buffer进行遍历：
1、如果<某无人机>的子图中不存在该chunk，则将该chunk压入到<某无人机>子图中；
2、且进行该<某无人机>子图的idx_List更新【【chunk最小id  (id对『buffer中相邻chunk的id差值大于1的前chunk的id、后chunk的id』)  chunk最大id】】
3、根据新加入到<某无人机>子图的chunk进行占据珊格地图、膨胀占据珊格地图的更新、子图包围框及整个map包围框的更新
4、chunk_buffer及buffer_map清空

12、MultiMapManager::mergeChunkIds

输入：
1、chunk_buffer中<某无人机>的chunk_buffer的idx_list【chunk最小id  (id对『buffer中相邻chunk的id差值大于1的前chunk的id、后chunk的id』)  chunk最大id】
2、<某无人机>子图的idx_list_【首次更新为空】       
3、int的向量【欲求】
作用：chunk_buffer中<某无人机>的id融合操作【<某无人机>子图的idx_list_内保存的为上一时刻的idx_list，<某无人机>的chunk_buffer【id融合后会清空】的idx_list保存为当前时刻接受到chunk的id情况【具体与子图idx_list_的参数注释同】】
备注：融合后的output表示<某无人机>的子图的最新id情况【偶数索引起，相邻之间为已经完全接收到的chunk的id范围；奇数索引起，相邻之间为没有接受到的chunk的id范围】

13、MultiMapManager::adrToIndex

输入：当前珊格的一维数组地址    珊格索引{欲求}
作用：根据某个珊格的一维数组地址计算对应的珊格索引

14、MultiMapManager::insertChunkToMap

输入：<某无人机>子图中某chunk{珊格一维数组地址、珊格占据状态、idx、empty_=true、need_query_=true}     
     <某无人机>的id
作用：遍历该chunk拥有的珊格一维地址：
1、根据chunk中珊格状态进行对应的占据珊格map中珊格占据值的更新；
2、进行<某无人机>子图包围框上下边界的更新；
3、全局map包围框的上界限和下界限的更新；
4、进行chunk中占据珊格、膨胀占据珊格map的状态更新。

三、多图管理逻辑图

四、其余地图模块链接

1、fuel无人机自主探索代码解读1——map_ros.cpp【地图ros接口】
2、fuel无人机自主探索代码解读2——sdf_map.cpp【占据栅格地图、膨胀占据栅格地图、ESDF】