技术文档 | 如何添加激光雷达驱动程序

Lidar是一种常用的环境感知传感器，利用脉冲激光来照射目标并接收目标的反射脉冲，根据激光返回的时间来计算与目标的距离。

通过对目标多次全方位的测量，可以得到目标环境的数字3D结构模型。

Apollo平台默认支持Velodyne 16线、32线、64线和128线等多种型号的Lidar。

本文主要介绍Lidar驱动的主要功能以及如何在Apollo平台中添加一款新的Lidar设备驱动。

• Driver: 通过网络端口接收Lidar硬件产生的UDP数据包，将每一帧封装成VelodyneScan格式后发送。

• Parser:接收VelodyneScan数据，把VelodyneScan中的点由球面坐标系转换成空间直角坐标系下的Pointcldoud点云格式后发送。

• Compensator:接收点云数据和Pose数据，根据每个点的对应的Pose信息把点转换到点云中最大时刻对应的坐标系下，减小由车辆自身的运动带来的误差。需要点云数据中包含每个点的时间戳信息。

1.熟悉Cyber框架

Cyber框架下系统中每一个功能单元都可以抽象为一个Component，通过Channel相互间进行通信，然后根据Dag(有向无环图)配置文件，构建成相应的Pipeline，实现数据的流式处理。

2.消息定义

Apollo已经预定义了点云的消息格式，所以只需要为新Lidar定义一个存储原始扫描数据的proto消息，用于数据的存档和离线开发调试。

相比于点云数据，存档原始数据可以大量节省存储空间。一个新的扫描数据消息可以类似如下定义:

1  // a scan message sample
2  message ScanData {
3  optional apollo.common.Header header = 1;  // apollo header
4  optional Model model = 2;                  // device model
5  optional Mode mode = 3;                    // work mode
6  // device serial number, corresponds to a specific calibration file
7  optional string sn = 4;
8  repeated bytes raw_data = 5;               // raw scan data
9  }

在Velodyne驱动中，其扫描数据消息定义为VelodyneScan。

3.读取原始数据

Lidar每秒会产生大量数据，一般通过UDP协议来进行数据的高效传输。

编写一个Driver Component类，继承于无模版参数Component类；

在Init函数中启动一个异步Poll线程，不断从相应的端口读取Lidar数据；

然后根据需求如将一段时间内的数据打包为一帧Scan Data，如扫描一圈为一帧；

最后通过Writer将ScanData写至相应的Channel发送出去。

1 // Inherit component with no template parameters,
2 // do not receive message from any channel
3 class DriverComponent : public Component<> {
4 public:
5  ~VelodyneDriverComponent();
6  bool Init() override {
7      poll_thread_.reset(new thread([this]{
8          this->Poll();
9      }));
10  }
11
12 private:
13  void Poll() {
14      while (apollo::cyber::Ok()) {
15        // poll data from port xxx
16        // ...
17        austo scan = std::make_shared();
18        // pack ScanData
19        // ...
20        writer_.write(scan);
21      }
22  }
23
24  std::shared_ptr<:thread>poll_thread_;
25  std::shared_ptr<apollo::cyber::writer> writer_;
26  };
27
  YBER_REGISTER_COMPONENT(DriverComponent)<:cyber::writer style="color:#68615e;font-family:Consolas,Inconsolata,Courier,monospace">

4.解析扫描数据，生成点云

编写一个Parser类，输入为一帧ScanData，根据Lidar自己的数据协议，解析出每一个点的时间戳，x/y/z三维坐标，以及反射强度，并组合成一帧点云。

每个点都位于以Lidar为原点的FLU（Front: x, Left: y, Up: z）坐标系下。

1  message PointXYZIT {
2  optional float x = 1 [default = nan];
3  optional float y = 2 [default = nan];
4  optional float z = 3 [default = nan];
5  optional uint32 intensity = 4 [default = 0];
6  optional uint64 timestamp = 5 [default = 0];
7  }

然后定义一个ParserComponent，继承于ScanData实例的Component模板类。

接收ScanData消息，生成点云消息，发送点云消息。

1 class ParserComponent : public Component{
2  public:
3  bool Init() override {
4      ...
5   }
6
7  bool Proc(const std::shared_ptr& scan_msg) override {
8    // get a pointcloud object from objects pool
9      auto point_cloud_out = point_cloud_pool_->GetObject();
10      // clear befor using
11      point_cloud_out->clear();  
12      // parse scan data and generate pointcloud
13      parser_->parse(scan_msg, point_cloud_out);
14      // write pointcloud to a specific channel
15      writer_->write(point_cloud);
16  }
17
18 private:
19  std::shared_ptr<writer> writer_;
20  std::unique_ptrparser_ = nullptr;
21
22  std::shared_ptr<ccobjectpool> point_cloud_pool_ = nullptr;
23  int pool_size_ = 8;
24  };
25
26 CYBER_REGISTER_COMPONENT(ParserComponent)

5.对点云进行运行补偿

运动补偿是一个通用的点云处理过程，可以直接复用Velodyne Driver中Compensator模块的算法逻辑。

6.配置dag文件

将各个数据处理环节定义为Component后。

需要将各个Component组成Lidar数据处理Pipeline，如下配置lidar_driver.dag:

1#  Define all coms in DAG streaming.
2  module_config {
3    module_library : "/apollo/bazel-bin/modules/drivers/xxx/driver/libxxx_driver_component.so"
4    components {
5      class_name : "DriverComponent"
6      config {
7        name : "xxx_driver"
8        config_file_path : "/path/to/lidar_driver_conf.pb.txt"
9      }
10    }
11  }
12
13 module_config {
14    module_library : "/apollo/bazel-bin/modules/drivers/xxx/parser/libxxx_parser_component.so"
15    components {
16      class_name : "ParserComponent"
17      config {
18        name : "xxx_parser"
19        config_file_path : "/path/to/lidar_parser_conf.pb.txt"
20        readers { channel: "/apollo/sensor/xxx/Scan" }
21      }
22    }
23  }
24
25  module_config {
26    module_library : "/apollo/bazel-bin/modules/drivers/xxx/compensator/libxxx_compensator_component.so"
27    components {
28      class_name : "CompensatorComponent"
29      config {
30        name : "pointcloud_compensator"
31        config_file_path : "/apollo/modules/drivers/xxx/conf/xxx_compensator_conf.pb.txt"
32        readers {channel: "/apollo/sensor/xxx/PointCloud2"}
33      }
34    }
35  }

7.运行Lidar驱动并查看点云

完成以步骤后，就可以通过以下命令来启动Lidar驱动。

mainboard -d /path/to/lidar_driver.dag

此时通过cyber_visualizer选择对应的点云Channel，便可可视化查看点云。

在完成以上步骤后，您的新驱动便可在Apollo系统中生效。

﹏﹏﹏﹏﹏﹏﹏   END  ﹏﹏﹏﹏﹏﹏

点击阅读原文，了解更多

◤