# 1 aglidar_sdk 源代码解析

## 1 简介

aglidar_sdk 是基于 ROS/ROS2 的雷达驱动。aglidar_sdk 依赖 ag_driver 接收和解析 MSOP/DIFOP Packet。

aglidar_sdk 的基本功能如下：
+ 从在线雷达或 PCAP 文件得到点云，通过 ROS 主题 `/aglidar_points` 发布。使用者可以订阅这个主题，在 rviz 中看到点云。
+ 从在线雷达得到原始的 MSOP/DIFOP Packet，通过 ROS 主题 `/aglidar_packets` 发布。使用者可以订阅这个主题，将 Packet 记录到 rosbag 文件。
+ 从 ROS 主题 `/aglidar_packets` 得到 MSOP/DIFOP Packet，解析得到点云，再发布到主题 `/aglidar_points`。
  + 这里的主题 `/aglidar_packets`，由使用者通过回放 Packet rosbag 文件发布。

## 2 Source 与 Destination

如前面所说，aglidar_sdk 从在线雷达、PCAP 文件、ROS 主题这三种源得到 MSOP/DIFOP Packet，将 Packet 发布到 ROS 主题 `/aglidar_packets`，将点云发布到目标 - ROS 主题 `/aglidar_points`。

+ Source 定义源接口；
+ DestinationPointCloud 定义发送点云的目标接口；
+ DestinationPacket 定义发送 MSOP/DIFOP Packet 的目标接口。

![source](./img/class_source_destination.png)

### 2.1 DestinationPointCloud

DestinationPointCloud 定义发送点云的接口。

+ 虚拟成员函数 init() 对 DestinationPointCloud 实例初始化；
+ 虚拟成员函数 start() 启动实例；
+ 虚拟成员函数 sendPointCloud() 发送 PointCloud 消息。

### 2.2 DestinationPacket

DestinationPacket 定义发送 MSOP/DIFOP Packet 的接口。

+ 虚拟成员函数 init() 对 DestinationPacket 实例初始化；
+ 虚拟成员函数 start() 启动实例；
+ 虚拟成员函数 sendPacket() 启动发送 Packet 消息。

### 2.3 Source

Source 是定义源的接口。

+ 成员 `src_type_` 是源的类型

  ```cpp
  enum SourceType
  {
    MSG_FROM_LIDAR = 1,
    MSG_FROM_ROS_PACKET = 2,
    MSG_FROM_PCAP = 3,
  };
  ```

+ 成员 `pc_cb_vec_[]` 中是一组 DestinationPointCloud 的实例。成员函数 sendPointCloud() 调用 `point_cb_vec_[]` 中的实例，发送点云消息。
+ 成员 `pkt_cb_vec_[]` 中是一组 DestinationPacket 实例。成员函数 sendPacket() 将 Packet 消息发送到 `pkt_cb_vec_[]` 中的实例中。
+ 虚拟成员函数 init() 初始化 Source 实例。
+ 虚拟成员函数 start() 启动实例。
+ 虚拟成员函数 regPointCloudCallback() 将 PointCloudDestination 实例注册到 `point_cb_vec_[]`。
+ 虚拟成员函数 regPacketCallback() 将 PacketDestination 实例注册到 `packet_cb_vec_[]`。

### 2.4 DestinationPointCloudRos

DestinationPointCloudRos 在 ROS 主题 `/aglidar_points` 发布点云。

+ 成员 `pkt_pub_` 是 ROS 主题发布器。
+ 成员 `frame_id_` 保存 `frame_id`。`frame_id` 是坐标系名字。

![destination pointcloud ros](./img/class_destination_pointcloud.png)

#### 2.4.1 DestinationPointCloudRos::init()

init() 初始化 DestinationPointCloudRos 实例。

+ 从 YAML 文件读入用户配置参数。
  + 读入 `frame_id`，保存在成员 `frame_id_`，默认值是 `aglidar`。
  + 读入 ROS 主题，保存在本地变量 `ros_send_topic_`，默认值是 `/aglidar_points`。
+ 创建 ROS 主题发布器，保存在成员 `pkt_sub_`.

#### 2.4.2 DestinationPointCloudRos::sendPointCloud()

sendPacket() 在 ROS 主题 `/aglidar_points` 发布点云。

+ 调用 `Publisher::publish()` 发布 ROS 格式的点云消息。

### 2.5 DestinationPacketRos

DestinationPacketRos 在 ROS 主题 `/aglidar_packets` 发布 MSOP/DIFOP Packet。
+ 成员 `pkt_sub_` 是 ROS 主题发布器。
+ 成员 `frame_id_` 保存 `frame_id`。`frame_id` 是坐标系名字。

![destination packet ros](./img/class_destination_packet.png)

#### 2.5.1 DestinationPacketRos::init()

init() 初始化 DestinationPacketRos 实例。

+ 从 YAML 文件读入用户配置参数。
  + 读入 `frame_id`，保存在成员 `frame_id_`，默认值是 `aglidar`
  + 读入 ROS 主题，保存在本地变量 `ros_send_topic_`，默认值是 `/aglidar_packets`。
+ 创建 ROS 主题发布器，保存在成员 `pkt_sub_`。

#### 2.5.2 DestinationPacketRos::sendPacket()

sendPacket() 在 ROS 主题 `/aglidar_packets` 发布 MOSP/DIFOP packet。

+ 调用 `Publisher::publish()` 发布 ROS 格式的 Packet 消息。

### 2.6 SourceDriver

SourceDriver 从在线雷达和 PCAP 文件得到 MSOP/DIFOP Packet，并解析得到点云。
+ 成员 `driver_ptr_` 是 ag_driver 驱动的实例，也就是 LidarDriver。
+ 成员 `free_point_cloud_queue_` 和 `point_cloud_queue_`，分别是空闲点云的队列和待处理点云的队列。
+ 成员 `point_cloud_handle_thread_` 是点云的处理线程。

![source driver](./img/class_source_driver.png)

#### 2.6.1 SourceDriver::init()

init() 初始化 SourceDriver 实例。

+ 读取 YAML 配置文件，得到雷达的用户配置参数。
+ 根据源类型，也就是成员 `src_type_`，创建相应类型的 LidarDriver 实例，也就是成员 `driver_ptr_`。
  + `src_type_` 是在 SourceDriver 中的构造函数中指定的。
+ 调用 LidarDriver::regPointCloudCallback()，注册回调函数。这里是 getPointCloud() 和 putPointCloud()。前者给 `driver_ptr_` 提供空闲点云，后者从 `driver_ptr_` 得到填充好的点云。
  + 注意，这里没有注册 MSOP/DIFOP Packet 的回调函数，因为 Packet 是按需获取的。这时为了避免不必要地消耗 CPU 资源。
+ 调用 LidarDriver::init()，初始化 `driver_ptr_`。
+ 创建、启动点云处理线程 `point_cloud_handle_thread_`， 线程函数是 processPointCloud()。

#### 2.6.2 SourceDriver::getPointCloud()

getPointCloud() 给成员 `driver_ptr_` 提供空闲的点云。

+ 优先从成员 `free_point_cloud_queue_` 得到点云。
+ 如果得不到，分配新的点云。

#### 2.6.3 SourceDriver::putPointCloud()

putPointCloud() 给从成员 `driver_ptr_` 得到填充好的点云。

+ 将得到的点云推送到成员 `point_cloud_queue_`，等待处理。

#### 2.6.4 SourceDriver::processPointCloud()

processPointCloud() 处理点云。在 while 循环中，

+ 从待处理点云的队列 `point_cloud_queue_`，得到点云，
+ 调用 sendPointCloud()，其中调用成员 `pc_cb_vec_[]` 中的 DestinationPointCloud 实例，发送点云。
+ 回收点云，放入空闲点云的队列 `free_cloud_queue_`，待下次使用。

#### 2.6.5 SourceDriver::regPacketCallback()

regPacketCallback() 用来注册 DestinationPacket。
+ 调用 Source::regPacketCallback()，将 DestinationPacket 实例，加入成员 `pkt_cb_vec_[]`。
+ 如果这是首次要求 Packet（`pkt_cb_vec_[]` 的第1个实例），调用 LidarDriver::regPacketCallback()，向 `driver_ptr_` 注册 Packet 回调函数，开始接收 Packet。回调函数是 putPacket()。

#### 2.6.6 SourceDriver::putPacket()

putPacket() 调用 sendPacket()，其中调用成员 `pkt_cb_vec_[]` 中的所有实例，发送 MSOP/DIFOP Packet。

### 2.7 SourcePacketRos

SourcePacketRos 在 ROS 主题 `/aglidar_packets` 得到 MSOP/DIFOP Packet，解析后得到点云。
+ SourcePacketRos 从 SourceDriver 派生，而不是直接从 Source 派生，是因为它用 SourceDriver 解析 Packet 得到点云。
+ 成员 `pkt_sub_`，是 ROS 主题 `/aglidar_packets` 的订阅器。

![source](./img/class_source_packet_ros.png)

#### 2.7.1 SourcePacketRos::init()

init() 初始化 SourcePacketRos 实例。

+ 调用 SourceDriver::init() 初始化成员 `driver_ptr_`。
  + 在 SourcePacketRos 的构造函数中，SourceType 设置为 `SourceType::MSG_FROM_ROS_PACKET`。这样，在 SourceDriver::init() 中，`driver_ptr_` 的 `input_type` 就是 `InputType::RAW_PACKET`，它通过 LidarDriver::feedPacket 接收 Packet 作为源。
+ 解析 YAML 文件得到雷达的用户配置参数
  + 得到接收 Packet 的主题，默认值为 `/aglidar_packets`。
+ 创建 Packet 主题的订阅器，也就是成员 `pkt_sub_`，接收函数是 putPacket()。

#### 2.7.2 SourcePacketRos::putPacket()

putPacket() 接收 Packet，送到 `driver_ptr_` 解析。

+ 调用 LidarDriver::decodePacket()，将 Packet 喂给 `driver_ptr_`。
+ 点云的接收，使用 SourceDriver 的已有实现。

## 3 NodeManager

NodeManager 管理 Source 实例，包括创建、初始化、启动、停止 Source。它支持多个源，但是这些源的类型必须相同。

+ 成员 `sources_[]` 是一个 Source 实例的数组。

![node_manager](./img/class_node_manager.png)

### 3.1 NodeManager::init()

init() 初始化 NodeManger 实例。

+ 从 config.yaml 文件得到用户配置参数
  + 本地变量 `msg_source`，数据源类型
  + 本地变量 `send_point_cloud_ros`， 是否在 ROS 主题发送点云。
  + 本地变量 `send_packet_ros`，是否在 ROS 主题发送 MSOP/DIFOP packet，

+ 在 .yaml 文件中遍历数据源。在循环中，
  + 根据 `msg_source` 创建 Source 实例。
    + 如果是在线雷达（`SourceType::MSG_FROM_LIDAR`），创建 SourceDriver 实例并初始化， 源类型为 `MSG_FROM_LIDAR`。
    + 如果是 PCAP 文件（`SourceType::MSG_FROM_PCAP`），创建 SourceDriver 实例并初始化，源类型为 `MSG_FROM_PCAP`。
    + 如果是 ROS 主题（`SourceType::MSG_FROM_ROS_PACKET`），创建 SourcePacketRos 并初始化。SourcePacketRos 构造函数已将源类型设置为 `MSG_FROM_ROS_PACKET`
  + 如果在 ROS 主题发送点云（`send_point_cloud_ros = true`），则创建 DestinationPointCloudRos 实例、初始化，调用 Source::regPointCloudCallback()，将它加入 Source 的 `pc_cb_vec_[]`。
  + 如果在 ROS 主题发送 Packet（`send_packet_ros = true`），则创建 DestinationPacketRos 实例、初始化，调用 Source::regPacketCallback() 将它加入 Source 的 `pkt_cb_vec_[]`。
  + 将 Source 实例，加入成员 `sources_[]`。

### 3.2 NodeManager::start()

start() 启动成员 `sources_[]` 中的所有实例。