rs_driver 是RoboSense雷达的基本驱动程序。本文是rs_driver的源代码解析文档，原文地址在：
https://github.com/RoboSense-LiDAR/rs_driver/blob/v1.5.7/doc/src_intro/rs_driver_intro_CN.md
1 基本概念
1.1 机械式雷达、MEMS雷达
rs_driver支持RoboSense的两种雷达：
- 机械式雷达。如RS16/RS32/RSBP/RSHELIOS/RS80/RS128。机械式雷达有控制激光发射角度的旋转部件，有360°扫描视场。
- MEMS雷达。如RSM1。MEMS雷达是单轴、谐振式的MEMS扫描镜，其水平扫描角度可达120°。
1.2 通道 Channel
对于机械式雷达，通道指的是垂直方向上扫描的点数，每个通道上的点连成一条线。比如，RS16是16线雷达，也就是16个通道； RSBP是32线雷达，RS128是128线雷达。
MEMS雷达的通道与机械式雷达不同，它的每个通道可能对应一块区域，比如一个矩形区域。
1.3 MSOP/DIFOP
RoboSense雷达与电脑主机的通信协议有三种。
- MSOP (Main data Stream Ouput Protocol)。 激光雷达将扫描出来的距离、角度、反射率等信息封装成MSOP Packet，输出给电脑主机。
- DIFOP (Device Information Output Protocol)。激光雷达将自身的配置信息，以及当前的状态封装成DIFOP Packet，输出给电脑主机。
- UCWP (User Configuration Write Protocol)。用户可以修改激光雷达的某些配置参数。
rs_driver处理前两类协议的包，也就是MSOP Packet和DIFOP Packet。
一般来说，激光雷达与电脑主机通过以太网连接，使用UDP协议。MSOP/DIFOP的格式，不同的雷达可能有较大差异。
1.4 点云帧
-
机械式雷达持续旋转，输出点。扫描一圈360°得到的所有点，构成一帧点云。
- 使用者可以指定一个角度，rs_driver按照这个角度，分割MSOP Pacekt序列得到点云。
-
对于MEMS雷达，点云在MSOP Packet序列中的开始和结束位置，由雷达自己确定。
- 一帧点云包含固定数目（比如N）的MSOP Packet。雷达对MSOP Packet从 1 到 N 编号，并一直循环。
2 rs_driver的组件
rs_driver主要由三部分组成： Input、Decoder、LidarDriverImpl。
- Input部分负责从Socket/PCAP文件等数据源，获取MSOP/DIFOP Packet。Input的类一般有自己的接收线程
recv_thread_。 - Decoder部分负责解析MSOP/DIFOP Packet，得到点云。Decoder部分没有自己的线程，它运行在LiarDriverImpl的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_是用户配置参数RSInputParam，其中包括从哪个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的使用者调用Input::regCallback()，提供两个回调函数cb_get_pkt和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值可以在RSInputParam中设置，分别对应于
RSInputParam::msop_port和RSInputParam::difop_port。
- 成员变量
- 一般情况下，MSOP/DIFOP Packet直接构建在UDP协议上。但在某些客户的场景下(如车联网)，MSOP/DIFOP Packet可能构建在客户的协议上，客户协议再构建在UDP协议上。这时，InputSock派发MSOP/DIFOP Packet之前，会先丢弃
USER_LAYER的部分。成员变量sock_offset_保存了USER_LAYER部分的字节数。-
USER_LAYER部分的字节数可以在RSInputParam中设置，对应于RSInputParam::user_layer_bytes。
-
- 有的场景下，客户的协议会在MSOP/DIFOP Packet尾部附加额外的字节。这时，InputSock派发MSOP/DIFOP Packet之前，会先丢弃
TAIL_LAYER的部分。成员变量sock_tail_保存了TAIL_LAYER部分的字节数。-
TAIL_LAYER部分的字节数可以在RSInputParam中设置，对应于RSInputParam::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的配置参数可以在RSInputParam中设置。根据设置的不同，createSocket()支持如下几种模式。
| msop_port/difop_port | host_address | group_address | |
|---|---|---|---|
| 6699/7788 | 0.0.0.0 | 0.0.0.0 | 雷达的目的地址可以为广播地址、或电脑主机地址 |
| 6699/7788 | 192.168.1.201 | 0.0.0.0 | 雷达的目的地址可以为电脑主机地址 |
| 6699/7788 | 192.168.1.201 | 239.255.0.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，对当前RoboSense雷达来说，够大了。 - 调用recvfrom()接收Packet，保存到这个缓存中
- 调用回调函数
cb_put_pkt_，将Packet派发给InputSock的使用者。- 注意在派发之前，调用Buffer::setData()设置了MSOP Packet在Buffer的中偏移量及长度，以便剥除
USER_LAYER和TAIL_LAYER（如果有的话）。
- 注意在派发之前，调用Buffer::setData()设置了MSOP Packet在Buffer的中偏移量及长度，以便剥除
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。
- MSOP/DIFOP Packet都是UDP Packet，所以给pcap_compile()指定选项
用户配置参数RSInputParam中指定选项udp dst port。有如下几种情况。
| msop_port | difop_port | 说明 |
|---|---|---|
| 0 | 0 | 如果PCAP文件中只包含一个雷达的Packet |
| 6699 | 7788 | 如果PCAP文件中包含多个雷达的Packet，则可以只提取指定雷达的Packet（该雷达MSOP/DIFOP端口不同） |
| 6699 | 6699/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，对当前的RoboSense雷达来说，够大了。 - 调用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_。
用户配置RSInputParam的设置决定是否重新进行下一轮的解析。这个选项是RSInputParam::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章节中说明。