宇树 G1 部署（十三）——本体部署 SLAM 导航

考虑到宇树自身带了一块ARM开发板，因此导航可以分为两种思路：

额外上位机远程部署通过无线发送位姿，主要难点在于通讯频率
在 pc2 上直接部署，主要难点在于算法轻量化

这个笔记记一下在本体上部署的一种简单方法

基于项目：元启・鸿图HongTu

测评感觉：非常好的一个基础学习项目，由于本体硬盘带的是 ubuntu20.04 系统，所以适配度蛮好的

由于在 G1 上部署依然有很多 bug，记录一下全流程

[1 环境安装与修改配置](#1 环境安装与修改配置)

[1.1 克隆仓库](#1.1 克隆仓库)

[1.2 安装 Livox SDK2](#1.2 安装 Livox SDK2)

[1.3 更改雷达ip](#1.3 更改雷达ip)

[1.4 更改地图保存路径](#1.4 更改地图保存路径)

[1.5 编译项目](#1.5 编译项目)

[1.5.1 安装相关依赖](#1.5.1 安装相关依赖)

[1.5.2 工作空间编译](#1.5.2 工作空间编译)

[1.5.3 安装依赖包](#1.5.3 安装依赖包)

[2 部署应用](#2 部署应用)

[2.1 建图及保存](#2.1 建图及保存)

[2.2 编辑地图](#2.2 编辑地图)

[2.3 开启导航](#2.3 开启导航)

[2.4 开启运控](#2.4 开启运控)

1 环境安装与修改配置

1.1 克隆仓库

bash 复制代码

git clone https://github.com/yuanqizhiti/HongTu.git

1.2 安装 Livox SDK2

bash 复制代码

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

1.3 更改雷达ip

bash 复制代码

# 修改本机与雷达ip
cd HongTu/G1Nav2D/src/livox_ros_driver2-master/config/
gedit MID360_config.json

修改逻辑和之前笔记记录的一样，主要是 ip 地址：

python 复制代码

{
  "lidar_summary_info" : {
    "lidar_type": 8
  },
  "MID360": {
    "lidar_net_info" : {
      "cmd_data_port": 56100,
      "push_msg_port": 56200,
      "point_data_port": 56300,
      "imu_data_port": 56400,
      "log_data_port": 56500
    },
    "host_net_info" : {
      "cmd_data_ip" : "192.168.123.164",
      "cmd_data_port": 56101,
      "push_msg_ip": "192.168.123.164",
      "push_msg_port": 56201,
      "point_data_ip": "192.168.123.164",
      "point_data_port": 56301,
      "imu_data_ip" : "192.168.123.164",
      "imu_data_port": 56401,
      "log_data_ip" : "",
      "log_data_port": 56501
    }
  },
  "lidar_configs" : [
    {
      "ip" : "192.168.123.120",
      "pcl_data_type" : 1,
      "pattern_mode" : 0,
      "extrinsic_parameter" : {
        "roll": 0.0,
        "pitch": 0.0,
        "yaw": 0.0,
        "x": 0,
        "y": 0,
        "z": 0
      }
    }
  ]
}

1.4 更改地图保存路径

bash 复制代码

# 修改地图保存路径，将该文件下最底部路径改为自己的电脑
cd HongTu/G1Nav2D/src/fastlio2/src/
gedit map_builder_node.cpp

python 复制代码

    // 设置全局指针和保存路径
    g_ground_pub_thread = &ground_pub_thread;
    g_map_path = "/home/unitree/Codes/HongTu/G1Nav2D/src/fastlio2/PCD/map.pcd";
    g_ground_map_path = "/home/unitree/Codes/HongTu/G1Nav2D/src/fastlio2/PCD/ground_map.pcd";
    g_keyposes_path = "/home/unitree/Codes/HongTu/G1Nav2D/src/fastlio2/path/key_poses.txt";
    map_builder.run();
    ground_pub_worker.join();
    return 0;

1.5 编译项目

1.5.1 安装相关依赖

python3-empy：

bash 复制代码

sudo apt-get update
sudo apt-get install python3-empy

tf2_sensor_msgs：

bash 复制代码

sudo apt-get update
sudo apt-get install ros-noetic-tf2-sensor-msgs

装完之后，重新加载下环境（或者开个新终端）：

bash 复制代码

source /opt/ros/noetic/setup.bash

（建议加到 ~/.bashrc 里，这样每次开终端就自动 source）

==================================增值税==================================

如果之前没有修正过环境，G1 自带的环境大概率报 404 / GPG 错误，这时候进行ROS 软件源修正：

1️⃣ 删除所有旧的 ROS 软件源

bash 复制代码

sudo rm /etc/apt/sources.list.d/ros-latest.list 2>/dev/null
sudo rm /etc/apt/sources.list.d/ros2-latest.list 2>/dev/null

# 彻底清理，把 sources.list 里可能残留的 ROS 源也删掉
sudo sed -i '/ros\/ubuntu/d' /etc/apt/sources.list
sudo sed -i '/ros2\/ubuntu/d' /etc/apt/sources.list

2️⃣ 删除旧的 ROS GPG Key

ROS 官方之前换过 key，宇树 G1 自带的就是过期的那个，需要删除后重新导入：

bash 复制代码

sudo apt-key del F42ED6FBAB17C654 2>/dev/null

# 检查一下是否还残留 key（如果为空就没问题）：
apt-key list | grep -i ros

3️⃣ 重新添加 ROS 官方源

创建源文件：

bash 复制代码

echo "deb http://packages.ros.org/ros/ubuntu focal main" | sudo tee /etc/apt/sources.list.d/ros-latest.list

4️⃣ 重新添加最新 ROS GPG key

bash 复制代码

sudo apt-get install curl gnupg2 lsb-release -y

# 导入最新密钥
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
# 成功时应该输出：OK

5️⃣ 更新 apt

python 复制代码

sudo apt-get update

# ⚠️ 这一步必须不再出现 GPG 错误，否则说明前面没清干净
# 如果看到以下内容那就成功了：
# Get: ... focal InRelease
# Hit: ...
# Reading package lists... Done

6️⃣ 再次安装 tf2_sensor_msgs

python 复制代码

sudo apt-get install ros-noetic-tf2-sensor-msgs

==================================增值税==================================

1.5.2 工作空间编译

如果直接编译，会发现多个报错，接下来先逐个解决避免，最后再编译

1️⃣ 如果直接编译，会报错：

bash 复制代码

fatal error: livox_ros_driver2/CustomMsg.h: No such file or directory

这是因为 livox_ros_driver2 的自定义消息 CustomMsg 的头文件没被找到

因此，先单独把 livox_ros_driver2 编译一遍，让它把消息生成出来，然后再编整个工作空间

bash 复制代码

# 根据自己项目路径修改
cd ~/Codes/HongTu/G1Nav2D

# 确保是 ROS1 环境
source /opt/ros/noetic/setup.bash

# 单独编译 livox_ros_driver2（注意带上 ROS_EDITION=ROS1）
catkin_make -DROS_EDITION=ROS1 --pkg livox_ros_driver2

这一步的目标就是生成 CustomMsg.h 等相关文件，编完之后可以确认一下：

bash 复制代码

ls devel/include/livox_ros_driver2

# 如果里面能看到 CustomMsg.h，就说明这一关 OK 了

2️⃣ livox_ros_driver2 的头文件已经有了，但依然会报新的错误：

bash 复制代码

fatal error: fastlio/SlamReLoc.h: No such file or directory
fatal error: fastlio/SaveMap.h: No such file or directory

这说明 fastlio 包里通过 srv 生成的头文件都还没生成

因此，先单独编 fastlio，在工作空间里依次执行：

bash 复制代码

cd ~/Codes/HongTu/G1Nav2D
source /opt/ros/noetic/setup.bash

catkin_make -DROS_EDITION=ROS1 --pkg fastlio

确认头文件已经生成：

bash 复制代码

cd ~/Codes/HongTu/G1Nav2D
ls devel/include/fastlio

# 应该能看到类似：MapConvert.h  SaveMap.h  SlamHold.h  SlamReLoc.h  SlamRelocCheck.h  SlamStart.h  ...

3️⃣ livox_ros_driver2

这个问题其实是最早出现的，但是由于需要解决掉前两个，这个才能没问题，因此按照安装顺序进行了优化

livox_ros_driver2 默认按照 ROS2 的方式在编译（用 ament_cmake_auto），而 G1 的工作空间是 ROS1（Noetic + catkin），所以会报错

livox_ros_driver2 这个仓库本身是"同时兼容 ROS1 和 ROS2"的，它在 CMakeLists.txt 里用一个变量 ROS_EDITION 来区分走 ROS1 还是 ROS2 分支。因此，按照官方 README 的推荐编译方式，在 G1Nav2D 工作空间里，加上 ROS_EDITION 选项：

bash 复制代码

cd ~/Codes/HongTu/G1Nav2D

# 如果之前安装过，则需要清理然后按照全部的流程安装一遍
# 如果是按照正确流程，先编译了头文件，则不要清理！！！否则需要编译安装头文件
# 1. 清理之前的 build / devel（非常重要，避免旧的 CMake 缓存）
# rm -rf build devel

# 2. 确保当前终端加载的是 ROS1 Noetic
source /opt/ros/noetic/setup.bash

# 3. 重新用字符串形式指定 ROS_EDITION 为 ROS1
catkin_make -DROS_EDITION=ROS1

解释一下这一步：

-DROS_EDITION=1 相当于手动做了一次 ./build.sh ROS1 里的核心操作
这样 CMakeLists.txt 里判断 ROS1 / ROS2 的逻辑就会进 ROS1 分支，不再调用 find_package(ament_cmake_auto ...)

最后显示：

bash 复制代码

[100%] Built target ros_map_edit
[100%] Built target body2map_pointcloud

这一段输出就说明：整个工程已经编译成功了 ✅

没有再出现 Error、undefined reference、Invoking "make ... failed" 之类的字样

→ 说明 catkin_make -DROS_EDITION=ROS1 已经完整通过了，包括之前卡住的 fastlio、livox_ros_driver2、ikd-Tree 那些问题都已经解决

4️⃣ 最后，进行环境设置（每次用前）

在工作空间根目录执行：

bash 复制代码

cd ~/Codes/HongTu/G1Nav2D
source devel/setup.bash

可以把这一行加进 ~/.bashrc，以后开终端自动生效：

bash 复制代码

echo "source ~/Codes/HongTu/G1Nav2D/devel/setup.bash" >> ~/.bashrc

1.5.3 安装依赖包

此处由于项目是 ROS1（Noetic），做了防 bug 优化：

bash 复制代码

sudo apt update
sudo apt install ros-noetic-teb-local-planner ros-noetic-global-planner ros-noetic-costmap-2d

单独安装：

bash 复制代码

sudo apt-get install ros-noetic-octomap-server

此外还可以安装 move_base（常用于 2D 导航）：

bash 复制代码

sudo apt install ros-noetic-move-base

2 部署应用

2.1 建图及保存

终端1建图：

bash 复制代码

# 建图
cd HongTu/G1Nav2D/
source devel/setup.bash
roslaunch fastlio mapping.launch

终端2保存：

bash 复制代码

# 打开新终端
cd HongTu/G1Nav2D/
source devel/setup.bash
# 保存地图，自定义路径及地图名称
rosrun map_server map_saver map:=/projected_map -f /home/nvidia/mymap

简单分析一下这个指令：

map_server 是 ROS 包名
map_saver 是这个包中的可执行文件，用于保存栅格地图（PGM + YAML），运行后会将订阅到的 map 数据写入磁盘
map:=/projected_map 是一条 topic 重映射（topic remap），默认 map_saver 会订阅 /map 话题，通过 map:=/projected_map 让它订阅 /projected_map。也就是说：告诉 map_saver：去 /projected_map 获取栅格地图
-f /home/nvidia/mymap：-f 表示输出文件（file prefix），不包含扩展名，map_saver 会自动生成：将保存两个文件：/home/nvidia/mymap.pgm 和 /home/nvidia/mymap.yaml，如果路径不存在，会保存失败
综上，这条命令的含义：从 /projected_map 话题获取 2D 栅格地图，并保存为 /home/nvidia/mymap.yaml 和 /home/nvidia/mymap.pgm

2.2 编辑地图

bash 复制代码

# 打开地图，利用Map Eraser Tool修改地图，ctrl+加号或减号可修改画笔大小，保存地图
source devel/setup.bash
roslaunch ros_map_edit map_edit.launch

也可以使用 GIMP 编辑：

bash 复制代码

gimp ~/Desktop/Codes/SLAM/g1_map/mymap.pgm

此处简单记录几个小技巧：

墙体可通过画笔/铅笔绘制，Shift 自动画直线
油漆桶填充工具 -> 改变前景色/背景色 -> 滴管取色 -> 矩形选择工具选择后可以大范围变色
矩形选择工具选择后 delete 变为可行区域
不能导出 PNG/JPG！必须导出 PGM！文件 → 导出为...（Export As），文件类型请选择：*.pgm，弹出 PGM 参数设置：保持默认（ASCII/RAW 都可以），不要修改分辨率、DPI
最麻烦的是裁剪，裁剪完需要对应修改 map.yaml，根据裁剪的像素进行修改，为了简化流程写了个脚本自动编写 yaml，基本原理就是 OpenCV 的模板匹配方法，因此要先处理完保存，再裁剪输出 yaml

github（以后各脚本更新升级都会放在此项目中）：https://github.com/YemuRiven/unitree-g1-scripts/tree/main/SLAM/ros-map-tools

python 复制代码

python auto_update_map_yaml.py \
    dataset/orig_map.yaml \
    dataset/orig_map.pgm \
    dataset/cropped_map.pgm \
    dataset/new_map.yaml

2.3 开启导航

修改地图路径（源项目中位置写错了）：

bash 复制代码

cd HongTu/G1Nav2D/src/fastlio2/launch/

# 可以 ls 看一下文件
# unitree@ubuntu:~/Codes/HongTu/G1Nav2D/src/fastlio2/launch$ ls
# gridmap_load.launch  mapping.launch  navigation.launch  octomap_server.launch

gedit gridmap_load.launch

修改为自己的地图路径：

python 复制代码

    <!-- 设置地图的配置文件 -->
    <arg name="2dmap_file" default="/home/unitree/Codes/HongTu/map/mymap.yaml" />

终端1启动导航：

bash 复制代码

# 启动导航，启动导航后需自行按照雷达位置重定位
cd HongTu/G1Nav2D/
source devel/setup.bash
roslaunch fastlio navigation.launch

2.4 开启运控

安装 unitree_sdk2_python 参考宇树官方文档

或者之前的笔记：G1部署（八）

终端2 G1 控制（此处新建了环境 unitree_sdk2py）：

python 复制代码

# 打开新终端，网口可通过ifconfig命令查询自行更改
cd HongTu/unitree_sdk2_python/example/g1/high_level/
python3 g1_control.py eth0

在 rviz 中发布目标点即可自主导航：