Fast‐Drone‐XI35激光雷达适配 - HIT-K1928-LAB/HIT-K1928-wiki GitHub Wiki

说明

原始Fast-Drone-XI35适配的传感器为双目相机+IMU，这篇文档记录了将其适配固态激光雷达MID360的过程，涉及驱动安装、标定、fast-lio适配等过程，对应的分支为board-lidar。

驱动安装

需要安装两个驱动，分别是Livox_SDK2和ros驱动livox_ros_driver2，官方github如下：

Livox_SDK2：https://github.com/Livox-SDK/Livox-SDK2

livox_ros_driver2：https://github.com/Livox-SDK/livox_ros_driver2

针对xi35工程的驱动安装过程：

Livox_SDK2需要安装到系统库路径（/usr/local/lib），以下指令为在容器内直接安装，后续可将安装过程转移到Docker构建中：

git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd ./Livox-SDK2/
mkdir build
cd build
cmake .. && make -j
sudo make install

livox_ros_driver2为livox系列的ros支持功能包，考虑到可能会对其源码进行修改，将其放到主工程的realflight_modules/目录下，原始livox_ros_driver2借助其目录下build.sh完成编译，直接使用catkin_make会报错，为了方便使用catkin_make完成所有功能包编译，修改livox_ros_driver2的编译逻辑：

a. 创建package.xml符号链接
```
cd /root/Fast-Drone-XI35/src/realflight_modules/livox_ros_driver2
ln -sf package_ROS1.xml package.xml
```
b. 修改CMakeLists.txt添加默认值，在CMakeLists.txt开头添加默认ROS版本设置
```
if(NOT DEFINED ROS_EDITION)
    set(ROS_EDITION "ROS1" CACHE STRING "ROS edition: ROS1 or ROS2")
    message(STATUS "ROS_EDITION not specified, defaulting to ROS1")
endif()
```
c. 完成上述操作即可在/Fast-Drone-XI35下直接使用catkin_make
```
#传递ROS_EDITION参数
catkin_make -DROS_EDITION=ROS1
#不传递参数，默认ros1
catkin_make
```

fast-lio适配

需要标定lidar与imu外参以及时间差，标定算法采用li-init，具体流程参考：https://github.com/hku-mars/LiDAR_IMU_Init，orin06的标定结果示例：

Initialization result:
Rotation LiDAR to IMU (degree)     = -2.966341 21.525501 -2.426220
Translation LiDAR to IMU (meter)   =  0.078901 -0.027025  0.059571
Time Lag IMU to LiDAR (second)     = -0.025335
Bias of Gyroscope  (rad/s)         = -0.000388 -0.002575 -0.010448
Bias of Accelerometer (meters/s^2) =  0.002657 -0.010082 -0.000112
Gravity in World Frame(meters/s^2) =  4.177000  0.081221 -8.875932

Homogeneous Transformation Matrix from LiDAR to IMU: 
 0.929431  0.023305  0.368260  0.078901
-0.039378  0.998569  0.036190 -0.027025
-0.366890 -0.048137  0.929018  0.059571
 0.000000  0.000000  0.000000  1.000000


Refinement result:
Rotation LiDAR to IMU (degree)     = -2.347340 20.386953 -3.175725
Translation LiDAR to IMU (meter)   =  0.072290 -0.017996  0.057616
Time Lag IMU to LiDAR (second)     = -0.025335
Bias of Gyroscope  (rad/s)         = -0.000733 -0.000009 -0.002320
Bias of Accelerometer (meters/s^2) =  0.007757  0.016187 -0.001306
Gravity in World Frame(meters/s^2) =  4.146224  0.080914 -8.885441

Homogeneous Transformation Matrix from LiDAR to IMU: 
 0.935931  0.041104  0.349776  0.072290
-0.051925  0.998417  0.021612 -0.017996
-0.348334 -0.038389  0.936584  0.057616
 0.000000  0.000000  0.000000  1.000000

创建适配xi35的config/xi35.yaml和launch/mapping_xi35.launch，并将标定好的外参以及时间差写进config里。
fast-lio输出里程计的世界系是第一帧IMU系，实际工程要求世界系为水平坐标系，即世界系z轴与重力平行，因此需要修改源码调整其里程计的世界系，方法：利用fast-lio输出的重力向量，计算出第一帧IMU系到水平系的旋转，然后将里程计输出转换到水平系。具体修改见程序。
fast-lio输出里程计的时间戳是一帧点云的尾点时间戳，并且是基于lidar时间源的，需要将其转换到imu时间源（后续kf融合是基于imu时间源的），具体修改见程序。

MID360配置PTP时间同步

配置时间同步的目的是使主机与MID360处于同一时间源且能定时同步，是多传感融合算法的基础。MID360支持PTP时间同步，以下过程参考：https://livox-wiki-cn.readthedocs.io/zh-cn/latest/tutorials/new_product/common/time_sync.html#ptp

流程概括：检查网卡是否支持ptp、配置ptp主时钟、抓取报文验证time_type是否为1。

检查连接mid360的网卡是否支持ptp
```
ethtool -T eth0
```
如果网卡capabilities选项包括以下参数, 表明网卡支持软件时间戳：
```
SOF_TIMESTAMPING_SOFTWARE
SOF_TIMESTAMPING_TX_SOFTWARE
SOF_TIMESTAMPING_RX_SOFTWARE
```
如果网卡capabilities选项包括以下参数, 表明网卡支持硬件时间戳：
```
SOF_TIMESTAMPING_RAW_HARDWARE
SOF_TIMESTAMPING_TX_HARDWARE
SOF_TIMESTAMPING_RX_HARDWARE
```
硬件时间戳同步精度能达到us甚至ns级，软件时间戳同步进度在ms级，orin板上的网卡eth0只支持软件时间戳。

配置网卡eth0为ptp主时钟（软件时间戳）安装linuxptp：

git clone http://git.code.sf.net/p/linuxptp/code linuxptp && \
cd linuxptp && \
make && \
make install

在网卡eth0上运行ptp（eth0需要连接上mid360，否则会显示eth0 网卡的物理链路已断开）：

ptp4l -i eth0 -S -m -l 6

成功运行后终端日志打印如下：

ptp4l[6950.986]: port 1 (eth0): INITIALIZING to LISTENING on INIT_COMPLETE
ptp4l[6950.987]: port 0 (/var/run/ptp/ptp4l): INITIALIZING to LISTENING on INIT_COMPLETE
ptp4l[6950.987]: port 0 (/var/run/ptp/ptp4lro): INITIALIZING to LISTENING on INIT_COMPLETE
ptp4l[6958.580]: port 1 (eth0): LISTENING to MASTER on ANNOUNCE_RECEIPT_TIMEOUT_EXPIRES
ptp4l[6958.580]: selected local clock 3c6d66.fffe.0d75d2 as best master
ptp4l[6958.580]: port 1 (eth0): assuming the grand master role

检验是否有ptp报文发出：

tcpdump -i eth0 -nn udp port 319 or udp port 320

运行roslaunch livox_ros_driver2 msg_MID360.launch，抓取点云原始数据包，查看time_type字段是否为1（1 表示lidar时间同步配置为ptp模式）

# 使用tcpdump抓包并保存（运行一段时间后中断即可，抓取的数据保存为mid360_raw.pcap）
tcpdump -i eth0 -nn udp port 56300 -w mid360_raw.pcap
# 解析mid360_raw.pcap
tshark -r mid360_raw.pcap -T fields \
  -e frame.number \
  -e udp.srcport \
  -e udp.dstport \
  -e frame.time_relative \
  -e data | \
  awk '{
    if(length($5)>=72) {
      time_type = substr($5, 21, 2);
      printf("包号:%s 源端口:%s 目的端口:%s 相对时间:%s time_type:%s\n", 
             $1, $2, $3, $4, time_type);
    }
  }'

如果打印显示time_type为01，说明ptp配置成功。

ego-planner适配

只需将建立局部栅格地图的数据源由原来的深度图改为激光点云即可。

复制single_run_in_exp.launch为single_run_in_exp_lio.launch，修改camera_pose_topic和depth_topic为nouse，cloud_topic修改为/fast_lio/cloud_registered。

<arg name="camera_pose_topic" value="nouse"/>
<arg name="depth_topic" value="nouse"/>
<arg name="cloud_topic" value="/fast_lio/cloud_registered"/>

启动

单独启动激光里程计：

sh shfiles/rspx4_lio.sh

tmux一键启动完整上下游算法：

sh tmux-ctrl-plane-lidar.sh