LIO-SAM 算法在 Ubuntu 22.04 与 ROS2 Humble 下的仿真实现

LIO-SAM 算法在 Ubuntu 22.04 与 ROS2 Humble 下的仿真实现 | 极客日志

# 创建 4GB Swap 文件
sudo fallocate -l 4G /swapfile
sudo chmod 600 /swapfile
sudo mkswap /swapfile
sudo swapon /swapfile
# 验证 (Total 应该显示约 3.8Gi)
free -h

cd ~/Downloads
mv gtsam-4.1.1 ~/gtsam

cd ~/gtsam
mkdir build && cd build

cmake -DGTSAM_USE_SYSTEM_EIGEN=ON ..

make -j$(nproc)

sudo make install

ls /usr/local/include/gtsam

sudo apt update && sudo apt install curl gnupg lsb-release -y

sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg

echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(source /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null

sudo apt update

sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup ros-humble-pcl-conversions

sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup ros-humble-pcl-conversions ros-humble-xacro ros-humble-robot-localization ros-humble-tf-transformations -y

sudo apt update
sudo apt install ros-humble-gazebo-ros-pkgs ros-humble-gazebo-ros

sudo apt update
sudo apt install ros-humble-gazebo-plugins ros-humble-gazebo-ros2-control

source /opt/ros/humble/setup.bash
cd ~/lio_sam_gazebo_ros2
source install/setup.bash

gedit ~/lio_sam_gazebo_ros2/src/velodyne_simulator/velodyne_description/package.xml

<?xml version="1.0"?>
<package format="3">
  <name>velodyne_description</name>
  <version>2.0.3</version>
  <description>URDF description for Velodyne 3D LIDARs</description>
  <maintainer email="[email protected]">maintainer</maintainer>
  <license>BSD</license>
  <buildtool_depend>ament_cmake</buildtool_depend>
  <exec_depend>urdf</exec_depend>
  <exec_depend>xacro</exec_depend>
  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

sudo apt update
sudo apt install ros-humble-pcl-conversions

修改代码以适配 GTSAM 4.1.1 (关键)：
- 找到并打开 CMakeLists.txt：
```
# 假设文件夹名叫 LIO-SAM，如果叫别的名字请自行替换
gedit ~/lio_sam_gazebo_ros2/src/LIO-SAM/CMakeLists.txt
```
- **修改 C++ 标准：**在 project(...) 这一行下面手动添加：
```
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
```
  解释：这两行命令强制告诉编译器：'不管你默认是什么，我必须用 C++17 标准来编译，否则就报错'。
- **添加 find_package：**找到文件前面的一堆 find_package，在其中找个空行，插入下面这行：
```
find_package(pcl_conversions REQUIRED)
```
- **添加头文件路径：**在 find_package 后面找到 include_directories，把它修改为包含 pcl_conversions 的路径。如果找不到，可以直接在 include_directories(include) 的下一行添加：
```
include_directories(include ${pcl_conversions_INCLUDE_DIRS})
```
- **添加依赖：**搜索 ament_target_dependencies，然后在括号里的最后加上 pcl_conversions，例如：
```
ament_target_dependencies(lio_sam_imageProjection ${dependencies} pcl_conversions)
```
  (注意：文件里会有 4-5 个 ament_target_dependencies，建议每一个都检查一下)
- 保存并关闭。
修改配置文件 (Params.yaml)：
- 打开配置文件
```
gedit ~/lio_sam_gazebo_ros2/src/LIO-SAM/config/params.yaml
```
- 修改保存路径：找到 savePCDDirectory，将其修改为您当前用户下的路径：
```
savePCD: true
savePCDDirectory: "~/Downloads/LOAM/"
```
  (提示：如果您还没创建这个文件夹，记得去终端运行 mkdir -p ~/Downloads/LOAM)
- 添加依赖：在 <depend>...</depend> 的区域，找个空行添加：
```
<depend>pcl_conversions</depend>
```
- 保存并关闭。

开始编译：

回到工作空间根目录：
```
cd ~/lio-sam-gazebo-ros2
```
清理旧的编译缓存 (为了防止之前的错误干扰，建议先清理一下)：
```
rm -rf build install log
rm -rf build/lio_sam install/lio_sam
```

编译

colcon build --symlink-install
# 如果仍不行，尝试单线程编译，可能需要更久时间（10-20 分钟）
colcon build --symlink-install --parallel-workers 1

模型准备

创建模型目录
```
mkdir -p ~/.gazebo/models
```
将 lio_sam_gazebo_ros2/models 下的三个文件夹（brick_box、construction_barrel、rtab_map_stage）移入到 .gazebo/models 文件夹中。
若在 home 中没有看见 .gazebo 文件夹，可能是被隐藏了，按下 Ctrl + H 即可。

覆盖雷达配置文件

# 1. 切换到目标目录
cd ~/lio_sam_gazebo_ros2/src/velodyne_simulator/velodyne_description/urdf
# 2. 删除旧文件（防止权限或损坏问题）
rm -f VLP-16.urdf.xacro
# 3. 创建新文件并打开编辑器
gedit VLP-16.urdf.xacro

粘贴标准修复代码将以下代码完整复制，粘贴到刚才打开的 gedit 编辑器中，然后保存并关闭。

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="VLP-16">
  <xacro:property name="M_PI" value="3.1415926535897931"/>
  <xacro:macro name="VLP-16" params="*origin parent:=base_link name:=velodyne topic:=/velodyne_points hz:=10 lasers:=16 samples:=1875 collision_range:=0.3 min_range:=0.9 max_range:=130.0 noise:=0.008 min_angle:=-${M_PI} max_angle:=${M_PI} gpu:=false">
    <joint name="${name}_base_mount_joint" type="fixed">
      <xacro:insert_block name="origin"/>
      <parent link="${parent}"/>
      <child link="${name}_base_link"/>
    </joint>
    <link name="${name}_base_link">
      <inertial>
        <mass value="0.83"/>
        <origin xyz="0 0 0.03585"/>
        <inertia ixx="${(0.83 * (3.0*0.0516*0.0516 + 0.0717*0.0717)) / 12.0}" ixy="0" ixz="0" iyy="${(0.83 * (3.0*0.0516*0.0516 + 0.0717*0.0717)) / 12.0}" iyz="0" izz="${0.5 * 0.83 * (0.0516*0.0516)}"/>
      </inertial>
      <visual>
        <geometry>
          <mesh filename="package://velodyne_description/meshes/VLP16_base_1.dae"/>
        </geometry>
      </visual>
      <visual>
        <geometry>
          <mesh filename="package://velodyne_description/meshes/VLP16_base_2.dae"/>
        </geometry>
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0.03585"/>
        <geometry>
          <cylinder radius="0.0516" length="0.0717"/>
        </geometry>
      </collision>
    </link>
    <joint name="${name}_base_scan_joint" type="fixed">
      <origin xyz="0 0 0.0377" rpy="0 0 0"/>
      <parent link="${name}_base_link"/>
      <child link="${name}"/>
    </joint>
    <link name="${name}">
      <inertial>
        <mass value="0.01"/>
        <origin xyz="0 0 0"/>
        <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
      </inertial>
      <visual>
        <origin xyz="0 0 -0.0377"/>
        <geometry>
          <mesh filename="package://velodyne_description/meshes/VLP16_scan.dae"/>
        </geometry>
      </visual>
    </link>
    <!-- Gazebo requires the velodyne_gazebo_plugins package -->
    <gazebo reference="${name}">
      <!-- 关键修改：将 gpu_ray 改为 ray，强制使用 CPU 计算 -->
      <sensor type="ray" name="${name}-VLP16">
        <pose>0 0 0 0 0 0</pose>
        <visualize>false</visualize>
        <update_rate>${hz}</update_rate>
        <ray>
          <scan>
            <horizontal>
              <samples>${samples}</samples>
              <resolution>1</resolution>
              <min_angle>${min_angle}</min_angle>
              <max_angle>${max_angle}</max_angle>
            </horizontal>
            <vertical>
              <samples>${lasers}</samples>
              <resolution>1</resolution>
              <min_angle>-${15.0*M_PI/180.0}</min_angle>
              <max_angle> ${15.0*M_PI/180.0}</max_angle>
            </vertical>
          </scan>
          <range>
            <min>${collision_range}</min>
            <max>${max_range}</max>
            <resolution>0.001</resolution>
          </range>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>${noise}</stddev>
          </noise>
        </ray>
        <!-- 关键修改：插件名称去掉 gpu_ -->
        <plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_velodyne_laser.so">
          <topicName>${topic}</topicName>
          <frameName>${name}</frameName>
          <min_range>${min_range}</min_range>
          <max_range>${max_range}</max_range>
          <gaussianNoise>${noise}</gaussianNoise>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>
</robot>

编译并重启

# 确保插件库存在 (保险措施)
sudo apt update
sudo apt install ros-humble-velodyne-simulator
# 清理旧的编译文件（非常重要）
cd ~/lio_sam_gazebo_ros2
rm -rf build/velodyne_description install/velodyne_description
# 重新编译
colcon build --symlink-install --packages-select velodyne_description
# 启动仿真
source install/setup.bash
export GAZEBO_MODEL_DATABASE_URI=""
export SVGA_VGPU10=0
export LIBGL_ALWAYS_SOFTWARE=1
ros2 launch robot_gazebo robot_sim.launch.py

最终检查：打开新终端
```
ros2 topic list
```

进入工作空间并刷新环境：

cd ~/lio_sam_gazebo_ros2
source install/setup.bash

启动仿真：

# 这一步通过修改环境变量，禁止 Gazebo 去连接模型库
export GAZEBO_MODEL_DATABASE_URI=""
# 这是 VMware 的专用修复补丁
export SVGA_VGPU10=0
export LIBGL_ALWAYS_SOFTWARE=1
# 启动仿真
ros2 launch robot_gazebo robot_sim.launch.py

如果报错找不到包：请输入 ros2 launch 然后按两下 Tab 键，看看有哪些包是以 robot_ 或 neo_ 开头的，替换上面的命令。

**列出话题：**打开终端 1 没有 gazebo 窗口弹出的话，打开终端 4，输入以下命令
```
ros2 topic list
```
(确认有没有 /velodyne_points 或类似名字)
- 强制重新编译雷达插件：
  1. 清理旧文件：
```
cd ~/lio_sam_gazebo_ros2
rm -rf build/velodyne* install/velodyne*
```
  2. 重新编译：(这次我们只编译雷达相关包，速度会很快)
```
colcon build --symlink-install --packages-select velodyne_description velodyne_gazebo_plugins velodyne_simulator
```
    检查点：观察编译输出。如果在编译 velodyne_gazebo_plugins 时没有报错，且显示 Finished，说明插件生成成功了。
  3. 再次刷新环境：
```
source install/setup.bash
```
  4. 再次启动仿真 (终端 1)：
```
export GAZEBO_MODEL_DATABASE_URI=""
export SVGA_VGPU10=0
export LIBGL_ALWAYS_SOFTWARE=1
ros2 launch robot_gazebo robot_sim.launch.py
```
  5. 再次检查话题 (终端 4)：
```
ros2 topic list
```
    重要：检查有没有类似 /velodyne_points 的输出，或者出现 /gazebo_ros_laser_controller/out……
  6. 确定雷达点云名称
```
ros2 topic info /gazebo_ros_laser_controller/out
```
    在终端 4 中输入以上命令，观察结果中 Type 这一行：如果显示 Type: sensor_msgs/msg/PointCloud2
    - 操作：直接去修改 LIO-SAM 的 params.yaml，把 pointCloudTopic 改成 /gazebo_ros_laser_controller/out，然后直接运行！否则如果显示 Type: sensor_msgs/msg/LaserScan 或者其他
    - 操作：请检查插件是否正确加载。
  7. 重新编译与验证
    - 编译
```
cd ~/lio_sam_gazebo_ros2
colcon build --packages-select robot_gazebo --symlink-install
```
    - 启动仿真
```
export GAZEBO_MODEL_DATABASE_URI=""
export SVGA_VGPU10=0
export LIBGL_ALWAYS_SOFTWARE=1
source install/setup.bash
ros2 launch robot_gazebo robot_sim.launch.py
```

进入工作空间并刷新环境：

cd ~/lio_sam_gazebo_ros2
source install/setup.bash

启动算法：
```
ros2 launch lio_sam run.launch.py
```
成功标志：Rviz (可视化界面) 会自动弹出来。

运行键盘控制节点 (这是一个通用的 ROS 工具)：
```
ros2 run teleop_twist_keyboard teleop_twist_keyboard
```
(如果没有安装，运行 sudo apt install ros-humble-teleop-twist-keyboard 安装)
操作方法：确保鼠标点击在这个终端窗口上（终端 3）。按 i 前进，, 后退。按 j 左转，l 右转。按 k 停止。

注意： 速度不要太快（1 左右就差不多了），否则雷达匹配可能会跟丢。终端没有输出显示是正常的，长按上述按键即可。

可以看见随机器人（小车）移动，Rviz 窗口点云图越来越完善。

安装转换工具 (我们需要一个 Python 小工具)：
```
pip install rosbags
```
(如果没有安装 pip，请先运行 sudo apt install python3-pip)

执行转换：(假设您的文件名叫 campus_small_dataset.bag，请替换为您真实的文件名)

# 语法：--src <源文件名> --dst <输出文件夹名>
python3 -m rosbags.convert --src campus_small_dateset.bag --dst campus_small_dataset_ros2

注意：需要在文件所在目录下运行

等待完成：转换完成后，当前目录下会多出一个新的文件夹，名字为上述输出文件夹名

修复 metadata.yaml 文件并查找话题名：

打开 metadata.yaml 文件：

gedit ~/campus_small_dataset_ros2/metadata.yaml

修复 metadata.yaml 格式错误 (解决 bad conversion 报错)

cd ~/campus_small_dataset_ros2
sed -i 's/offered_qos_profiles: \[\]/offered_qos_profiles: ""/g' metadata.yaml

寻找 topics_with_message_count 或者 topics 这一段。

点云话题 (Lidar)：
- 寻找包含 points 或 velodyne 字样的名字。
- 通常是：/velodyne_points 或 /points_raw。
IMU 话题：
- 寻找包含 imu 字样的名字。
- 通常是：/imu/data 或 /imu_raw。

示例（您的文件长得可能像这样）：

...topics_with_message_count:
  - topic_metadata:
      name: /velodyne_points  # 这就是点云话题名！
      type: sensor_msgs/msg/PointCloud2
  ...
  - topic_metadata:
      name: /imu/data  # 这就是 IMU 话题名！
      type: sensor_msgs/msg/Imu
  ...

修改 LIO-SAM 配置拿到上面两个名字后，修改 LIO-SAM 的配置文件：
```
gedit ~/lio_sam_gazebo_ros2/src/LIO-SAM-ros2/config/params.yaml
```
- 把 pointCloudTopic 改成您在文件里看到的点云名字。
- 把 imuTopic 改成您在文件里看到的 IMU 名字。
- 保存并关闭。

编译（确保配置生效）：

cd ~/lio_sam_gazebo_ros2
colcon build --packages-select lio_sam --symlink-install

运行（终端 1）：

source install/setup.bash
ros2 launch lio_sam run.launch.py

终端 2：

ros2 bag play campus_small_dataset_ros2

启动仿真 (终端 1)：

export GAZEBO_MODEL_DATABASE_URI=""
export SVGA_VGPU10=0
export LIBGL_ALWAYS_SOFTWARE=1
ros2 launch robot_gazebo robot_sim.launch.py

再次检查话题 (终端 4)：
```
ros2 topic list
```
重要：检查有没有类似 /velodyne_points 的输出，或者出现 /gazebo_ros_laser_controller/out……

确定雷达点云名称

ros2 topic info /gazebo_ros_laser_controller/out

通过上述操作我确定的雷达点云名称为 /gazebo_ros_laser_controller/out，imu 名称为 /imu_plugin/out 在 params.yaml 文件中修改如下：

pointCloudTopic: "/gazebo_ros_laser_controller/out"  # Point cloud data
imuTopic: "/imu_plugin/out"                          # IMU data

重新编译

cd ~/lio_sam_gazebo_ros2
colcon build --symlink-install

启动仿真（终端 1）

export GAZEBO_MODEL_DATABASE_URI=""
export SVGA_VGPU10=0
export LIBGL_ALWAYS_SOFTWARE=1
source install/setup.bash
ros2 launch robot_gazebo robot_sim.launch.py

启动 Rviz（终端 2）：

cd ~/lio_sam_gazebo_ros2
source install/setup.bash
ros2 launch lio_sam run.launch.py

运行键盘控制节点 (终端 3)：

ros2 run teleop_twist_keyboard teleop_twist_keyboard

打开 metadata.yaml 文件查找话题名

gedit ~/campus_small_dataset_ros2/metadata.yaml

我查找到的雷达点云名称为 /points_raw，imu 名称为 /imu_correct 在 params.yaml 文件中修改如下：

pointCloudTopic: "/points_raw"  # Point cloud data
imuTopic: "/imu_correct"         # IMU data

重新编译 (因为改了 yaml)

cd ~/lio_sam_gazebo_ros2
colcon build --packages-select lio_sam --symlink-install

启动 LIO-SAM（终端 1）

source install/setup.bash
ros2 launch lio_sam run.launch.py

播放数据（终端 2:）

ros2 bag play campus_small_dataset_ros2

LIO-SAM 算法在 Ubuntu 22.04 与 ROS2 Humble 下的仿真实现

背景

LIO-SAM 算法工作机理

2.1 点云去畸变处理

2.2 因子图优化

仿真操作

3.1 环境准备与配置

3.2 安装依赖

3.2.1 手动下载源码包

3.2.2 准备编译环境

3.3 运行仿真

3.3.1 启动 Gazebo 仿真环境（终端 1）

3.3.2 启动 LIO-SAM 建图算法 (终端 2)

3.3.3 控制机器人移动 (终端 3)

3.3.4 点云图保存

利用已有数据集进行仿真（无需启动 Gazebo）

4.1 安装转换

4.2 文件修复

4.3 运行仿真（运行 LIO-SAM）

方法一与方法二之间的转换

5.1 方法 1：

5.1.1 获取点云及 IMU 话题名

5.1.2 编译运行

5.2 方法 2：

5.2.1 获取点云及 IMU 话题名

5.2.2 编译运行

更多推荐文章

相关免费在线工具

LIO-SAM 算法在 Ubuntu 22.04 与 ROS2 Humble 下的仿真实现

背景

LIO-SAM 算法工作机理

2.1 点云去畸变处理

2.2 因子图优化

仿真操作

3.1 环境准备与配置

3.2 安装依赖

3.2.1 手动下载源码包

3.2.2 准备编译环境

3.3 运行仿真

3.3.1 启动 Gazebo 仿真环境（终端 1）

3.3.2 启动 LIO-SAM 建图算法 (终端 2)

3.3.3 控制机器人移动 (终端 3)

3.3.4 点云图保存

利用已有数据集进行仿真（无需启动 Gazebo）

4.1 安装转换

4.2 文件修复

4.3 运行仿真（运行 LIO-SAM）

方法一与方法二之间的转换

5.1 方法 1：

5.1.1 获取点云及 IMU 话题名

5.1.2 编译运行

5.2 方法 2：

5.2.1 获取点云及 IMU 话题名

5.2.2 编译运行

微信扫一扫，关注极客日志

更多推荐文章

相关免费在线工具