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 的目标接口。

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_ 是源的类型

  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 是坐标系名字。

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 是坐标系名字。

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_ 是点云的处理线程。

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 的订阅器。

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 实例的数组。

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_[] 中的所有实例。