fuel无人机自主探索代码解读1——map_ros.cpp【地图ros接口】

一、概述

map_ros.cpp订阅深度图/雷达点云、世界坐标下传感器pose，每0.05s启动esdf地图更新，每0.05s进行全局地图和局部地图可视化。主要功能为：接受传感器数据、调用sdf_map.cpp文件内部膨胀地图及esdf地图更新接口、地图可视化等。涉及传感器时间同步及深度图预处理等。

二、主要函数及作用

1、MapROS::setMap

作用：sdf地图赋值

2、MapROS::init

作用：地图ros接口初始化
1、launch参数的读取
2、map及接口参数的初始化
3、高斯噪声设置
4、ros定时器；pub声明；sub消息近似时间同步设置

3、MapROS::visCallback

作用：可视化定时器0.05s执行一次可视化
1、当前帧点云AABB包围框膨胀后的xy范围，z为探索高度范围，且可视化截断高度内，进行占据珊格位置点云及膨胀后被占据珊格位置点云的ros发布【局部地图】
2、被探索区域的边界框：下边界栅格索引   上边界珊格索引：将可视化截断范围内的占据珊格进行ros发布；统计探索范围内的已知状态珊格体积并存入文件【全局地图】

4、MapROS::updateESDFCallback

作用：定时器0.05s执行一次esdf更新
针对膨胀后的局部点云帧包围框采用欧式距离传输方法进行esdf的计算【最接近障碍物距离，结果存储在distance_buffer_中】
两种情况：无符号esdf：占据或未知为0，空白为正     有符号esdf：障碍物边缘珊格值为0，内部珊格值为负数，空白珊格为正

5、MapROS::depthPoseCallback

时间同步步骤：回调函数会在同步消息到达时被触发，并接收同步后的消息作为参数
作用：深度图-pose做时间同步后的消息回调
1、深度图深度像素滤波处理作为ros点云，继而得到各帧融合后占据珊格map【天花板作为占据】
2、膨胀占据珊格map

6、MapROS::cloudPoseCallback

时间同步步骤：回调函数会在同步消息到达时被触发，并接收同步后的消息作为参数
作用：点云-传感器pose做时间同步后的回调
1、输入激光雷达点云，传感器与点云点之间做ray-cast，得到各帧融合后占据珊格map【天花板作为占据】
2、膨胀占据珊格map

7、MapROS::proessDepthImage

作用：
1、深度图中像素深度值滤波处理【深度有最大最小阈值限制；深度图像素上下左右需要剔除一定带宽；深度图像素进行间隔提取访问】
2、将像素深度值转换到世界坐标系下构成深度点云并进行ros发布

8、MapROS::publishMapAll

作用：
在被探索区域的边界框：下边界栅格索引 上边界珊格索引内
1、将可视化截断范围内的占据珊格进行ros发布
2、统计探索范围内的已知状态珊格体积并存入文件

9、MapROS::publishMapLocal

作用：
当前帧点云AABB包围框膨胀后的xy范围，z为探索高度范围，可视化截断高度内：进行占据珊格位置点云及膨胀后被占据珊格位置点云的ros发布

10、MapROS::publishUnknown

作用：地图范围内的当前帧局部包围框内未知状态珊格点云生成及ros发布【可视化截断范围】

11、MapROS::publishDepth

作用：世界坐标系：深度图滤波后点云ros格式数据发布

12、MapROS::publishUpdateRange

作用：当前帧点云局部包围框的立方体可视化发布

13、MapROS::publishESDF

作用：进行当前帧点云AABB包围框xy各扩展55个珊格，z取给定高度值的珊格esdf点云ros发布【esdf对应于点云强度值】

三、参考资料

1、时间同步1
2、时间同步2
3、时间同步3