# 1 ag_driver 源代码解析



## 1 基本概念

### 1.1 MEMS雷达

ag_driver 目前只支持一种雷达：

+ MEMS 雷达。如 A0。MEMS 雷达是单轴、谐振式的MEMS扫描镜，其水平扫描角度可达 120°。

### 1.2 通道 Channel

MEMS 雷达的每个通道对应一块区域，比如一个矩形区域。

### 1.3 MSOP/DIFOP

Asensing 雷达与电脑主机的通信协议有三种。
+ **MSOP**（Main data Stream Ouput Protocol）。激光雷达将扫描出来的距离、角度、反射率等信息封装成 MSOP Packet，输出给电脑主机。
+ **DIFOP**（Device Information Output Protocol）。激光雷达将自身的配置信息，以及当前的状态封装成 DIFOP Packet，输出给电脑主机。
+ **UCWP**（User Configuration Write Protocol）。用户可以修改激光雷达的某些配置参数。

ag_driver 处理前两类协议的包，也就是 MSOP Packet 和 DIFOP Packet。

一般来说，激光雷达与电脑主机通过以太网连接，使用 UDP 协议。MSOP/DIFOP 的格式，不同的雷达可能有较大差异。

### 1.4 点云帧

+ 对于 MEMS 雷达，点云在 MSOP Packet 序列中的开始和结束位置，由雷达自己确定。
+ 一帧点云包含固定数目（比如N）的 MSOP Packet。雷达对 MSOP Packet 从 1 到 N 编号，并一直循环。



## 2 ag_driver的组件

ag_driver 主要由三部分组成： Input、Decoder、LidarDriverImpl。

![components](./img/components.png)

+ Input 部分负责从 Socket 或 PCAP 文件等数据源，获取 MSOP/DIFOP Packet。Input 的类一般有自己的接收线程 `recv_thread_`。
+ Decoder 部分负责解析 MSOP/DIFOP Packet，得到点云。Decoder 部分没有自己的线程，它运行在 LidarDriverImpl 的 Packet 处理线程 `handle_thread_` 中。
+ LidarDrvierImpl 部分将 Input 和 Decoder 组合到一起。它从 Input 得到 Packet，根据 Packet 的类型将它派发到 Decoder。得到点云后，通过用户的回调函数传递给用户。
  + LidarDriverImpl 提供 Packet 队列。Input 收到 MSOP/DIFOP Packet 后，调用 LidarDriverImpl 的回调函数。回调函数将它保存到 Packet 队列。
  + LidarDriverImpl 提供 Packet 处理线程 `handle_thread_`。在这个线程中，将 MSOP Packet 和 DIFOP Packet 分别派发给 Decoder 相应的处理函数。
  + Decoder 解析完一帧点云时，通知 LidarDriverImpl。后者再将点云传递给用户。


## 3 Packet接收

Input 部分负责接收 MSOP/DIFOP Packet，包括：
+ Input，
+ Input 的派生类，如 InputSock、InputPcap、InputRaw
+ Input 的工厂类 InputFactory

![input classes](./img/classes_input.png)

### 3.1 Input

Input 定义接收 MSOP/DIFOP Packet 的接口。
+ 成员 `input_param_` 是用户配置参数 AGInputParam，其中包括从哪个 port 接收 Packet 等信息。
+ Input 自己不分配接收 Packet 的缓存。
  + Input 的使用者调用 Input::regCallback()，提供两个回调函数 cb_get_pkt 和 cb_put_pkt， 它们分别保存在成员变量 `cb_get_pkt_` 和 `cb_put_pkt_` 中。
  + Input 的派生类调用 `cb_get_pkt_` 可以得到空闲的缓存；在缓存中填充好 Packet 后，可以调用 `cb_put_pkt_` 将它返回。

+ Input 有自己的线程 `recv_thread_`。
  + Input 的派生类启动这个线程读取 Packet。

![Input](./img/class_input.png)

### 3.2 InputSock

InputSock 类从 UDP Socket 接收 MSOP/DIFOP Packet。雷达将 MSOP/DIFOP Packet 发送到这个 Socket。

![InputSock](./img/class_input_sock.png)

+ 一般情况下，雷达将 MSOP/DIFOP Packet 发送到不同的目的 Port，所以 InputSock 创建两个 Socket 来分别接收它们。
  + 成员变量 `fds_[2]` 保存这两个 Socket 的描述符。`fds_[0]` 是 MSOP socket, `fds_[1]` 是 DIFOP socket。但也可以配置雷达将 MSOP/DIFOP Packet 发到同一个 Port，这时一个 Socket 就够了，`fds_[1]` 就是为无效值 `-1`。
  + MSOP/DIFOP 对应的 Port 值可以在 AGInputParam 中设置，分别对应于 `AGInputParam::msop_port` 和 `AGInputParam::difop_port`。
+ 一般情况下，MSOP/DIFOP Packet 直接构建在 UDP 协议上。但在某些客户的场景下(如车联网)，MSOP/DIFOP Packet 可能构建在客户的协议上，客户协议再构建在 UDP 协议上。这时，InputSock 派发 MSOP/DIFOP Packet 之前，会先丢弃 `USER_LAYER` 的部分。成员变量 `sock_offset_` 保存了 `USER_LAYER` 部分的字节数。
  + `USER_LAYER` 部分的字节数可以在 AGInputParam 中设置，对应于 `AGInputParam::user_layer_bytes`。
+ 有的场景下，客户的协议会在 MSOP/DIFOP Packet 尾部附加额外的字节。这时，InputSock 派发 MSOP/DIFOP Packet 之前，会先丢弃 `TAIL_LAYER` 的部分。成员变量 `sock_tail_` 保存了 `TAIL_LAYER` 部分的字节数。
  + `TAIL_LAYER` 部分的字节数可以在 AGInputParam 中设置，对应于 `AGInputParam::tail_layer_bytes`。

![layers of packet](./img/packet_layers.png)

#### 3.2.1 InputSock::createSocket()

createSocket() 用于创建 UDP Socket。

+ 调用 setsockopt(), 设置选项 `SO_REUSEADDR`；
+ 调用 bind() 将 socket 绑定到指定的 (IP, PORT) 组上；
+ 如果雷达是组播模式，则将指定 IP 加入该组播组；
+ 调用 fcntl() 设置 `O_NONBLOCK` 选项，以异步模式接收 MSOP/DIFOP Packet。

该 Socket 的配置参数可以在 AGInputParam 中设置。根据设置的不同，createSocket() 支持如下几种模式。

| msop_port/difop_port       |    host_address            |  group_address          |               |
|:-----------:|:----------------------:|:-----------------:|:-------------|
| 51180/7788       |    0.0.0.0            | 0.0.0.0          | 雷达的目的地址可以为广播地址、或电脑主机地址 |
| 51180/7788       |    192.168.101.101     | 0.0.0.0          | 雷达的目的地址可以为电脑主机地址 |
| 51180/7788       |    192.168.101.101     | 224.1.1.1      | 雷达的目的地址可以为组播地址、或电脑主机地址   |

#### 3.2.2 InputSock::init()

init() 调用 createSocket()，创建两个 Socket，分别接收 MSOP Packet 和 DIFOP Packet。

#### 3.2.3 InputSock::start()

start() 开始接收 MSOP/DIFOP Packet。

+ 启动接收线程，线程函数为 InputSock::recvPacket()。

#### 3.2.4 InputSock::recvPacket()

recvPacket() 接收 MSOP/DIFOP Packet。

在 while() 循环中，调用 FD_ZERO() 初始化本地变量 `rfds`，调用 FD_SET() 将 `fds_[2]` 中的两个 fd 加入 `rfds`。当然，如果 MSOP/DIFOP Packet 共用一个 socket， 无效的 `fds_[1]` 就不必加入了。

调用 select() 在 `rfds` 上等待 Packet, 超时值设置为 1 秒。如果 select() 的返回值提示 `rfds` 上有信号，调用 FD_ISSET() 检查是 `fds_[]` 中的哪一个 fd 可读。对这个 fd，调用回调函数 `cb_get_pkt_`， 得到大小为 `MAX_PKT_LEN` 的缓存，`MAX_PKT_LEN = 1500`，对当前 Asensing 雷达来说，够大了；调用 recvfrom() 接收 Packet，保存到这个缓存中；调用回调函数 `cb_put_pkt_`，将 Packet 派发给 InputSock 的使用者。

注意在派发之前，调用 Buffer::setData() 设置了 MSOP Packet 在 Buffer 的中偏移量及长度，以便剥除 `USER_LAYER` 和 `TAIL_LAYER`（如果有的话）。

### 3.3 InputPcap

InputPcap 解析 PCAP 文件得到 MSOP/DIFOP Packet。使用第三方工具，如 WireShark，可以将雷达数据保存到 PCAP 文件中。

![InputSock](./img/class_input_pcap.png)

+ InputPcap 基于第三方的 libpcap 库，使用它可以遍历 PCAP 文件，依次得到所有 UDP Packet。
  + 成员变量 `pcap_` 变量保存 Pcap 文件指针，`pcap_t` 定义来自 libpcap 库。

+ 与 InputSock 一样，在有的客户场景下，InputPcap 也需要处理 `USER_LAYER` 和 `TAIL_LAYER` 的情况。InputPcap 的成员 `pcap_offset_` 和 `pcap_tail_` 分别保存 `USER_LAYER` 和 `TAIL_LAYER` 的字节数。
+ 但也有不同的地方。InputSock 从 Socket 接收的 Packet 只有 UDP 数据部分，而 InputPcap 从 PCAP 文件得到的 Packet 不同，它包括所有 Packet 的所有层。`pcap_offset_` 除了 `USER_LAYER` 的长度之外，还要加上其他所有层。
  + 对于一般的以太网包，`pcap_offset_` 需要加上其他层的长度，也就是 14 (ETHERNET) + 20 (IP) + 8 (UDP) = 42 字节。
  + 如果还有 VLAN 层，`pcap_offset_` 还需要加上 4 字节。

![layers of packet](./img/packet_layers_full.png)

+ PCAP 文件中可能不止包括 MSOP/DIFOP Packet，所以需要使用 libpcap 库的过滤功能。libpcap 过滤器 `bpf_program`，由库函数 pcap_compile() 生成。成员 `msop_filter_` 和 `difop_filter_` 分别是 MSOP Packet 和 DIFOP Packet 的过滤器。
  + MSOP/DIFOP Packet 都是 UDP Packet，所以给 pcap_compile() 指定选项 `udp`。
  + 如果是基于 VLAN 的，则需要指定选项 `vlan`。
  + 如果在一个 PCAP 文件中包含多个雷达的 Packet，则还需要指定选项 `udp dst port`，以便只提取其中一个雷达的 Packet。
  

用户配置参数 AGInputParam 中指定选项 `udp dst port`。有如下几种情况。

| msop_port  |  difop_port  |     说明      |
|:-----------:|:-------------:|:-------------|
| 0          |    0         | 如果 PCAP 文件中只包含一个雷达的 Packet |
| 51180       |    7788      | 如果 PCAP 文件中包含多个雷达的 Packet，则可以只提取指定雷达的Packet（该雷达 MSOP/DIFOP 端口不同） |
| 51180       |    51180/0    | 如果 PCAP 文件中包含多个雷达的 Packet，则可以只提取指定雷达的 Packet（该雷达 DIFOP/DIFOP 端口相同）       |

#### 3.3.1 InputPcap::init()

init() 打开 PCAP 文件，构造 PCAP 过滤器。

+ 调用 pcap_open_offline() 打开 PCAP 文件，保存在成员变量 `pcap_` 中。
+ 调用 pcap_compile() 构造 MSOP/DIFOP Packet 的 PCAP 过滤器。
  + 如果它们使用不同端口，则需要两个过滤器，分别保存在 `mosp_filter_` 和 `difop_filter_` 中。
  + 如果使用同一端口，那么 `difop_filter_` 就不需要了。

#### 3.3.2 InputPcap::start()

start() 开始解析 PCAP 文件。

+ 调用 std::thread()，创建并启动 PCAP 解析线程，线程的函数为 recvPacket()。

#### 3.3.3 InputPcap::recvPacket()

recvPacket() 解析 PCAP 文件。

在循环中，

+ 调用 pcap_next_ex() 得到文件中的下一个 Packet。

如果 pcap_next_ex() 还能读出 Packet，

+ 本地变量 `header` 指向 Packet 的头信息，变量 `pkt_data` 指向 Packet 的数据。
+ 调用 pcap_offline_filter()，使用 PCAP 过滤器校验 Packet（检查端口、协议等是否匹配）。

如果是 MSOP Packet,

  + 调用 `cb_get_pkt_` 得到大小为 `MAX_PKT_LEN` 的缓存。`MAX_PKT_LEN = 1500`，对当前的 Asensing 雷达来说，够大了。
  + 调用 memcpy()将 Packet 数据复制到缓存中，并调用 Buffer::setData() 设置 Packet 的长度。复制时剥除了不需要的层，包括 `USER_LAYER` 和 `TAIL_LAYER`（如果有的话）。
  + 调用回调函数 `cb_put_pkt_`，将 Packet 派发给 InputSock 的使用者。

如果是 DIFOP Packet，处理与 MSOP Packet 一样。

+ 调用 this_thread::sleep_for() 让解析线程睡眠一小会。这是为了模拟雷达发送 MSOP Packet 的间隔。这个间隔时间来自每个雷达的 `Decoder` 类，每个雷达有自己的值。在 Decoder 部分，会说明如何计算这个值。

如果 pcap_next_ex() 不能读出 Packet，一般意味着到了文件结尾，则：
+ 调用 pcap_close() 关闭 pcap 文件指针 `pcap_` 。

用户配置 AGInputParam 的设置决定是否重新进行下一轮的解析。这个选项是 `AGInputParam::pcap_repeat`。
+ 如果这个选项为真，调用 pcap_open_offline() 重新打开 PCAP 文件。这时成员变量 `pcap_` 回到文件的开始位置。下一次调用 pcap_next_ex()，又可以重新得到 PCAP 文件的第一个 Packet 了。

### 3.4 InputRaw

InputRaw 是为了重播 MSOP/DIFOP Packet 而设计的 Input 类型。将在后面的 Packet Record/Replay 章节中说明。

### 3.5 InputFactory

InputFactory 是创建 Input 实例的工厂。

![InputFactory](./img/class_input_factory.png)

Input 类型如下。

```cpp
enum InputType
{
  ONLINE_LIDAR = 1, // InputSock
  PCAP_FILE,        // InputPcap
  RAW_PACKET        // InputRaw
};
```

#### 3.5.1 InputFactory::creatInput()

createInput() 根据指定的类型，创建 Input 实例。

+ 创建 InputPcap 时，需指定 `sec_to_delay`。这是 InputPcap 回放 MSOP Packet 的间隔。
+ 创建 InputRaw 时，需指定 `cb_feed_pkt`。这个将在后面的 Packet Record/Replay 章节中说明。



## 4 Packet解析

### 4.1 MSOP格式

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

+ 距离 `distance`
+ 角度 `azimuth`, `elevation`
+ 发射率 `intensity`
+ 通道 `ring`
+ 时间戳 `timestamp`
+ 温度 `temperature`
+ 回波模式 `echo_mode`

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

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

#### 4.1.1 Distance

Distance 用两个字节表示。它乘以一个解析度得到真实距离。
+ 不同的雷达的解析度可能不同。
+ 特定于雷达的配置参数 `AGDecoderConstParam::DISTANCE_RES` 保存这个解析度。

```cpp
uint16_t distance;
```
#### 4.1.2 角度

+ 对于 MEMS 雷达， 角度是 `azimuth` 和 `elevation`。

  ```cpp
  uint16_t azimuth;
  uint16_t elevation;
  ```
  从 `distance`、`azimuth` 和 `elevation`，可计算直角坐标系的坐标。

+ 雷达的角度分辨率是 0.01 度。这意味着一圈 360 度，可以划分成 36000 个单位。
+ MSOP 格式中，角度以 0.01 度为单位，范围是 (0, 36000)，所以可以用 `uint16_t` 来表示。

#### 4.1.3 intensity

`intensity` 保存在 1 个字节中。

```cpp
uint8_t intensity;
```

#### 4.1.4 ring

`ring` 在后面的 ChanAngles 章节说明。

#### 4.1.5 timestamp

Asensing 雷达使用了一种时间戳格式。

##### 4.1.5.2 UTC 格式 

UTC 格式定义如下：

+ 成员 `sec[6]` 保存的是秒数；
+ 成员 `ss[4]` 保存微秒值或纳秒值。

```cpp
typedef struct 
{
  uint8_t sec[6]; //year
                  //month
                  //day
                  //hour
                  //minute
                  //second
  uint8_t ss[4];  //identify the timestamp for point cloud
} AGTimestampUTC;
```
+ 如果 `ss[4]` 保存微秒值，使用 `parseTimeUTCWithUs()` 解析。（A0 使用 `parseTimeWithA0()`）。

当前版本的 ag_driver 只支持微秒格式的解析。

#### 4.1.6 temperature

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

##### 4.1.6.1 用解析度表示温度

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

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


##### 4.1.6.2 相对温度

另一类用 1 个字节表示温度。这个值加上一个初始值得到真实温度。遵循这种格式的有 A0。
+ 特定于雷达的配置参数 `AGDecoderConstParam::TEMPERATURE_RES` 保存这个初始值。

```cpp
int8_t temperature;
```

#### 4.1.7 echo_mode

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

+ 最强回波，
+ 最后回波，
+ 双回波，

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

```cpp
int8_t return_mode;
```

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

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

```cpp
enum AGEchoMode
{
  ECHO_SINGLE = 0,
  ECHO_DUAL
};
```

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

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

### 4.2 ChanAngles

#### 4.2.1 垂直角/水平角的修正

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

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

MEMS 雷达的角度修正，在雷达内部完成，所以 ag_driver 不需要做什么。


### 4.3 Trigon

#### 4.3.1 查表计算三角函数值

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

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

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

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

#### 4.3.2 Trigon定义

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

![Trigon](./img/class_trigon.png)

+ 成员变量 `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](./img/trigon_sinss.png)

#### 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 分帧

#### 4.4.1 MEMS 雷达的分帧模式

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

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

#### 4.4.2 SplitStrategyBySeq

SplitStrategyBySeq 按 Packet 编号分帧。
+ 成员变量 `prev_seq_` 是前一个 Packet 的编号。
+ 成员变量 `safe_seq_min_` 和 `safe_seq_max`，是基于 `prev_seq_` 的一个安全区间。

![split strategy by seq](./img/classes_split_strategy_by_seq.png)

##### 4.4.2.1 SplitStrategyBySeq::newPacket()

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

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

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

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

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

+ 以 `prev_seq_` 为参考点，划定一个范围值 `RANGE`

```cpp
safe_seq_min_ = prev_seq_ - RANGE
safe_seq_max_ = prev_seq_ + RANGE
```

![safe range](./img/safe_range.png)

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


### 4.5 点云的有效性校验

#### 4.5.1 DistanceSection

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

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

![distance section](./img/class_distance_section.png)

##### 4.5.1.1 DistanceSection::in()

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

### 4.6 Decoder

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

+ 它是针对所有雷达的接口类。

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

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

![decoder classes](./img/classes_decoder.png)

#### 4.6.1 Decoder定义

如下图是 Decoder 的详细定义。

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

```cpp
#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() 设置。

![decoder class](./img/class_decoder.png)

##### 4.6.1.1 AGDecoderConstParam

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

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

```cpp
struct AGDecoderConstParam
{
  // 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];

  // 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.6.1.2 Decoder::processDifopPkt()

processDifopPkt() 处理 DIFOP Packet。

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

##### 4.6.1.3 Decoder::processMsopPkt()

processMsopPkt() 处理 MSOP Packet。

+ 检查当前配置信息是否已经就绪（`angles_ready_`）。
  + 通过用户配置参数 `AGDecoderParam::wait_for_difop`，可以设置是否等待配置信息就绪。
+ 校验 DIFOP Packet 的长度是否匹配。
+ 校验 DIFOP Packet 的标志字节是否匹配。
+ 如果以上校验通过，调用 decodeMsopPkt()。这是一个纯虚拟函数，由各雷达的派生类提供自己的实现。

##### 4.6.1.4 Decoder::transformPoint()

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

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

```bash
cmake -DENABLE_TRANSFORM .
```


#### 4.6.2 DecoderA0

A0 是一款 MEMS 激光雷达，下面介绍 A0 的 Decoder。

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

![decoder rsm1](./img/class_decoder_A0.png)

##### 4.6.2.1 AGDecoderConstParam 设置

| 常量参数      |  值  |     说明      |
|:-------------:|:---------:|:-------------|
| MSOP_LEN     |    1172  | MSOP Packet字节数              |
| DIFOP_LEN    |    108   | DIFOP Packet字节数  |
| MSOP_ID[]    |    {0xAA, 0x55, 0xA5, 0x5A}   | MSOP Packet标志字节，长度为4 |
| MSOP_ID_LEN  |    4     | MSOP_LEN[]中标志字节的长度 |
| DIFOP_ID[]   |    {0xA5, 0xFF, 0x00, 0x5A}   | DIFOP Packet标志字节，长度为4 |
| DIFOP_ID_LEN |    4     | DIFOP_LEN[]中的字节长度 |
| LASER_NUM    |    10  | 10通道(5个模组*2个通道) |
| BLOCKS_PER_PKT |    12  | 每Packet中12个Block |
| CHANNEL_PER_BLOCK | 10  | A0有10个通道 |
| DISTANCE_MIN |    0.2   | 测距最小值，单位米 |
| DISTANCE_MAX |  200.0   | 测距最大值，单位米 |
| DISTANCE_RES |  0.005   | Packet中distance的解析度，单位米 |
| TEMPERATURE_RES| 80 | 雷达温度的初始值 |

##### 4.6.2.2 DecoderA0::decodeDifopPkt()

decodeDifopPkt() 解析 DIFOP Packet。

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

##### 4.6.2.3 DecodeA0::decodeMsopPkt()

decodeMsopPkt() 解析 MSOP Packet。

+ 解析 Packet 中的 `temperature` 字段，得到雷达温度，保存到 `temperature_`。
+ 调用 parseTimeWithA0() 得到 Packet 的时间戳，保存到本地变量 `pkt_ts`（因为 A0 直接在下位机处理，把 tm 结构体拆开发过来，因此不使用 parseTimeUTCWithUs获取）。
+ 调用 SplitStrategyBySeq::newPacket()，检查是否分帧。如果是，调用回调函数 `cb_split_frame_`，通知使用者。
  `cb_split_frame_` 应该转移点云 `pont_cloud_`，并重置它。

+ 遍历 Packet 内所有的 Block。
  
  + 从 Block 相对于 Packet 的偏移，得到 Block 的时间戳。对于 A0, Block 内的所有 Channel 的时间戳都是这个时间戳。
  
+ 遍历 Block 内所有的 Channel。
  + 解析 Channel 的 distance。
  + 调用 DistanceSection::in() 校验 `distance`。

    如果 distance 合法，
     + 计算点云坐标 (`x`, `y`, `z`)。
     + 调用 transfromPoint() 做坐标转换。
     + 设置点云的 `intensity`、`timestamp`、`ring`、`range`。
     + 将点保存到点云 `point_cloud_` 的尾部。

    如果 `distance` 不合法，
     + 将 `NAN` 点保存到点云 `point_cloud_` 尾部。

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

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

#### 4.6.3 DecoderFactory

DecoderFactory 是创建 Decoder 实例的工厂。

![decoder factory](./img/class_decoder_factory.png)

Decoder 雷达的类型如下。

```cpp
num LidarType
{
  A0 = 1
};
```

##### 4.6.3.1 DecoderFactory::creatDecoder()

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

### 4.7 LidarDriverImpl - 组合 Input 与 Decoder

LidarDriverImpl 组合 Input 部分和 Decoder 部分。

+ 成员 `input_ptr_` 是 Input 实例。成员 `decoder_ptr_` 是 Decoder 实例。
  + LidarDriverImpl 只有一个 Input 实例和一个 Decoder 实例，所以一个 LidarDriverImpl 实例只支持一个雷达。如果需要支持多个雷达，就需要分别创建多个 LidarDriverImpl 实例。
+ 成员 `handle_thread_` 是 MSOP/DIFOP Packet 的处理线程。
+ 成员 `driver_param_` 是 AGDriverParam 的实例。
  + AGDriverParam 打包了 AGInputParam 和 AGDecoderParam，它们分别是 Input 部分和 Decoder 部分的参数。
  
```cpp
typedef struct AGDriverParam
{ 
  LidarType lidar_type = LidarType::A0;  ///< Lidar type
  InputType input_type = InputType::ONLINE_LIDAR; ///< Input type
  AGInputParam input_param;
  AGDecoderParam decoder_param;
} AGDriverParam;
```

![lidar driver impl](./img/class_lidar_driver_impl.png)

组合 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.7.1 LidarDriverImpl::getPointCloud()

LidarDriverImpl 的成员 `cb_get_cloud_` 是 ag_driver 的使用者提供的。getPointCloud() 对它加了一层包装，以便较验它是否合乎要求。

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

#### 4.7.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.7.3 LidarDriverImpl::start()

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

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

#### 4.7.4 LidarDriverImpl::packetGet()

packetGet() 分配空闲的 Buffer。

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

#### 4.7.5 LidarDriverImpl::packetPut()

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

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

#### 4.7.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.7.7 LidarDriverImpl::splitFrame()

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

+ 得到点云，也就是成员 `decoder_ptr` 的 `point_cloud_`。
+ 校验 `point_cloud_`，

如果点云有效，

+ 调用 setPointCloudHeader() 设置点云的头部信息，
+ 调用 `cb_put_pc_`，将点云传给驱动的使用者。
+ 调用 getPointCloud() 得到空闲点云，重新设置成员 `decoder_ptr` 的 `point_cloud_`。

#### 4.7.8 LidarDriverImpl::getTemperature()

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


## 5 Packet 的录制与回放

使用者调试自己的程序时，点云的录制与回放是有用的，只是点云占的空间比较大。MSOP/DIFOP Packet 占的空间较小，所以 Packet 的录制与回放是更好的选择。

与 MSOP/DIFP Packet 的录制与回放相关的逻辑，散布在 ag_driver 的各个模块中，所以这里单独分一个章节说明。

![packet record and replay](./img/packet_record_replay.png)

### 5.1 录制

#### 5.1.1 LidarDriverImpl::regPacketCallback()

通过 regPacketCallback()，ag_driver 的使用者注册一个回调函数，得到原始的 MSOP/DIFOP Packet。
+ 回调函数保存在成员 `cb_put_pkt_`。

#### 5.1.2 LidarDriverImpl::processMsopPkt()

在 processMsopPkt() 中，

+ 调用 Decoder::processMsopPkt()，
+ 调用 `cb_put_pkt_`，将 MSOP Packet 传给调用者。
  + 设置 Packet 的时间戳。这个时间戳调用 Decoder::prevPktTs() 得到。
  + 设置这个 Packet 是否触发分帧。这个标志是 Decoder::processMsopPkt() 的返回值。

#### 5.1.3 LidarDriverImpl::processDifopPkt()

在 processDifopPkt() 中，

+ 调用 Decoder::processDifopPkt()，
+ 调用 `cb_put_pkt_`，将 MSOP Packet 传给调用者。DIFOP Packet 的时间戳不重要。

### 5.2 回放

#### 5.2.1 InputRaw

![InputRaw](./img/class_input_raw.png)

InputRaw 回放 MOSP/DIFOP Packet。

+ 使用者从某种数据源（比如 rosbag 文件）中解析 MSOP/DIFOP Packet，调用 InputRaw 的成员函数 feedPacket()，将 Packet 喂给它。
  + 在 feedPacket() 中，InputRaw 简单地调用成员 `cb_put_pkt_`，将 Packet 推送给调用者。这样，它的后端处理就与 InputSock/InputPcap 一样了。

#### 5.2.2 LidarDriverImpl

+ InputRaw::feedBack() 在 InputFactory::createInput() 中被打包，最后保存在 LidarDriverImpl 类的成员 `cb_feed_pkt_` 中。
+ 使用者调用 LidarDriverImpl 的成员函数 decodePacket()，可以将 Packet 喂给它。decodePacket() 简单地调用成员 `cb_feed_pkt_`。

### 5.3 时间戳处理

点云的时间戳来自于 MSOP Packet 的时间戳。MSOP Packet 的时间戳可以有两种产生方式。
+ 用户配置参数 `AGDecoderParam::use_lidar_clock` 决定使用哪种方式。
+ `use_lidar_clock = true` 时， 使用雷达产生的时间戳，这个时间戳在 Packet 中保存。这种情况下，一般已经使用时间同步协议对雷达做时间同步。
+ `use_lidar_clock = false` 时， 忽略 Packet 中的时间戳，在电脑主机侧由 ag_driver 重新产生一个时间戳。此时，如果 `write_pkt_ts = true`，那么就会将 packets 中的时间戳重写为电脑主机侧新产生的时间戳。

#### 5.3.1 使用雷达时间戳

录制时，设置 `use_lidar_clock = true`

+ 解析 MSOP Packet 的时间戳。这个时间戳是雷达产生的。
+ 输出的点云使用这个时间戳。
+ 如果输出 Packet，也是这个时间戳。

回放时，设置`use_lidar_clock = true`

+ MSOP Packet 内保存的仍然是雷达产生的时间戳。
+ 输出点云仍然使用这个时间戳。

#### 5.3.2 使用主机时间戳

录制时，设置 `use_lidar_clock = false`

+ ag_driver 在电脑主机侧重新产生时间戳。如果这时有点云输出，使用 ag_driver 产生的时间戳。
  + 在 DecoderA0::internDecodeMsopPacket() 中，ag_driver 调用 getTimeHost() 产生时间戳，然后调用 createTime()，用这个新时间戳。（Packet中原有时间戳没变，只是不使用）
+ 这时输出的 Packet 的时间戳是 ag_driver 产生的时间戳

回放时，设置 `use_lidar_clock = true`

+ 解析 MSOP Packet 的时间戳。这个时间戳是录制时 ag_driver 在电脑主机侧产生的。
+ 输出的点云使用这个时间戳。

回放时，设置 `use_lidar_clock = false`

+ 电脑主机侧又会重新产生时间戳(即此时回放的时间戳)。
+ 输出的点云使用这个时间戳。

#### 5.3.3 使用录制 rosbag 时，用当时主机侧重写 packets 的时间戳（即当时主机侧的时间戳）

录制时，设置 `use_lidar_clock = false` 且 `write_pkt_ts = true`

+ ag_driver 在电脑主机侧重新产生时间戳。这个时间戳覆盖 Packet 文件中原有的时间戳；如果这时有点云输出，使用 ag_driver 产生的时间戳
  + 在 DecoderA0::internDecodeMsopPacket() 中，ag_driver 调用 getTimeHost() 产生时间戳，然后调用 createTime()，用这个新时间戳替换 Packet 中原有的时间戳。（Packet 中时间戳被替换）
+ 这时输出的 Packet 的时间戳是 ag_driver 产生的时间戳。


回放时，设置 `use_lidar_clock = true`

+ 解析MSOP Packet的时间戳。这个时间戳是录制时，当时主机侧 重写packets 的时间戳（即当时主机侧的时间戳）
+ 输出的点云使用这个时间戳。

回放时，设置 `use_lidar_clock = false`

+ 电脑主机侧又会重新产生时间戳（即此时回放的时间戳）。
+ 输出的点云使用这个时间戳。