rs_driver v1.5.7 源代码解析（二）

rs_driver 是RoboSense雷达的基本驱动程序。本文是rs_driver的源代码解析文档，原文地址在：
https://github.com/RoboSense-LiDAR/rs_driver/blob/v1.5.7/doc/src_intro/rs_driver_intro_CN.md

4 Packet解析

4.1 MSOP格式

这里说明MSOP格式中的字段。

• 距离 distance、
• 角度
• 发射率 intensity、
• 通道 ring、
• 时间戳 timestamp、
• 温度 temperature
• 回波模式 echo_mode

其中前五个与点云直接相关。

MSOP格式中的点是极坐标系的坐标，包括极径和极角。距离就是这里的极径。从距离和角度可计算直角坐标系的坐标，也就是点云使用的坐标。

4.1.1 Distance

Distance用两个字节表示。它乘以一个解析度得到真实距离。

• 不同的雷达的解析度可能不同。
• 特定于雷达的配置参数RSDecoderConstParam::DISTANCE_RES保存这个解析度。

uint16_t distance;

4.1.2 角度

• 对于机械式雷达， MSOP格式的azimuth保存点的极角（水平角）。

  uint16_t azimuth;
  

  要计算点的直角坐标系坐标，除了distance和azimuth之外，还需要一个垂直角。

  • 垂直角从DIFOP Packet得到，一个机械式激光雷达有一组固定的垂直角，每个通道一个。后面的章节将说明垂直角。

  • 水平角是MOSP Packet中点的azimuth。

• 对于MEMS雷达， 角度是pitch和yaw。

  uint16_t yaw;
  uint16_t pitch;
  

  从distance、pitch、和yaw，可计算直角坐标系的坐标。

• 雷达的角度分辨率是0.01度。这意味着一圈360度，可以划分成36000个单位。

• MSOP格式中，角度以0.01度为单位，范围是(0, 36000)，所以可以用uint16_t来表示。

4.1.3 intensity

intensity保存在1个字节中。

uint8_t intensity;

4.1.4 ring

ring在后面的ChanAngles章节说明。

4.1.5 timestamp

RoboSense雷达使用了几种时间戳格式。

4.1.5.1 YMD 格式

YMD格式定义如下，parseTimeYMD()负责解析这种时间戳格式。遵循这种格式的有RS16/RS32/RSBP等。

typedef struct 
{ 
  uint8_t year; 
  uint8_t month; 
  uint8_t day; 
  uint8_t hour; 
  uint8_t minute; 
  uint8_t second; 
  uint16_t ms; 
  uint16_t us; 
} RSTimestampYMD; 

4.1.5.2 UTC 格式

UTC格式定义如下。

• 成员sec[6]保存的是秒数，
• 成员ss[4]保存微秒值或纳秒值。

typedef struct 
{ 
  uint8_t sec[6];
  uint8_t ss[4];
} RSTimestampUTC;

• 如果ss[4]保存微秒值，使用parseTimeUTCWithUs()解析。遵循这种格式的有RSHELIOS/RSM1。
• 如果ss[4]保存纳秒值，使用parseTimeUTCWithNs()解析。
• 目前出货的RS128/RS80都遵循微秒格式，只有早期出货的一些RS128/RS80是纳秒格式。当前版本的rs_driver只支持微秒格式的解析。

4.1.6 temperature

RoboSense雷达使用了几种温度格式。

4.1.6.1 用解析度表示温度

一种是用2字节表示温度值，这个值乘以一个解析度得到真实温度。

• 特定于雷达的配置参数RSDecoderConstParam::TEMPERATURE_RES保存这个解析度。

typedef struct 
{ 
  uint8_t tt[2];
} RSTemperature;

• 如果这两个字节是littlen endian格式，使用parseTempInBe()解析。遵循这种格式的有RS16/RS32/RSBP/RSHELIOS。
• 如果这两个字节是big endian格式，使用parseTempInLe()解析。遵循这种格式的有RS80/RS128。

4.1.6.2 相对温度

另一类用1个字节表示温度。这个值加上一个初始值得到真实温度。遵循这种格式的有RSM1。

• 特定于雷达的配置参数RSDecoderConstParam::TEMPERATURE_RES保存这个初始值。

int8_t temperature;

4.1.7 echo_mode

雷达内部有多种回波模式。

• 最强回波，
• 最后回波，
• 双回波，

MSOP格式中用一个字节表示：

int8_t return_mode;

但rs_driver并不在意是回波是“最强的”，还是“最后的”。因为影响MSOP解析的只是：有几个回波？

如下是才是rs_driver关心的回波模式。

enum RSEchoMode
{
  ECHO_SINGLE = 0,
  ECHO_DUAL
};

不同雷达有不同的回波模式return_mode。每个Decoder实现自己的解析函数getEchoMode()，得到rs_driver的回波模式。

回波模式会影响MSOP Packet中数据的布局，还可能影响点云的分帧。

4.2 ChanAngles

4.2.1 垂直角/水平角的修正

如前面MSOP格式的章节所说，理论上，从distance、垂直角、水平角就可以计算点的直角坐标系的坐标。

但在生产实践中，装配雷达总是无可避免地有细微的误差，导致雷达的角度不精确，需要进行修正。雷达装配后的参数标定过程，会找出相关的修正值，写入雷达的寄存器。标定后，使用修正值调整点，就可以使其精度达到要求。

MEMS雷达的角度修正，在雷达内部完成，所以rs_driver不需要做什么；机械式雷达的角度修正，由rs_driver在电脑主机端完成。

这里说明机械式雷达的垂直角/水平角修正。

对于机械式雷达，每个通道的垂直角是固定的。以RSBP举例，它的理论垂直角如下。（这里有32个值，对应RSBP的32个通道）

  89.5,    81.0625, 78.25,   72.625,  67,      61.375,  55.75,   50.125, 
  86.6875, 83.875,  75.4375, 69.8125, 64.1875, 58.5625, 52.9375, 47.3125, 
  44.5,    38.875,  33.25,   27.625,  22,      16.375,  10.75,   5.125, 
  41.6875, 36.0625, 30.4375, 24.8125, 19.1875, 13.5625, 7.9375,  2.3125

装配过程中的误差，导致雷达的垂直角不是这里列出的理论值，水平角azimuth也不是从零开始。标定过程找出两组修正值，一组针对垂直角，一组针对水平角。

还是以RSBP为例。标定过程后，实际的垂直角可能是这样的。这里修正值已经累加了原来的垂直角。

  89.4375, 81.0625, 78.25,   72.625,  67,      61.375,  55.75,   50.125, 
  86.8125, 83.875,  75.4375, 69.8125, 64.1875, 58.5,    52.9375, 47.3125, 
  44.5625, 38.875,  33.25,   27.625,  22,      16.375,  10.75,   5.125, 
  41.6875, 36.1875, 30.4375, 24.8125, 19.0625, 13.5625, 7.9375,  2.3125

类似的，水平角修正值的例子如下。（这里也有32个值，对应RSBP的32个通道）

   0.0625,  0.0625, -0.25,    0.625,  0,      -0.375,   0.75,   -0.125, 
  -0.3125,  0.875,  -0.4375,  0.8125, 0.1875,  0.5,    -0.9375,  0.3125, 
   0.0,    -0.875,   0.25,   -0.625,  0,      -0.375,   0.75,    0.125, 
   0.125,   0.1875,  0.4375,  0.8125, 0.0625,  0.5625,  0.9375,  0.3125

这两组修正值在参数标定过程中写入雷达寄存器，它们也包含在DIFOP Packet中。

4.2.2 ChanAngles定义

ChanAngles从DIFOP Packet读入机械式雷达的垂直角/水平角的修正值。如果雷达修正值无效，也可以从外部文件读入。

如前面所说，只有机械式雷达需要ChanAngles。

class_chan_angles

• 成员变量chan_num_是雷达的通道数，用于决定修正值数组的大小。
• 成员变量vert_angles_是保存垂直角修正值的数组
• 成员变量horiz_angles_是保存水平角修正值的数组。

4.2.3 ChanAngles::loadFromDifop()

loadFromDifop()从DIFOP Packet读入角度修正值，写入成员vert_angles_[]和horiz_angles_[]。

• 它还调用genUserChan()， 设置用户通道编号数组。

• 在DIFOP Packet中，修正值保存在RSClibrationAngle结构中。

  • 成员value是非负值，
  • 成员sign则指定正负; 0则修正值为正；除0xFF以外的非0值，则修正值为负；为0xFF时，修正值无效。

typedef struct
{
  uint8_t sign;
  uint16_t value;
} RSCalibrationAngle;

对于RSBP, MSOP Packet中的修正值保存在成员vert_angle_cali[]和horiz_angle_cali[]中。

typedef struct
{
  ...

  RSCalibrationAngle vert_angle_cali[32];
  RSCalibrationAngle horiz_angle_cali[32];

  ...
} RSBPDifopPkt;

4.2.4 早期雷达适配ChanAngles

• 不是所有雷达的修正值都保存在RSCalibrationAngle中。比如早期的雷达RS16，它的修正值保存在成员pitch_cali[]中。

  typedef struct
  {
    ...
    uint8_t pitch_cali[48];
    ...
  } RS16DifopPkt;
  
  

  为了像其他雷达一样处理RS16，将RS16DifopPkt适配到一个能兼容RSCalibrationAngle的结构 AdapterDifopPkt,

  typedef struct
  {
    uint16_t rpm;
    RSFOV fov;
    uint8_t return_mode;
    RSCalibrationAngle vert_angle_cali[32];
    RSCalibrationAngle horiz_angle_cali[32];
  } AdapterDifopPkt;
  

  RS16使用了转换函数RS16DifopPkt2Adapter()，从RS16DifopPkt构造一个AdapterDifopPkt。

  void RS16DifopPkt2Adapter (const RS16DifopPkt& src, AdapterDifopPkt& dst);
  

• RS32也有类似的情况。虽然它的修正值也保存在RSCalibrationAngle数组中，但角度值的含义不同。

  typedef struct
  {
    ...
    RSCalibrationAngle vert_angle_cali[32];
    RSCalibrationAngle horiz_angle_cali[32];
    ...
  } RS32DifopPkt;
  

  与RS16类似，也将RS32DifopPkt适配到AdapterDifopPkt。RS32使用的转换函数是RS32DifopPkt2Adapter()。

4.2.5 ChanAngles::loadFromFile()

有些场景下，雷达可能还没有写入有效的修正值，或者是因为还没有标定，或者是由于雷达故障。这时可以从外部文件读入角度修正值。

文件格式如下。

• 每行是对应一个通道的修正值。其中第1个值是垂直角，第2个值是水平角修正值。
• 每行对应一个通道。所以对于RSBP来说，应该有32行。这个例子略去了部分行。

89.5,0.125
81.0625,-0.025
78.25,0
72.625,0
...
30.4375,0.625
24.8125,0
19.1875,-0.25
13.5625,0
7.9375,-0.1
2.3125,0

loadFromFile() 解析这个文件得到修正值，写入成员vert_angles_[]和horiz_angles_[]。

• 它还调用genUserChan()， 设置用户通道编号数组。

4.2.6 ChanAngles::horizAdjust()

horizAdjust()对参数给出的水平角作修正

• 根据内部通道编号得到水平角修正值，
• 水平角加入这个修正值，并返回

4.2.7 ChanAngles::vertAdjust()

vertAdjust()根据内部通道编号，得到修正后的垂直角。

4.2.8 点云的ring值

点云中的ring值是从用户角度看的通道编号，它来自于ChanAngles的成员变量user_chans_。

回到RSBP的例子。如下是它的垂直角，它们按照雷达内部通道编号排列，而不是降序或升序排列。换句话说，雷达内部通道不是按照垂直角的升序/降序编号的。

 89.5,    81.0625, 78.25,   72.625,  67,      61.375,  55.75,   50.125, 
 86.6875, 83.875,  75.4375, 69.8125, 64.1875, 58.5625, 52.9375, 47.3125, 
 44.5,    38.875,  33.25,   27.625,  22,      16.375,  10.75,   5.125, 
 41.6875, 36.0625, 30.4375, 24.8125, 19.1875, 13.5625, 7.9375,  2.3125

但用户希望看到通道编号按照垂直角按升序/降序排列。

ChanAngles的成员变量user_chans保存的是按升序排列的编号，也就是角度小的通道在前。

4.2.9 ChanAngles::genUserChan()

genUserChan()根据成员变量vert_angles_[]中的角度值，计算升序排列的用户通道编号数组。

4.2.10 ChanAngles::toUserChan()

toUserChan()，从给出的雷达内部通道编号，得到用户通道编号。

4.3 Trigon

4.3.1 查表计算三角函数值

如前面所说，MSOP Packet中的点是极坐标系的坐标。rs_driver将点坐标，从极坐标系转换为用户使用的直角坐标系。这时需要计算角度的sin/cos值。

调用三角函数又耗时又耗CPU资源，优化的方式是查表。

• 首先确定表的范围。

  • 垂直角的范围在(-90,90)内。加上修正值，也还是在(-90, 90)内。
  • 水平角的范围在(0, 360)内。加上修正值，在(-90, 450)内。

• MSOP格式中，角度以0.01度为单位。rs_driver也是这样。对于(-90, 450)的角度范围，需要对(-9000, 45000)内的整数角度值，计算sin/cos值。

4.3.2 Trigon定义

Trigon用于计算指定范围内的sin/cos值，并用于查询。

class_trigon

• 成员变量ANGLE_MIN和ANGLE_MAX保存角度范围。这里ANGLE_MIN = -9000, ANGLE_MAX = 45000。
• 成员变量o_sins_保存所有角度的sin值，o_coss_保存所有角度的cos值。o_sins_[]和o_coss_[]是两个大小为 AMGLE_MAX - ANGLE_MIN 的数组。
• 引用os_sins_[]和o_coss_[]计算三角函数值时，需要减去一个偏移。为了免去这个麻烦，重新定义了两个指针sins_和coss_，让它们分别指向os_sins_[9000]和os_cons_[9000]。这样就可以用角度值直接引用sins_和coss_了。

trigon_sinss

4.3.3 Trigon::Trigon()

Trigon的构造函数Trigon() 负责初始化o_sins_[]和o_coss_[]。

• 根据角度范围，给o_sins[]和o_coss_[]分配相应大小的空间，
• 遍历范围内的角度值，调用std::sin()和std::cos()，将三角函数值分别保存到o_sins_[]和o_coss_[]中。
• 让sins_指向sins_[]中0度角的位置，这里是sins_[9000]。类似地设置coss_。

4.3.4 Trigon::sin()

sin()查表返回角度的sin值。

4.3.5 Trigon::cos()

cos()查表返回角度的cos值。

4.4 BlockIterator

这一节"BlockIterator"，仅针对机械式雷达。

4.4.1 Packet、Block、Channel

在MSOP格式中，每个Packet中有BLOCKS_PER_PKT个Block，每个Block中有CHANNELS_PER_BLOCK个Channel。

• 这里的BLOCKS_PER_PKT和CHANNEL_PER_BLOCK分别在雷达配置参数RSDecoderConstParam中指定。

对于机械式雷达，雷达持续旋转，垂直方向上的每一轮激光发射，在MSOP Packet中对应一个Block。
以RSBP雷达为例，

• 一轮就是32次激光发射，对应32个channel。所以RSDecoderConstParam::CHANNELS_PER_BLOCK = 32。
• MSOP的设计初衷，当然是向每个Packet尽可能多塞几个Block，这样就有RSDecoderConstParam::BLOCKS_PER_PKT = 12。

msop_packet

雷达的每轮激光发射时序包括充能、发射等步骤。虽然每轮发射的持续时间（也是相邻两轮发射的时间差）相同，但在每轮发射内，每次发射的时间不是均匀分布。

以RSBP为例，

• 一轮发射的时长为55.52微秒，这是Block之间的时间差。
• 一轮发射内，32次发射的时间戳如下（相对于Block的相对时间，单位微秒）。这是每个Channel对所属Block的相对时间。

  0.00,  2.56,  5.12,  7.68, 10.24, 12.80, 15.36, 17.92, 
 25.68, 28.24, 30.80, 33.36, 35.92, 38.48, 41.04, 43.60,
  1.28,  3.84,  6.40,  8.96, 11.52, 14.08, 16.64, 19.20,
 26.96, 29.52, 32.08, 34.64, 37.20, 39.76, 42.32, 44.88

4.4.2 Channel的时间戳

MSOP格式中，Packet头部包含一个时间戳。

如RSBP雷达，Packet的时间戳如下。

RSTimestampYMD timestamp;

通过如下方式可以计算Channel的时间戳。

Block的时间戳 = Packet的时间戳 + Block的持续时间 * Block数
Channel的时间戳 = 所在Block的时间戳  + Channel对Block的相对时间

4.4.3 Channel的角度

在MSOP格式中，Block的成员中包括水平角azimuth。

雷达的旋转当然不是匀速的，但放到一个Block这么短的时间内看，认为旋转是匀速的，还是合理的。

所以，通过Channel占Block的时间比例，可以估计Channel对Block的相对水平角。

Channel的水平角 = Block的水平角 + 当前Block与下一个Block的水平角差 * Channel对Block的相对时间 / Block的持续时间

4.4.4 双回波模式的影响

双回波模式下，虽然一个Packet还是塞了同样数目的Block，但是第二个回波的Block，其水平角/时间戳与第一个回波相同。

如下是双回波模式下，RSBP的MSOP格式。

5.png

这样，遍历Block序列时，计算Block时间戳/角度差的方式就不一样了。

4.4.5 BlockIterator定义

引入BlockIterator的目的，是定义一个接口来计算：

• Block的时间戳。这个时间戳是相对于Packet的时间戳。
• Block与下一次Block的水平角差。也就是当前Block内雷达旋转的水平角。

BlockIterator的成员如下。

• 成员变量pkt_是Packet

• 成员变量BLOCKS_PER_PKT是Packet中的Block数

• 成员BLOCK_DURATION是Block的持续时间

• 成员az_diffs[]保存所有Block的水平角差

• 成员tss[]保存所有Block的时间戳

class_block_iterator

4.4.5.1 BlockIterator::get()

成员函数get()从成员az_diffs[]和tss[]，得到Block的时间戳和水平角差。BlockIterator的派生类应该计算这两个数组中的值。

4.4.6 SingleReturnBlockIterator

SingleReturnBlockIterator实现单回波模式下的BlockIterator接口。

4.4.6.1 SingleReturnBlockIterator()

单回波模式下。
在构造函数中，遍历Packet中的block，并计算az_diffs[]和tss[]。

• Block之间的时间差是固定值，也就是BLOCK_DURATION。

• 1个Packet有BLOCKS_PER_PKT个Block。

  • 对于前面的Block，

  Block水平角差 = 下一个Block的水平角 - 当前Block的水平角
  

  • 最后一个Block的水平角差，认为等于BLOCK_AZ_DURATION，这是雷达理论上每个Block的水平角差。

• 相邻Block可能跨角度0，所以它们的水平角差可能小于0，这时需要将它修正到[0, 36000)内。

4.4.7 DualReturnBlockIterator

DualReturnBlockIterator实现双回波模式下的BlockIterator接口。

4.4.7.1 DualReturnBlockIterator()

双回波模式下，Block两两成对。
在构造函数中，遍历Packet中的Block，并计算az_diffs[]和tss[]。遍历时步进值为2。

• 步进差为2的Block, 时间差为BLOCK_DURATION。奇数Block和前一个偶数Block的时间戳相同。

  • 对于前面的Block,

  Block水平角差 = 下下个Block的水平角 - 当前Block的水平角
  

  • 最后两个Block的角度差，认为等于BLOCK_AZ_DURATION，这是雷达理论上每个Block的水平角差。

• 由于相邻Block可能跨角度0，所以它们的水平角差可能小于0，这时需要将它修正到 [0, 36000)内。

4.4.8 ABReturnBlockIterator

双回波模式下，Ruby128是个特殊情况。

Ruby128的每个Block有128个Channel，每个Block占的空间太大，以至于每个packet只能放下3个Block。这样一次扫描的两个Block可能在不同的Packet中。相邻的两个packet格式如下图。

msop_ruby128

ABReturnBlockIterator类用于计算Ruby128的双回波模式的时间戳和角度。

4.4.8.1 ABDualReturnBlockIterator()

根据第0个Block和第1个Block的角度是否相同，判断这是一个AAB Packet还是BAA Packet。

• A与B之间的时间差为BLOCK_DURATION。

• 不论是AAB Packet，还是BAA Packet, Block的角度差都是A与B之间的角度差。

Block水平角差 = 第三个Block的水平角 - 第一个Block的水平角

4.4.9 RS16的Packet格式

为了充分利用MSOP Packet的空间，16线雷达(如RS16)的Packet格式与其他机械式雷达不同。

在单回波模式下，一组16通道数据包含一个回波，将相邻两组的回波数据放在同一Block中。如下图所示。

rs16_msop_single_return

在双回波模式下，一组16通道数据就有两个回波，将两个回波的数据放在同一Block中。如下图所示。

rs16_msop_dual_return

这样的结果是，

• MSOP Packet中的相邻Block之间的角度差不再一定是BLOCK_AZ_DURATION，时间差也不再一定是BLOCK_DURATION。
• 对于RS16，RSDecoderConstParam.CHANNELS_PER_BLOCK = 32。遍历所有通道时，这个值需要做一次映射，才能得到实际的通道值。

uint16_t laser = chan % 16;

RS16SingleReturnBlockIterator和RS16DualReturnBlockIterator，分别处理RS16单回波模式和双回波模式的情况。

• 新加入的成员函数calcChannel()，计算Block内每个Channel的角度占比，和时间戳偏移。

classes_rs16_block_iterator

4.4.9 RS16SingleReturnBlockIteratore

4.4.9.1 Rs16SingleReturnBlockIterator()

在构造函数中，遍历Packet中的Block，并计算az_diffs[]和tss[]。

与SingleReturnBlockIterator()中不同的地方是：单回波模式下，一个Block中包括相邻的两个通道数据。

• 缺省的角度差是BLOCK_AZ_DURATION * 2
• 缺省的时间差是BLOCK_DURATION * 2

4.4.9.2 RS16SingleReturnBlockIterator::calcChannel()

calcChannel()计算单回波模式下，32个Channel的角度占比，和时间戳偏移。

4.4.10 RS16DualReturnBlockIterator

4.4.10.1 Rs16DualReturnBlockIterator()

在构造函数中，遍历Packet中的Block，并计算az_diffs[]和tss[]。

与Rs16DualReturnBlockIterator不同的地方是：双回波模式下，一个Block中包括两个回波的数据。

• 缺省的角度差是BLOCK_AZ_DURATION
• 缺省的时间差是BLOCK_DURATION

4.4.10.2 RS16SingleReturnBlockIterator::calcChannel()

calcChannel()计算双回波模式下，32个Channel的角度占比,和时间戳偏移。

4.5 FOV

机械式雷达的扫描角度是[0,360)，这也是雷达输出点的角度范围。

也可以对雷达进行设置，限制它的输出角度，如下图。

• FOV的范围是[start_angle,end_angle)。
• 与FOV互补的角度范围FOV_blind是FOV的盲区，雷达不会输出这个范围的点。

fov

4.5.1 FOV设置

FOV可以从DIFOP Packet得到。

  RSFOV fov;

RSFOV的定义如下。

typedef struct
{ 
  uint16_t start_angle;
  uint16_t end_angle;
} RSFOV;

在DecoderMech::decodeDifopCommon()中解析DIFOP Packet得到FOV。

• 这里计算雷达扫描跨过盲区的时间差，也就是DecoderMech的成员fov_blind_ts_diff_.

void DecoderMech::decodeDifopCommon(const T_Difop& pkt);

4.5.2 FOV对BlockIterator的影响

在BlockIterator的各种实现中，需要考虑Packet的相邻两个Block跨过FOV盲区的情况。
如果跨过盲区，则：

• 第一个Block的水平角度差调整为BLOCK_AZ_DURATION。这时理论上每个Block的水平角差。
• 两个Block的时间差调整为FOV_BLIND_DURATION。这个值是盲区时间差，也就是前面说的fov_blind_ts_diff_。
  
  12.png

4.6 分帧

机械式雷达和MEMS雷达的分帧策略不同。

4.6.1 机械式雷达的分帧模式

机械式雷达持续旋转，输出点，驱动在某个分割位置分割前后帧。有三种分帧模式。

• 按Block的水平角分帧。这是默认的分帧模式。
  • 如果Block的水平角刚好跨过指定的水平角，则分帧。
  • 雷达的转动不是均匀的，所以每帧包含的Block数可能会有细微的变动，相应地，包含的点数也会变动。

split_angle

• 按理论上每圈的Block数分帧。这样每帧包含的Block数和点数都是固定的。
  • Robosense雷达支持两种转速：600圈/分钟和1200圈/分钟。以600圈/分钟距离，相当于每圈0.1秒，而Block的持续时间是固定的，由此计算理论上每圈的Block数（实际上是假设雷达转速均匀）

 每圈Block数 = 每圈秒数 / Block持续时间g 

• 理论上每圈Block数，在不同回波模式下不同。上面的计算是针对单回波模式。如果是双回波模式，则每圈Block数要加倍。

• 按照使用者指定的Block数分帧。当然这样每帧的Block数和点数也都是固定的。

4.6.2 SplitStrategy

SplitStrategy定义机械式雷达的分帧模式接口。

• 使用者遍历Packet中的Block，以Block的水平角为参数，调用SplitStrategy::newBlock()。应该分帧时，newBlock()返回true，否则返回false。

classes_split_strategy

4.6.3 SplitStrategyByAngle

SplitStrategyByAngle按Block角度分帧。

• 成员split_angle_保存分割帧的角度。
• 成员prev_angle_保存前一个Block的角度。

4.6.3.1 SplitStrategyByAngle::newBlock()

当前一个Block的角度prev_angle_在split_angle_之前，而当前Block的角度在split_angles_之后，则认为当前Block跨越了split_angles_，返回true。

• 这里考虑了Block的角度跨越角度0的情况。

4.6.4 SplitStrategyByNum

SplitStrategyByNum实现按Block数分帧。

• 成员max_blks_是每帧的Block数。
• 成员blks_是当前已累积的Block数。

4.6.4.1 SplitStrategyByAngle::newBlock()

newBlock()简单地累加Block数到成员blks_，当blk_达到max_blks_时，则返回true。

4.6.5 MEMS雷达的分帧模式

MEMS雷达的分帧是在雷达内部确定的。

• 一帧的MSOP Packet数是固定的。假设这个数为N, 则雷达给Packet编号，从1开始，依次编号到N。
• 对于RSM1，单回波模式下，Packet数是630；在双回波模式下，输出的点数要翻倍，相应的，Packet数也要翻倍，Packet数是1260。

4.6.6 SplitStrategyByPktSeq

SplitStrategyBySeq按Packet编号分帧。

• 注意SplitStrategyBySeq的接口与SplitStrategy不同，不是后者的派生类。
• 成员变量prev_seq_是前一个Packet的编号。
• 成员变量safe_seq_min_和safe_seq_max，是基于prev_seq_的一个安全区间。

class_split_strategy_by_seq

4.6.6.1 SplitStrategyByPktSeq::newPacket()

使用者用MSOP Packet的编号值，调用newPacket()。如果分帧，返回true。

MSOP使用UDP协议，理论上Packet可能丢包、乱序。

先讨论没有安全区间时，如何处理丢包、乱序。

• 理想情况下，如果不丢包不乱序，Packet编号从1到630, 只需要检查Packet编号是不是1。如果是就分帧。
• 那假如只有丢包呢？举个例子，如果编号为1的Packet丢了，则可以加入检查条件，就是当前Packet编号小于prev_seq_，就分帧。
• 在乱序的情况下，这个检查条件会导致另一个困境。举个例子，如果编号为300和301的两个Packet乱序，那么这个位置分帧，会导致原本的一帧拆分成两帧。

为了在一定程度上包容可能的Packet丢包、乱序情况，引入安全区间的概念。

• 以prev_seq_为参考点，划定一个范围值RANGE,

safe_seq_min_ = prev_seq_ - RANGE
safe_seq_max_ = prev_seq_ + RANGE

safe_range

• 如果Packet在范围(safe_seq_min_, safe_seq_max_)内，都不算异常，丢包、乱序都不触发分帧。这样在大多数情况下，之前的问题可以避免。

4.7 点云的有效性校验

4.7.1 AzimuthSection

AzimuthSection检查水平角是否在有效范围内。

• 成员变量start_指定这个范围的起始角度，end_指定这个范围的结束角度。支持跨水平角0的情况，比如start = 35000, end = 10。
• 用户可以通过用户配置参数RSDecoderParam::start_angle，和RSDecoderParam::start_angle指定这个范围。

class_azimuth_section

4.7.1.1 AzimuthSection::in()

in()检查指定的角度angle是否在有效范围内。

4.7.2 DistanceSection

DistanceSection检查指定的distance是否在有效范围内。

• 成员min_和max_分别是这个范围的最小值和最大值。
• 不同雷达有不同的测距范围。雷达配置参数RSDecoderConstParam::DISTANCE_MIN，和RSDecoderConstParam::DISTANCE_MAX指定这个范围。
• 用户也可以通过用户配置参数RSDecoderParam::min_distance， 和RSDecoderParam::max_distance进一步限缩这个范围。

class_distance_section

4.7.2.1 DistanceSection::in()

in()检查指定的distance是否在有效范围内。

4.8 Decoder

Decoder解析雷达MSOP/DIFOP Packet，得到点云。

• 它是针对所有雷达的接口类，包括机械式雷达和MEMS雷达。

DecoderMech派生于Decoder，给机械式雷达完成一些通用的功能，如解析DIFOP Packet。

每个机械式雷达分别派生自DecoderMech的类，如DecoderRS16、DecoderRS32、DecoderBP、DecoderRSHELIOS、DecoderRS80、DecoderRS128等。

MEMS雷达的类，如DecoderRSM1，直接派生自Decoder。

DecoderFactory根据指定的雷达类型，生成Decoder实例。

classes_decoder

4.8.1 Decoder定义

如下图是Decoder的详细定义。

• 成员const_param_是雷达的参数配置。
• 成员param_是用户的参数配置。
• 成员trigon_是Trigon类的实例，提供快速的sin/cos计算。定义如下的宏，可以清晰、方便调用它。

#define SIN(angle) this->trigon_.sin(angle)
#define COS(angle) this->trigon_.cos(angle)

• 成员packet_duration_是MSOP Packet理论上的持续时间，也就是相邻两个Packet之间的时间差。Decoder的派生类计算这个值。
  • InputPcap回放PCAP文件时，需要这个值在播放Packet之间设置延时。
• 成员echo_mode_是回波模式。Decoder的派生类解析DIFOP Packet时，得到这个值。
• 成员temperature_是雷达温度。Decoder的 派生类解析MSOP Packet时，应该保存这个值。
• 成员angles_ready_是当前的配置信息是否已经就绪。不管这些信息是来自于DIFOP Packet，还是来自外部文件。
• 成员point_cloud_是当前累积的点云。
• 成员prev_pkt_ts_是最后一个MSOP Packet的时间戳，成员prev_point_ts_则是最后一个点的时间戳。
• 成员cb_split_frame_是点云分帧时，要调用的回调函数。由使用者通过成员函数setSplitCallback()设置。

class_decoder

4.8.1.1 RSDecoderConstParam

RSDecoderConstParam是雷达配置参数，这些参数都是特定于雷达的常量

• MSOP_LEN是MSOP Packet大小
• DIFOP_LEN是DIFOP Packet大小
• MSOP_ID[]是MSOP Packet的标志字节。各雷达的标志字节不同，用MSOP_ID_LEN指定其长度。
• DIFOP_ID[]是DIFOP Packet的标志字节。各雷达的标志字节不同，用DIFOP_ID_LEN指定其长度。
• BLOCK_ID[]是MSOP Packet中Block的标志字节。所有雷达都是两个字节。
• LASER_NUM是雷达的通道数。如RS16是16， RS32是32，RS128是128。
• BLOCKS_PER_PKT、CHANNELS_PER_BLOCK分别指定每个MSOP Packet中有几个Block，和每个Block中有几个Channel。
• DISTANCE_MIN、DISTANCE_MAX指定雷达测距范围
• DISTANCE_RES指定MSOP格式中distance的解析度。
• TEMPERATURE_RES指定MSOP格式中，雷达温度值的解析度。

struct RSDecoderConstParam
{
  // packet len
  uint16_t MSOP_LEN;
  uint16_t DIFOP_LEN;

  // packet identity
  uint8_t MSOP_ID_LEN;
  uint8_t DIFOP_ID_LEN;
  uint8_t MSOP_ID[8];
  uint8_t DIFOP_ID[8];
  uint8_t BLOCK_ID[2];

  // packet structure
  uint16_t LASER_NUM;
  uint16_t BLOCKS_PER_PKT;
  uint16_t CHANNELS_PER_BLOCK;

  // distance & temperature
  float DISTANCE_MIN;
  float DISTANCE_MAX;
  float DISTANCE_RES;
  float TEMPERATURE_RES;
};

4.8.1.2 Decoder::processDifopPkt()

processDifopPkt()处理DIFOP Packet。

• 校验Packet的长度是否匹配。
• 校验Packet的标志字节是否匹配。
• 如果校验无误，调用decodeDifopPkt()。这是一个纯虚拟函数，由各雷达的派生类提供自己的实现。

4.8.1.3 Decoder::processMsopPkt()

processMsopPkt()处理MSOP Packet。

• 检查当前配置信息是否已经就绪(angles_ready_)。
  • 对于机械式雷达，当angles_readys = false时，驱动还没有获得垂直角信息，这时得到的点云是扁平的。所以用户一般希望等待angles_ready = true 才输出点云。
  • 通过用户配置参数RSDecoderParam::wait_for_difop，可以设置是否等待配置信息就绪。
• 校验DIFOP Packet的长度是否匹配。
• 校验DIFOP Packet的标志字节是否匹配。
• 如果以上校验通过，调用decodeMsopPkt()。这是一个纯虚拟函数，由各雷达的派生类提供自己的实现。

4.8.1.4 Decoder::transformPoint()

transformPoint() 对点做坐标变换。它基于第三方开源库Eigen。

默认情况下，transformPoint()功能不开启。要启用这个特性，编译时使用-DENABLE_TRANSFORM选项。

cmake -DENABLE_TRANSFORM .

4.8.2 DecoderMech

DecoderMech处理机械式雷达的共同特性，如转速，分帧角度、光心补偿等。

• 成员chan_angles_是ChanAngles类实例，保存角度修正信息。
• 成员scan_section_是AzimuthSection类实例，保存角度校验信息。
• 成员split_strategy_是SplitStrategy类实例，保存分帧策略。
• 成员rps_是雷达转速，单位是转/秒(round/second)。
• 成员blks_per_frame_是单回波模式下，理论上的每帧Block数。
• 成员split_blks_per_frame_是按Block数分帧时，每帧的Block数。包括按理论上每圈Block数分帧，和按用户指定的Block数分帧。
• 成员block_azi_diff_是理论上相邻block之间的角度差。
• 成员fov_blind_ts_diff_是FOV盲区的时间差

class_decoder_mech

4.8.2.1 RSDecoderMechConstParam

RSDecoderMechConstParam基于RSDecoderConstParam，增加机械式雷达特有的参数。

• RX、RY、RZ是雷达光学中心相对于物理中心的坐标。
• BLOCK_DURATION是Block的持续时间。
• CHAN_TSS[]是Block中Channel对Block的相对时间。
• CHAN_AZIS[]是Block中Channel占Block的时间比例，也是水平角比例。

struct RSDecoderMechConstParam
{
  RSDecoderConstParam base;

  // lens center
  float RX;
  float RY;
  float RZ;

  // firing_ts/chan_ts
  double BLOCK_DURATION;
  double CHAN_TSS[128];
  float CHAN_AZIS[128];
};

• Decoder初始化时，从每轮激光发射中的每次发射时间表，计算CHAN_TSS[]、CHAN_AZIS[]。这可以简化每个Channel的时间戳和角度的计算。
  前面的"Channel的时间戳"章节，已经列出过RSBP的发射时间表，如下。

  0.00,  2.56,  5.12,  7.68, 10.24, 12.80, 15.36, 17.92, 
 25.68, 28.24, 30.80, 33.36, 35.92, 38.48, 41.04, 43.60,
  1.28,  3.84,  6.40,  8.96, 11.52, 14.08, 16.64, 19.20,
 26.96, 29.52, 32.08, 34.64, 37.20, 39.76, 42.32, 44.88

4.8.2.2 DecoderMech::decodeDifopCommon()

decodeDifopCommon()解析DIFOP Packet。

• 解析Packet中的rpm，得到rps_.

 uint16_t rpm;

• 计算单回波模式下，每帧Block数，也就是blks_per_frame_。

每帧Block数 = (1/rps) / Block的持续时间

• 计算相邻Block之间的角度差，也就是block_azi_diff_。

Block间的角度差 = 360 / 每帧Block数

• 解析得到FOV的起始角度fov_start_angle和终止角度fov_end_angle，计算FOV的大小fov_range。

• 计算与FOV互补的盲区大小。按照盲区范围比例，计算盲区的时间戳差，也就是fov_blind_ts_diff_。

• 如果用户设置从DIFOP Packet读入角度修正值(RSDecoderParam.config_from_file = false)，则调用ChanAngles::loadFromDifop()得到他们。

  • 一般角度修正值不改变，所以一旦解析成功（angles_ready_ = true），就没必要解析第二次。

4.8.3 DecoderRSBP

以RSBP举例说明机械式雷达的Decoder。

• RSBP的常量配置由成员函数getConstParam()生成。这个配置定义为静态本地变量。

class_decoder_rsbp

4.8.3.1 RSDecoderConstParam设置

常量参数	值	说明
MSOP_LEN	1248	MSOP Packet字节数
DIFOP_LEN	1248	DIFOP Packet字节数
MSOP_ID[]	{0x55, 0xAA, 0x05, 0x0A, 0x5A, 0xA5, 0x50, 0xA0}	MSOP Packet标志字节，长度为8
MSOP_ID_LEN	8	MSOP_LEN[]中标志字节的长度
DIFOP_ID[]	{0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55}	DIFOP Packet标志字节，长度为8
DIFOP_ID_LEN	8	DIFOP_LEN[]中的字节长度
BLOCK_ID[]	{0xFF, 0xEE}	block标志字节，长度为2
LASER_NUM	32	32通道
BLOCKS_PER_PKT	12	每Packet中12个Block
CHANNEL_PER_BLOCK	32	RSBP为32线雷达
DISTANCE_MIN	0.1	测距最小值，单位米
DISTANCE_MAX	100.0	测距最大值，单位米
DISTANCE_RES	0.005	Packet中distance的解析度，单位米
TEMPERATURE_RES	0.0625	Packet中的温度的解析度

4.8.3.2 RSDecoderMechConstParam设置

常量参数	值	说明
RX	0.01473	光心相对于物理中心的X坐标
RY	0.0085	光心相对于物理中心的Y坐标
RZ	0.09427	光心相对于物理中心的Z坐标
BLOCK_DURATION	55.52	Block的持续时间，单位纳秒
CHAN_TSS[]	-	从发射时间列表得到
CHAN_AZIS[]	-	从发射时间列表得到

4.8.3.2 DecoderRSBP::getEchoMode()

getEchoMode()解析回波模式。

4.8.3.3 DecoderRSBP::decodeDifopPkt()

decodeDifopPkt() 解析DIFOP Packet。

• 调用DecoderMech::decodeDifopCommon()解析DIFOP Packet，得到转速等信息。
• 调用getEchoMode()，解析RSDifopPkt::return_mode，得到回波模式
• 根据回波模式，设置成员成员split_blks_per_frame_。如前所说，如果当前以理论上的每圈Block数分帧，则需要使用这个成员。

4.8.3.4 DecoderRSBP::decodeMsopPkt()

decodeMsopPkt()使用不同的模板参数调用internDecodeMsopPkt()。

• 单回波模式下，模板参数是SingleReturnBlockDiff，
• 双回波模式下，模板参数是DualReturnBlockDiff。

4.8.3.5 DecoderRSBP::internDecodeMsopPkt()

• 调用parseTempInLe(), 得到雷达温度，保存到temperature_。

• 调用parseTimeYMD()得到Packet的时间戳，保存到本地变量pkt_ts。Block时间戳的初值设置为pkt_ts。

• 构造模板参数BlockDiff的实例。

• 遍历Packet内所有的Block。

  • 校验Block的标志字节
  • 得到Block的角度值。
  • 计算得到Block的时间戳，保存到本地变量block_ts。
  • 调用SplitStrategy::newBlock()，检查是否分帧。如果是，调用回调函数cb_split_frame_，通知使用者。
    cb_split_frame_应该转移点云point_cloud_，并重置它。

• 遍历Block内所有的Channel。

  • 计算Channel的时间戳
  • 计算Channel的水平角
  • 调用ChanAngles::vertAdjust()，得到Channel的垂直角。
  • 调用ChanAngles::horizAdjust()，对Channel的水平角进行修正。
  • 解析Channel的distance。
  • 调用DistanceSection::in()校验distance，调用AzimuthSection::in()校验水平角。

如果合法，

• 计算点云坐标(x, y, z)。
• 调用transfromPoint()做坐标转换。
• 设置点云的intensity、timestamp，ring。
• 将点保存到点云point_cloud_的尾部。

如果不合法，

• 将NAN点保存到点云point_cloud_尾部。

• 当前点的时间戳保存到成员prev_point_ts。如果下一个Block分包，那么这个时间戳就是点云的时间戳。

• 将当前Packet的时间戳保存到成员prev_pkt_ts。这样，Decoder的使用者不需要重新解析Packet来得到它。

4.8.4 DecoderRS16

RS16的处理与其他机械式雷达有差异，请先参考前面的“RS16的Packet格式”等章节。

4.8.4.1 DecoderRS16::internDecodeMsopPkt()

在internDecoderPkt()的处理中，

• 因为Block的通道值在[0，31]，需要将它映射到实际的通道值。

4.8.5 DecoderRSM1

RSM1是MEMS雷达。这里说明RSM1的Decoder。

• DecoderRSM1的常量配置由成员函数getConstParam()生成。这个配置定义为静态本地变量。
• 成员split_strategy_是SplitStrategyBy类的实例，保存分帧策略。

class_decoder_rsm1

4.8.5.1 RSDecoderConstParam设置

常量参数	值	说明
MSOP_LEN	1210	MSOP Packet字节数
DIFOP_LEN	256	DIFOP Packet字节数
MSOP_ID[]	{0x55, 0xAA, 0x5A, 0xA5}	MSOP Packet标志字节，长度为4
MSOP_ID_LEN	4	MSOP_LEN[]中标志字节的长度
DIFOP_ID[]	{0xA5, 0xFF, 0x00, 0x5A, 0x11, 0x11, 0x55, 0x55}	DIFOP Packet标志字节，长度为8
DIFOP_ID_LEN	8	DIFOP_LEN[]中的字节长度
BLOCK_ID[]	{0xFF, 0xEE}	block标志字节，长度为2
LASER_NUM	5	5通道
BLOCKS_PER_PKT	25	每Packet中25个Block
CHANNEL_PER_BLOCK	5	RSM1有5个通道
DISTANCE_MIN	0.2	测距最小值，单位米
DISTANCE_MAX	200.0	测距最大值，单位米
DISTANCE_RES	0.005	Packet中distance的解析度，单位米
TEMPERATURE_RES	80	雷达温度的初始值

4.8.5.2 DecoderRSM1::decodeDifopPkt()

decodeDifopPkt() 解析DIFOP Packet。

• 调用getEchoMode()，解析RSDifopPkt::return_mode，得到回波模式
• 根据回波模式，设置成员成员max_seq_。

4.8.5.3 DecodeRSM1::decodeMsopPkt()

decodeMsopPkt()解析MSOP Packet。

• 解析Packet中的temperature字段，得到雷达温度，保存到temperature_。

• 调用parseTimeUTCWithUs()得到Packet的时间戳，保存到本地变量pkt_ts。

• 调用SplitStrategyBySeq::newPacket()，检查是否分帧。如果是，调用回调函数cb_split_frame_，通知使用者。
  cb_split_frame_应该转移点云pont_cloud_，并重置它。

• 遍历Packet内所有的Block。

  • 从Block相对于Packet的偏移，得到Block的时间戳。对于RSM1, Block内的所有Channel的时间戳都是这个时间戳。

• 遍历Block内所有的Channel。

  • 解析Channel的distance。

  • 调用DistanceSection::in()校验distance。

    如果distance合法，

    • 计算点云坐标(x, y, z)。
    • 调用transfromPoint()做坐标转换。
    • 设置点云的intensity、timestamp，ring。
    • 将点保存到点云point_cloud_的尾部。

    如果distance不合法，

    • 将NAN点保存到点云point_cloud_尾部。

• 当前点的时间戳保存到成员prev_point_ts_。如果下一个Block分包，那么这个时间戳就是点云的时间戳。

• 将当前Packet的时间戳保存到成员prev_pkt_ts_。这样，Decoder的使用者不需要重新解析Packet来得到它。

4.8.6 DecoderFactory

DecoderFactory是创建Decoder实例的工厂。

class_decoder_factory

Decoder/雷达的类型如下。

num LidarType
{
  RS16 = 1,
  RS32,
  RSBP,
  RS128,
  RS80,
  RSHELIOS,
  RSROCK,
  RSM1 = 10
};

4.8.6.1 DecoderFactory::creatDecoder()

createDecoder() 根据指定的雷达类型，创建Decdoer实例。

4.9 LidarDriverImpl - 组合Input与Decoder

LidarDriverImpl组合Input部分和Decoder部分。

class_lidar_driver_impl

• 成员input_ptr_是Input实例。成员decoder_ptr_是Decoder实例。
  • LidarDriverImpl只有一个Input实例和一个Decoder实例，所以一个LidarDriverImpl实例只支持一个雷达。如果需要支持多个雷达，就需要分别创建多个LidarDriverImpl实例。
• 成员handle_thread_是MSOP/DIFOP Packet的处理线程。
• 成员driver_param_是RSDriverParam的实例。
  • RSDriverParam打包了RSInputParam和RSDecoderParam，它们分别是Input部分和Decoder部分的参数。

typedef struct RSDriverParam
{ 
  LidarType lidar_type = LidarType::RS16;  ///< Lidar type
  InputType input_type = InputType::ONLINE_LIDAR; ///< Input type
  RSInputParam input_param;
  RSDecoderParam decoder_param;
} RSDriverParam;

组合Input,

• 成员free_pkt_queue_、pkt_queue_分别保存空闲的Packet， 待处理的MSOP/DIFOP Packet。
  • 这2个队列是SyncQueue类的实例。SyncQueue提供多线程访问的互斥保护。
• 函数packetGet()和packetPut()用来向input_ptr_注册。input_ptr_调用前者得到空闲的Buffer，调用后者派发填充好Packet的Buffer。

组合Decoder,

• 成员cb_get_cloud_和cb_put_cloud_是回调函数，由驱动的使用者提供。它们的作用类似于Input类的cb_get_pkt_和cb_put_pkt_。驱动调用cb_get_cloud_得到空闲的点云，调用cb_put_cloud_派发填充好的点云。
  • 驱动的使用者调用成员函数regPointCloudCallback()，设置cb_get_cloud_和cb_put_cloud_。
• 成员函数splitFrame()用来向decoder_ptr_注册。decoder_ptr_在需要分帧时，调用split_Frame()。这样LidarDriverImpl可以调用cb_put_cloud_将点云传给使用者，同时调用cb_get_cloud_得到空闲的点云，用于下一帧的累积。

4.9.1 LidarDriverImpl::getPointCloud()

LidarriverImpl的成员cb_get_cloud_是rs_driver的使用者提供的。getPointCloud(对它加了一层包装，以便较验它是否合乎要求。
在循环中，

• 调用cb_get_cloud_，得到点云，
  如果点云有效，
• 将点云大小设置为0。
  如果点云无效，
• 调用runExceptionCallback()报告错误。

4.9.2 LidarDriverImpl::init()

init()初始化LidarDriverImpl实例。

初始化Decoder部分，

• 调用DecoderFactory::createDecoder()，创建Decoder实例。
• 调用getPointCloud()得到空闲的点云，设置decoder_ptr_的成员point_cloud_。
• 调用Decoder::regCallback()， 传递成员函数splitFrame()作为参数。这样Decoder分帧时，会调用splitFrame()通知。
• 调用Decoder::getPacketDuration()得到Decoder的Packet持续时间。

初始化Input部分，

• 调用InputFactory::createInput()，创建Input实例。
• 调用Input::regCallback()，传递成员函数packetGet()和packetPut()。这样Input可以得到Buffer， 和派发填充好Packet的Buffer。
• 调用Input::init()，初始化Input实例。

4.9.3 LidarDriverImpl::start()

start()开始处理MSOP/DIFOP Packet。

• 启动Packet处理线程handle_thread_， 线程函数为processPacket()。
• 调用Input::start()， 其中启动接收线程，接收MSOP/DIFOP Packet。

4.9.4 LidarDriverImpl::packetGet()

packetGet()分配空闲的Buffer。

• 优先从free_pkt_queue_队列得到可用的Buffer。
• 如果得不到，重新分配一个Buffer。

4.9.5 LidarDriverImpl::packetPut()

packetPut()将收到的Packet，放入队列pkt_queue_。

• 检查msop_pkt_queue_/difop_pkt_queue中的Packet数。如果处理线程太忙，不能及时处理， 则释放队列中所有Buffer。

4.9.6 LidarDriverImpl::processPacket()

processMsop()是MSOP Packet处理线程的函数。在循环中,

• 调用SyncQueue::popWait()获得Packet，
• 检查Packet的标志字节。
  • 如果是MSOP Packet，调用Decoder::processMsopPkt()，处理MSOP Packet。如果Packet触发了分帧，则Decoder会调用回调函数，也就是DriverImpl::splitFrame()。
  • 如果是DIFOP Packet， 调用Decoder::processDifopPkt()，处理Difop Packet。
• 将Packet的Buffer回收到free_pkt_queue_，等待下次使用。

4.9.7 LidarDriverImpl::splitFrame()

splitFrame()在Decoder通知分帧时，派发点云。

• 得到点云，也就是成员decoder_ptr的point_cloud_。
• 校验point_cloud_，
  如果点云有效，
• 调用setPointCloudHeader()设置点云的头部信息，
• 调用cb_put_pc_，将点云传给驱动的使用者。
• 调用getPointCloud()得到空闲点云，重新设置成员decoder_ptr的point_cloud_。

4.9.8 LidarDriverImpl::getTemperature()

getTemperature()调用Decoder::getTemperature()， 得到雷达温度。