从gazebo搭建到点云地图、八叉树地图建立（仿真）
这篇文章记录一下我自己如何从gazebo中搭建仿真模型、然后搭载16线velodyne雷达和IMU传感器、采用LIO-SAM建立点云地图、采用octomap建立八叉树地图。（末尾附上相关model、包的云盘链接）
首先是gazebo搭建仿真模型，我自己搭载了一个模型，但是因为环境比较简单，所以即使传感器数据都用上后面在建立点云地图的时候也存在漂的现象，所以我就直接用gazebo的model了，其中有一个ISCAS Museum，如图：
然后保存这个模型就行了。
接下来就是要采用mbot搭载16线激光雷达和IMU传感器数据了，直接贴代码，16线激光雷达请查看我的其它文章，IMU查看下面的代码：
首先是配置文件imu_gazebo.xacro：
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="imu">
<xacro:property name="imu_size" value="0.1" />
<xacro:property name="imu_hight" value="0.1" />
<xacro:macro name="default_inertial" params="mass">
<inertial>
<mass value="${mass}" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
</inertial>
</xacro:macro>
<xacro:macro name="imu" params="prefix:=imu">
<link name="imu_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="${imu_size} ${imu_size} ${imu_size}"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<box size="${imu_size} ${imu_size} ${imu_size}"/>
</collision>
</link>
<gazebo reference="${prefix}_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>100.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
</xacro:macro>
</robot>
然后是mbot_with_laser_gazebo.xacro：
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find mbot_description)/urdf/mbot_base_gazebo.xacro" />
<xacro:include filename="$(find mbot_description)/urdf/sensors/VLP-16.urdf.xacro" />
<xacro:include filename="$(find mbot_description)/urdf/sensors/imu_gazebo.xacro" />
<!-- <xacro:include filename="$(find mbot_description)/urdf/sensors/lidar_gazebo.xacro" /> -->
<xacro:property name="lidar_offset_x" value="0" />
<xacro:property name="lidar_offset_y" value="0" />
<xacro:property name="lidar_offset_z" value="0.08" />
<!-- <xacro:property name="lidar_offset_z" value="0.12" /> -->
<xacro:property name="imu_offset_x" value="0" />
<xacro:property name="imu_offset_y" value="0" />
<xacro:property name="imu_offset_z" value="0.6" />
<!-- lidar -->
<joint name="lidar_joint" type="fixed">
<origin xyz="${lidar_offset_x} ${lidar_offset_y} ${lidar_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="velodyne_base_link"/>
<!-- <child link="laser_link"/> -->
</joint>
<!-- imu -->
<joint name="imu_joint" type="fixed">
<origin xyz="${imu_offset_x} ${imu_offset_y} ${imu_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="imu_link"/>
</joint>
<xacro:VLP-16/>
<xacro:imu prefix="imu"/>
<!-- <xacro:rplidar prefix="laser"/> -->
<mbot_base_gazebo/>
</robot>
这里我没有调整IMU的高度，凑合着用。
然后运行launch：
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
这里要提一下，如果你需要用cartographer建栅格地图，因为它的雷达数据接口是scan，意思就是需要将velodyne的16线数据转成单线数据，用pointcloud_to_scan这个包就可以了，我之前的文章里面也有说过这个问题。
机器人模型如图：
上面白色的方块就是IMU，查看topic中是否有imu话题：
rostopic list
一切正常，然后用LIO-SAM建点云图：
修改params.yaml：
lio_sam:
# Topics
pointCloudTopic: "velodyne_points" # Point cloud data，改为激光雷达话题
imuTopic: "imu" # IMU data，改为imu话题
odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU
gpsTopic: "odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file
# Frames
lidarFrame: "velodyne_base_link" # 改为激光雷达的"base_link"
baselinkFrame: "imu_link" # 改为IMU的"base_link"
odometryFrame: "odom"
mapFrame: "map"
# GPS Settings
useImuHeadingInitialization: true # if using GPS data, set to "true"
useGpsElevation: false # if GPS elevation is bad, set to "false"
gpsCovThreshold: 2.0 # m^2, threshold for using GPS data
poseCovThreshold: 25.0 # m^2, threshold for using GPS data
# Export settings
savePCD: true # https://github.com/TixiaoShan/LIO-SAM/issues/3
savePCDDirectory: "LIO-SAM_demo" # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation，修改保存路径，具体参考我的其它文章
# Sensor Settings
sensor: velodyne # lidar sensor type, either 'velodyne' or 'ouster'
N_SCAN: 16 # number of lidar channel (i.e., 16, 32, 64, 128)
Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
downsampleRate: 1 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used
lidarMaxRange: 100.0 # default: 1000.0, maximum lidar range to be used
# IMU Settings
imuAccNoise: 0 #3.9939570888238808e-03
imuGyrNoise: 0 #1.5636343949698187e-03
imuAccBiasN: 0 #6.4356659353532566e-05
imuGyrBiasN: 0 #3.5640318696367613e-05
imuGravity: 9.80511
imuRPYWeight: 0.01
# Extrinsics (lidar -> IMU)
extrinsicTrans: [0.0, 0.0, 0.0]
# extrinsicRot: [-1, 0, 0,
# 0, 1, 0,
# 0, 0, -1]
# extrinsicRPY: [0, 1, 0,
# -1, 0, 0,
# 0, 0, 1]
extrinsicRot: [1, 0, 0,
0, 1, 0,
0, 0, 1]
extrinsicRPY: [1, 0, 0,
0, 1, 0,
0, 0, 1] # 默认是前面两个没有注释，后面两个注释了，但是因为我的IMU和激光雷达只存在Z轴上的偏移，不存在角度上的差值，所以旋转矩阵就是单位阵
没有全部贴上来，因为后面用默认的就可以了，主要修改的地方添加了注释。
然后运行：
roslaunch lio_sam run.launch
rosbag play xxx.bag --clock
这里我是实现录制好了点云包的，所以直接播放数据包就行了，如果配置跟我一样的话应该就不会有问题。
跑完之后在保存的路径下可以看到5个pcd文件，用pcl查看一下：
pcl_viewer GlobalMap.pcd
到这里点云地图就建好了，然后是采用octomap建立八叉树地图，运行
roslaunch publish_pointcloud demo.launch
有两处需要修改，都是加载的pcd文件，一个是demo.launch的第5行，一个是publish_pointcloud.cpp的第37行，修改成你自己的pcd文件，如果没问题的话会看到下面的效果。
不同高度颜色不一样，好了，下课！
链接：https://pan.baidu.com/s/1GPrWW5xUJEkyfGlTcw9xZg
提取码：8888