【cartographer_slam源码阅读】4-6激光雷达数据的转换

HandleLaserScanMessage()函数

作用：

1. 利用 ToPointCloudWithIntensities函数 将ros中的数据转换为carto中定义的数据类型；
2. 传入 HandleLaserScan() 函数中进行处理；

Note:

1. sensor_msgs::LaserScan数据格式：
2. PointCloudWithIntensities数据格式：

HandleMultiEchoLaserScanMessage() 函数同上类似；

HandlePointCloud2Message()函数

作用：

1. 利用ToPointCloudWithIntensities函数 将ros中的数据转换为carto中定义的数据类型；
2. 传入 HandleRangefinder() 函数中进行处理；

上面三个函数都调用了ToPointCloudWithIntensities函数，该函数根据传参数据格式不同进行了重载；

ToPointCloudWithIntensities()函数

LaserScanToPointCloudWithIntensities()函数

模板函数，处理ros格式的sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan数据格式，得到PointCloudWithIntensities类型数据；

数据格式

LaserScan（单回波激光雷达）：

header: 消息头，包含了该消息的帧id、时间戳和坐标系信息。
angle_min: 扫描起始角度，单位为弧度。(180度）
angle_max: 扫描终止角度，单位为弧度。（-180度）
angle_increment: 角度分辨率，即每个激光束之间的角度差，单位为弧度。(-0.18度）
time_increment: 时间分辨率，相邻两激光点的扫描时间增量，单位为秒。
scan_time: 一次扫描的持续时间，单位为秒。（0.06秒）
range_min: 能够测量到的最小距离值，单位为米。(0米）
range_max: 能够测量到的最大距离值，单位为米。（40米）
ranges: 扫描得到的距离值数组，按照顺序存放，单位为米。
intensities: 扫描得到的反射强度值数组，如果激光雷达不支持反射强度测量，则该字段为空。

MultiEchoLaserScan（多回波激光雷达）：

程序逻辑

1. 激光雷达异常数据判断
   
   

最终需要得到的数据格式：

激光点在指定坐标系的坐标position；
激光点时间戳time；
激光点强度intensities；

2. point坐标获取

（1）以单回波雷达数据的处理为例，HasEcho判断测距数据是否为空；
（2）取ranges测距里面的第一个值，注意（ranges 里面的值是按 扫描起始角度->扫描终止角度 顺序存储的）；
（3）判断数据在激光雷达有限测距范围内；
（4）根据测得的距离以及扫描起始角度，角度分辨率信息，循环计算每个扫描点在设置坐标系下的坐标，原理如图；
（5）这里还定义了第一次扫描点的时间为0，依次按照时间增量递增（与后面的时间戳定义有关）；
（6）激光雷达长时间未扫描到障碍物没有办法建图；

3. intensities强度数据获取

（1）强度可以理解为障碍物对激光束的反射率；
（2）判断雷达原始数据中有强度数据，检查扫描点个数必须与强度数据对齐，即一个扫描点必须对应一个强度信息，满足则push到carto定义的强度数据中，否则push数据0进去；

4. time数据获取

(1)激光雷达每帧数据自带时间戳为开始扫描的时间；
(2)获取points数据时，最后一个点的points.time数据记录了扫描一帧的用时duration，时间戳为每帧扫描结束后的时间；
(3)相应的，每个扫描点记录的时间改为points.time-duration（其实就是保证这一帧数据的时间戳加上points.time是每个扫描点的扫描时间）;