三维固态激光SLAM(livox)

双系统ubuntu20.04复现三维固态激光SLAM,将配置过程分享给大家。

1.安装依赖

Livox激光雷达配置

点击查看代码

cd ~/catkin_ws/src
git clone https://github.com/Luchuanzhao/Livox_simulation_customMsg.git
# 先安装驱动的地层依赖库，没有SDK，ROS驱动无法编译
git clone https://github.com/Livox-SDK/Livox-SDK.git
cd Livox-SDK
cd build && cmake ..
make
sudo make install
cd ~/catkin_ws/src
git clone https://github.com/Livox-SDK/livox_ros_driver.git
mv Livox_simulation_customMsg livox_laser_simulation
cd ~/catkin_ws/src/livox_laser_simulation/src
vim livox_points_plugin.cpp
# 接着修改file_name路径为自己电脑上的路径

将/home/chime/XTDrone/sitl_config/models/livox_avia/livox_avia.sdf路径下面的samples按照自己电脑性能进行修改，否则会卡死，但是太少后续启动FASTER-LIO会由于特征点太少而无法定位。这里我保持原来的100。

FASTER-LIO配置
由于 Ubuntu 20.04 自带有 g++ 9.4.0, gcc 9.4.0 以及 libtbb-dev, libtbb2，因此不再需要安装tbb（不放心的话可以执行pkg-config --libs tbb,如果输出-ltbb就是正常的）

点击查看代码

cd ~/catkin_ws/src
git clone https://github.com/gaoxiang12/faster-lio.git

# 在target_link_libraries中增加`tbbmalloc` `tbbmalloc_proxy`
vim ~/catkin_ws/src/faster-lio/src/CMakeLists.txt 

cd ~/catkin_ws
catkin_make

编译成功

由于Faster-LIO（激光里程计）属于外部视觉+里程计输入，一定要确定KEF修改过来了，~/PX4_Firmware/build/px4_sitl_default/etc/init.d-posix/rcS:

2.通信问题

将~/PX4_Firmware/launch/outdoor4.launch路径下的sdf文件改为了iris_realsense_livox，然后执行：

点击查看代码

# 启动仿真文件
roslaunch px4 outdoor_my.launch
# 启动Faster-lio
roslaunch faster_lio mapping_avia.launch

注意Rviz启动后刚开始点云量会比较少，但是随着时间的推移，会多起来。

接着在启动通信和键盘时碰到了下面这个问题，非常难解决。

后来想到之前在复现ALOAM,VINS-Fusion,HectorSLAM时都进行了话题转换，因此联想到这里也需要将话题转到mavros，因为话题里面有无人机的位姿数据，而报错信息是由于不知道无人机的位置导致的。
使用rostopic list发现有一个/Odometry话题，执行rostopic info /Odometry得到Type: nav_msgs/Odometry，而这就是Fast-LIO的输出格式；同时Publishers为 * /laserMapping，Subscribers为 * /rviz，这指明了能够在rviz中看到点云的原因。接着执行rostopic echo /Odometry -n 1可以看到包含位置、姿态、协方差矩阵等信息。

定位到~/XTDrone/sensing/slam/laser_slam/script文件夹下面，然后将下面代码保存到laser_transfer_copy.py。

点击查看代码

import rospy
from geometry_msgs.msg import PoseStamped
from tf2_ros import TransformListener, Buffer
import sys
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry

vehicle_type = sys.argv[1]
vehicle_id = sys.argv[2]
laser_slam_type = sys.argv[3]
rospy.init_node(vehicle_type+vehicle_id+'_'+laser_slam_type+'_laser_transfer')
pose_pub = rospy.Publisher(vehicle_type+'_'+ vehicle_id+"/mavros/vision_pose/pose", PoseStamped, queue_size=1)
tfBuffer = Buffer()
tflistener = TransformListener(tfBuffer)
local_pose = PoseStamped()
local_pose.header.frame_id = 'map'
hector = PoseStamped()
height = 0

# 新增：用于 faster_lio 的回调
def odom_callback(odom_msg):
    pose_msg = PoseStamped()
    pose_msg.header = odom_msg.header
    pose_msg.pose = odom_msg.pose.pose
    pose_pub.publish(pose_msg)

def hector_callback(data):
    global hector
    hector = data

def height_distance_callback(msg):
    global height
    height = msg.ranges[0]
    if(height==float("inf")):
        height = 0
    
def hector_slam():
    global local_pose, height
    pose2d_sub = rospy.Subscriber(vehicle_type+'_'+ vehicle_id+"/pose", PoseStamped, hector_callback,queue_size=1)
    rate = rospy.Rate(100)
    while True:
        local_pose = hector
        local_pose.pose.position.z = height
        pose_pub.publish(local_pose)
        rate.sleep()
        
def cartographer():
    global local_pose, height
    rate = rospy.Rate(30)
    while not rospy.is_shutdown():
        try:
            tfstamped = tfBuffer.lookup_transform('map', 'base_link', rospy.Time(0))
        except:
            continue
        local_pose.header.stamp = rospy.Time().now()
        local_pose.pose.position = tfstamped.transform.translation
        local_pose.pose.position.z = height
        local_pose.pose.orientation = tfstamped.transform.rotation
        pose_pub.publish(local_pose)
        rate.sleep()
        
def aloam():
    global local_pose
    rate = rospy.Rate(30)
    while not rospy.is_shutdown():
        try:
            tfstamped = tfBuffer.lookup_transform('camera_init', 'aft_mapped', rospy.Time(0))
        except:
            continue
        local_pose.header.stamp = rospy.Time().now()
        local_pose.pose.position = tfstamped.transform.translation
        local_pose.pose.orientation = tfstamped.transform.rotation
        pose_pub.publish(local_pose)
        rate.sleep()

# 新增：faster_lio 转换函数（直接订阅 /Odometry）
def faster_lio():
    rospy.loginfo("Starting faster_lio converter, subscribing to /Odometry")
    rospy.Subscriber("/Odometry", Odometry, odom_callback)
    rospy.spin()  # 保持节点运行
    
if __name__ == '__main__':
    if laser_slam_type == 'hector':
        height_distance_sub = rospy.Subscriber(vehicle_type+'_'+ vehicle_id+"/distance", LaserScan, height_distance_callback,queue_size=1)
        hector_slam()
    elif laser_slam_type == 'cartographer':
        height_distance_sub = rospy.Subscriber(vehicle_type+'_'+ vehicle_id+"/distance", LaserScan, height_distance_callback,queue_size=1)
        cartographer()
    elif laser_slam_type == 'aloam':
        aloam()
    elif laser_slam_type == 'faster_lio':   # 新增分支
        faster_lio()
    else:
        print('input error')

3.成功建图喽

重新执行下面代码：

点击查看代码

# 终端1
roslaunch px4 outdoor4.launch
# 终端2
roslaunch faster_lio mapping_avia.launch
# 终端3
cd ~/XTDrone/sensing/slam/laser_slam/script
python laser_transfer_copy.py iris 0 faster_lio
# 终端4
cd ~/XTDrone/communication
python multirotor_communication.py iris 0 
# 终端5
cd ~/XTDrone/control/keyboard
python multirotor_keyboard_control.py iris 1 vel

4.补充

SLAM算法	输出方式	说明
Hector SLAM	TF 变换(`/pose`话题) + 激光测距	2D SLAM + 激光测距仪提供高度
Cartographer	TF 变换(`map`-->`base_link`)	2D SLAM，但通常也用激光测高
A-LOAM	TF变换(`camera_init`-->`aft_mapped`)	纯 3D SLAM，视觉提供完整位姿
Fast-LIO	Odometry 话题(`/Odometry`)	激光-惯性紧耦合，同样提供 6DoF

chime

三维固态激光SLAM(livox)

1.安装依赖

2.通信问题

3.成功建图喽

4.补充

相关参考

公告