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

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。

01_components.png

• 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

02_classes_input.png

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有自己的线程recv_thread_。

  • Input的派生类启动这个线程读取Packet。

03_class_input.png

3.2 InputSock

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

04_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值可以在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。

05_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的配置参数可以在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（如果有的话）。

3.3 InputPcap

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

06_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 字节。

07_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。

用户配置参数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实例的工厂。

08_class_input_factory.png

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章节中说明。