速腾雷达16线实现FAST-LIO2算法建图

很久之前老师就推荐利用fast-lio算法建图，由于很多没有时间所以一直耽搁着，现在有时间就来实现一下，手上现有的是速腾的16线雷达，在实现的过程中参考了一个大佬的两篇博客，以下是博客链接：
使用速腾雷达32线Helios和fast-LIO2算法建图（一）
使用速腾雷达32线Helios和fast-LIO2算法建图（三）

一、配置激光雷达数据

1、下载rsladir-SDK包到catkin_ws/src下

https://github.com/RoboSense-LiDAR/rslidar_sdk/releases/download/v1.3.0/rslidar_sdk.tar.gz

2、添加依赖

yaml（必须）

sudo apt-get update
sudo apt-get install -y libyaml-cpp-dev

pcap（必须）

sudo apt-get install -y  libpcap-dev

Protobuf (可选)
我没有下载protobuf，所以需要修改下面内容：
在下载的SDK的包里的CMakelist.txt文件里面修改

#Protobuf#
find_package(Protobuf QUIET)
find_program(PROTOC protoc)
if(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND)
  message(=============================================================)
  message("-- Protobuf Found, Protobuf Support is turned On!")
  message(=============================================================)
  add_definitions(-DPROTO_FOUND)

  include_directories(${PROTOBUF_INCLUDE_DIRS})
  SET(PROTO_FILE_PATH ${PROJECT_SOURCE_DIR}/src/msg/proto_msg)
  file(GLOB PROTOBUF_FILELIST ${PROTO_FILE_PATH}/*.proto)
  foreach(proto_file ${PROTOBUF_FILELIST})
    message(STATUS "COMPILING ${proto_file} USING ${PROTOBUF_PROTOC_EXECUTABLE}")
    execute_process(COMMAND ${PROTOBUF_PROTOC_EXECUTABLE}
                    --proto_path=${PROTO_FILE_PATH}
                    --cpp_out=${PROTO_FILE_PATH}
                    ${proto_file})
  endforeach()
else(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND)
  message(=============================================================)
  message("-- Protobuf Not Found, Protobuf Support is turned Off!")
  message(=============================================================)
endif(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND)

3、配置参数：

3.1 在下载的SDK的包里的CMakelist.txt文件里面修改一下参数：

set(COMPILE_METHOD ORIGINAL)改为set(COMPILE_METHOD CATKIN)。
set(POINT_TYPE XYZI)改为set(POINT_TYPE XYZIRT) #相较于XYZI，XYZIRT还有时间戳，能满足Fast-lio2时间补偿需求

3.2 将rslidar_sdk工程目录下的package_ros1.xml文件重命名为package.xml。
3.3 修改config.yaml参数（rslidar_sdk/config文件夹内）

lidar:
- driver:
//此处修改激光雷达型号
lidar_type: RS16    #LiDAR type - RS16, RS32, RSBP, RS128, RS128_40, RS80, RSM1, RSHELIOS
frame_id: /rslidar           
msop_port: 6699             
difop_port: 7788  

  msg_source: 1    #1: packet message comes from online Lidar

这个1不用改，表示在线连接激光雷达测试。

4、编译测试

catkin_make
source devel/setup.bash

运行雷达并显示数据

roslaunch start.launch

5、将雷达格式转换为velodyne格式

将以下包下载在catkin_wa/src下：

https://github.com/HViktorTsoi/rs_to_velodyne.git

在路径catkin_ws/src/rs_to_velodyne/下添加launch文件夹，在launch文件夹下，添加velodyne.launch文件，内容如下：

 <launch>
  <node pkg="rs_to_velodyne" name="rs_to_velodyne" type="rs_to_velodyne"  args="XYZIRT XYZIRT"   output="screen">
  </node>
</launch>

6、将转换格式嵌入到start.launch文件中

打开start.launch文件（在rsladir_sdk/launch下）
加入以下语句：

<include file="$(rs_to_velodyne)/launch/velodyne.launch" />

然后运行roslaunch start.launch 在运行rostopic 就会发现发布的话题中出现了/velodyne_points，说明转换成功。

二、IMU配置

目前尚未配置IMU所以这部分先空着，过几天配好了在写。

三、建图

源码下载：

配置环境

pcl可以不用下载直接运行下列指令：
sudo apt-get install ros-melodic-pcl-*
要确保Eigen >= 3.3.4，由于之前装好了就没装了

编译fast-lio包
用git下载源码时，不使用git clone 时，nclude中ikd-Tree文件夹是空的，因为源码git提供了一个git链接，可以自己去下，fast-lio2一大创新就在于使用了ikd-Tree来维护大量的地图点。、

第一步：下载链接fast-lio
https://github.com/hku-mars/FAST_LIO
下载链接kdtree链接
https://github.com/hku-mars/ikd-Tree

注意：下载好了kdtree包后要自己把FAST_LIO需要的那个ikd–Tree包复制进去，在下载的ikd-Tree包里还有一个ikd-Tree包，这个才是需要复制过去的，否则编译时会报错

这些包都放在catkin_ws/src下

配置文件

修改FAST_LIO\src\FAST_LIO\config\velodyne.yaml

common:
    lid_topic:  "/velodyne_points"
    imu_topic:  "/imu/data"

激光雷达配置

preprocess:
    lidar_type: 2      # 1 表示Livox 雷达, 2 表示Velodyne 雷达, 3 表示ouster 雷达
    scan_line: 16      # 32线
    scan_rate: 10      #   velodyne 频率, unit: Hz,

IMU和lidar相对外参、

如果不安装pcl1.8，还需要进行以下步奏，

1、将cmakelist.txt内的
find_package(PCL 1.8 REQUIRED)
改成
find_package(PCL  REQUIRED)
不搜索pcl1.8。

2、将cmakelist.txt生成头文件配置修改
添加add_dependencies(fastlio_mapping  ${catkin_EXPORTED_TARGETS})  
改在以下位置
add_executable(fastlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp) 
add_dependencies(fastlio_mapping  ${catkin_EXPORTED_TARGETS})                       
target_link_libraries(fastlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
target_include_directories(fastlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})

然后编译运行

catkin_make
source devel/setup.bash
roslaunch fast_lio mapping_velodyne.launch

在运行之后我出现了rviz黑屏的问题，经过排查是frameid的问题，rviz里面的FIXED frame给的是crame_init，而我们需要的是velodyne的frame，所以将其修改为velodyne即可，同时修改mapping下的surrond的话题名为/velodyne-points，以及currPoints的Topic为/velodyne_points，此时就可以显示图像并建图。

四、主从机通信

由于载体本身显示的问题，还是想配置一下主从机通信实现远程查看建图效果，但是现在还没写出来，这里先留着，等写完再来。

五、遇到的问题

今天在建图的过程中，出现了建了一会图之后就崩溃了，报下列错误：
[rviz-5] process has died [pid 10281, exit code -9, cmd nice /opt/ros/melodic/lib/rviz/rviz -d /home/unitree/catkin_ws/src/fast_lio/rviz_cfg/loam_livox.rviz __name:=rviz __log:=/home/unitree/.ros/log/b7182a6e-ba57-11ed-b235-48b02d5a882c/rviz-5.log]. log file: /home/unitree/.ros/log/b7182a6e-ba57-11ed-b235-48b02d5a882c/rviz-5*.log
目前还没解决，解决了在写。