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。

• 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

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。

3.2 InputSock

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

• 一般情况下，雷达将 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。

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 文件中。

• 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 字节。

• 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 实例的工厂。

Input 类型如下。

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 保存这个解析度。

uint16_t distance;

4.1.2 角度

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

  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 个字节中。

uint8_t intensity;

4.1.4 ring

ring 在后面的 ChanAngles 章节说明。

4.1.5 timestamp

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

4.1.5.2 UTC 格式

UTC 格式定义如下：

• 成员 sec[6] 保存的是秒数；

• 成员 ss[4] 保存微秒值或纳秒值。

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 保存这个初始值。

int8_t temperature;

4.1.7 echo_mode

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

• 最强回波，

• 最后回波，

• 双回波，

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

int8_t return_mode;

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

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

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 值，并用于查询。

• 成员变量 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_ 了。

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_ 的一个安全区间。

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

safe_seq_min_ = prev_seq_ - RANGE
safe_seq_max_ = prev_seq_ + RANGE

• 如果 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 进一步限缩这个范围。

4.5.1.1 DistanceSection::in()

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

4.6 Decoder

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

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

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

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

4.6.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() 设置。

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 格式中，雷达温度值的解析度。

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 选项。

cmake -DENABLE_TRANSFORM .

4.6.2 DecoderA0

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

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

• 成员 split_strategy_ 是 SplitStrategyBy 类的实例，保存分帧策略。

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 雷达的类型如下。

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 部分的参数。

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;

组合 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 的各个模块中，所以这里单独分一个章节说明。

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 回放 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

• 电脑主机侧又会重新产生时间戳（即此时回放的时间戳）。

• 输出的点云使用这个时间戳。