Files
Go2_navigation/README.md

24 KiB
Raw Permalink Blame History

Go2 Navigation

基于 Unitree Go2、Livox LiDAR、ROS/ROS2、FAST-LIO 和 Unitree SDK 的导航与建图资料仓库。仓库把驱动、点云里程计、传统 ROS1 建图、Unitree SLAM 示例、机器人 ROS2 消息包、C++ SDK、网页控制台和 LiDAR 外参标定工具集中在一起,便于在真实 Go2 与 Livox 设备上进行建图、定位、导航和调试。

仓库状态:这是一个多项目集合,不是可以用一条命令构建的单一 ROS workspace。不同目录对应不同上游项目、ROS 发行版和运行方式;请先选择目标模块,再按本文对应章节操作。

目录

项目组成

路径 内容 主要入口 运行环境
Livox-SDK2/ Livox HAP、Mid-360 等设备的 C/C++ SDK、示例和通信配置 CMakeLists.txt、samples/ Linux/Windows,C++11,CMake 3.0+
ws_livox/src/livox_ros_driver2/ Livox ROS Driver 2,发布标准点云或 CustomMsg build.sh、launch_ROS2/ ROS1 或 ROS2 Foxy/Humble/Jazzy
fast_lio_ws/src/FAST_LIO_ROS2/ FAST-LIO2 ROS2 激光惯性里程计和 PCD 建图 launch/mapping.launch.py Ubuntu 20.04+,ROS2,PCL,Eigen
livox_mapping/ Livox 的 ROS1 LOAM 低速建图包 launch/mapping_*.launch Ubuntu 16.04/18.04,ROS Kinetic/Melodic
Livox_automatic_calibration-master/ 双 Livox 雷达点云外参自动标定 mapping、calibration、fitline CMake,PCL 1.7,Eigen
unitree_ros2/ Unitree Go2/B2/H1/G1 的 ROS2 消息包和示例 cyclonedds_ws/、example/ ROS2 Foxy 或 Humble,CycloneDDS
unitree_sdk2/ Unitree C++ SDK2 及 Go2、G1、H1 等示例 CMakeLists.txt、example/ Ubuntu 20.04,GCC 9.4,C++17
unitree_slam/ Unitree SLAM 二进制、配置、动态库和 keyDemo 示例 example/src/keyDemo.cpp、bin/keyDemo ARM64 Unitree 设备或匹配的交叉环境
unitree_ui/unitree_ui-main/ 基于 TypeScript、Three.js、Vite 的 WebRTC 控制台 package.json、start.sh Node.js 18+,npm 9+,Chrome
go2_maps/ 本地 Go2 地图和初始位姿文件(默认被 Git 忽略) .yaml、.pgm、.pcd Unitree SLAM/导航数据
go2-environment-setup-handoff.md 另一套自主巡检仓库的环境交接记录 文档本身 仅作历史参考

仓库同时保留部分第三方源码、预编译库、构建目录和示例资源。3rdparty/、build/、node_modules/ 等内容不需要重新整理到本 README;使用某个模块时,以该模块的 README.md、CMakeLists.txt、package.xml 或 package.json 为准。

系统要求

建议在 Ubuntu 20.04/22.04 上运行。ROS1 建图和旧版标定工具依赖旧发行版;FAST-LIO 与 Livox ROS Driver 2 建议使用 ROS2 Humble。

组件 建议版本 用途
CMake 3.10+(SDK2 最低 3.0) C/C++ 模块构建
GCC/G++ 9.4+ SDK2、ROS2 示例和 FAST-LIO
ROS2 Humble(Foxy 也有脚本) Livox Driver 2、FAST-LIO、Unitree ROS2
ROS1 Kinetic/Melodic livox_mapping 和部分旧标定流程
PCL 1.8+;标定工具要求 1.7 点云处理
Eigen 3.3.4+ 状态估计与几何计算
Node.js/npm Node.js 18+,npm 9+ Unitree WebRTC UI
Python Python 3 UI BLE 服务和启动脚本
浏览器 Chrome WebRTC 控制台

先检查本机工具链:

gcc --version
cmake --version
python3 --version
node --version
source /opt/ros/humble/setup.bash
ros2 --version

ROS2 包构建前建议安装依赖:

sudo apt update
sudo apt install -y build-essential cmake libeigen3-dev libpcl-dev \
  python3-colcon-common-extensions python3-rosdep
rosdep update

Unitree ROS2 的官方依赖还包括 rmw-cyclonedds-cpp、rosidl-generator-dds-idl 和 libyaml-cpp-dev。不同 ROS 发行版的包名要把 foxy 替换成实际发行版。

目录结构

Go2_navigation/
├── Livox-SDK2/                         # Livox SDK2 与 C++ samples
├── Livox_automatic_calibration-master/ # 双雷达外参自动标定
├── fast_lio_ws/src/FAST_LIO_ROS2/      # FAST-LIO2 ROS2
├── livox_mapping/                      # Livox ROS1 LOAM 建图
├── ws_livox/src/livox_ros_driver2/     # Livox ROS Driver 2
├── unitree_ros2/                       # Unitree ROS2 消息与示例
├── unitree_sdk2/                       # Unitree SDK2
├── unitree_slam/                       # Unitree SLAM 二进制与配置
├── unitree_ui/unitree_ui-main/         # WebRTC/Three.js 控制台
├── go2_maps/                           # 本地地图资产
└── go2-environment-setup-handoff.md    # 历史环境交接

网络与设备

Unitree Go2

Unitree ROS2 文档以有线网卡连接为例:电脑网卡配置为 192.168.123.99/24,机器人通常位于同一网段。把 unitree_ros2/setup.sh 中的 enp3s0 改为实际网卡名,然后加载环境:

cd unitree_ros2
source setup.sh
ros2 topic list

仅本机仿真或回环测试可使用:

source unitree_ros2/setup_local.sh   # CycloneDDS 使用 lo
# 或
source unitree_ros2/setup_default.sh # 不指定网卡

注意:仓库中的这三个脚本仍然使用 /opt/ros/foxy 和 $HOME/unitree_ros2。如果仓库不在 $HOME/unitree_ros2,需要先修改脚本中的路径;使用 Humble 时也要将 Foxy 路径改为 Humble。

Livox LiDAR

Livox Driver 2 的 MID360 示例使用独立的设备网段。配置文件示例使用 192.168.1.x,控制、推送、点云、IMU、日志端口分别为 56100–56500(主机端通常为 56101–56501)。把 host_ip、LiDAR IP 和网卡地址改成现场实际值,确保防火墙允许这些 UDP 端口。

Go2 内置 SLAM 的配置则使用机器人 DDS 网络和 rt/unitree/... 话题。不要把 Livox 独立网段的配置直接套到 Unitree DDS 网卡上。

快速开始

下面是一条适用于“外置 Livox + ROS2 FAST-LIO”的最短流程。每个命令在独立终端执行。

1. 构建 Livox SDK2

cd Livox-SDK2
cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
cmake --build build -j$(nproc)
sudo cmake --install build

SDK 默认安装库到 /usr/local/lib、头文件到 /usr/local/include。如果不希望安装到系统目录,请为 Driver 2 和后续 CMake 工程显式设置 CMAKE_PREFIX_PATH 或库搜索路径。

2. 构建并启动 Livox ROS Driver 2

source /opt/ros/humble/setup.bash
cd ws_livox/src/livox_ros_driver2
bash build.sh humble
source ../../install/setup.bash
ros2 launch livox_ros_driver2 msg_MID360_launch.py

单雷达使用 MID360_config.json,多雷达使用 msg_MID360s_launch.py 和 MID360s_config.json。FAST-LIO 使用 Livox 每点时间戳时,必须发布 CustomMsg 格式;不要使用不能提供每点时间的旧 livox_lidar.launch 变体。

3. 构建并启动 FAST-LIO2

source /opt/ros/humble/setup.bash
cd fast_lio_ws
rosdep install --from-paths src --ignore-src -r -y
colcon build --symlink-install
source install/setup.bash
ros2 launch fast_lio mapping.launch.py config_file:=mid360.yaml

如果使用 Avia、Horizon、Velodyne 或 Ouster,把 config_file 换成 avia.yaml、horizon.yaml、velodyne.yaml 或 ouster64.yaml。先启动 Driver,再启动 FAST-LIO;检查 /livox/lidar、/livox/imu 是否有数据。

保存 PCD:

ros2 service call /map_save std_srvs/srv/Trigger "{}"

保存路径由对应 YAML 的 map_file_path 和 pcd_save.pcd_save_en 决定。默认 interval: -1 会将所有帧写入一个文件,长时间运行可能占用大量内存。

4. 启动网页控制台

cd unitree_ui/unitree_ui-main
npm install
npm run start:no-ble

浏览器打开 Vite 输出的地址(通常是 http://localhost:5173)。需要机器人 BLE 配网或 BLE 遥控器时使用:

npm run start

该命令会启动 Vite 和 FastAPI BLE 服务(端口 5051)。依赖来自 server/requirements.txt,包括 FastAPI、Uvicorn、Bleak、PyCryptodome、pygatt 和 websockets。

各模块构建与运行

Livox SDK2

SDK2 提供 C 风格 API,覆盖 HAP、Mid-360 等 Livox 设备。源码中包含快速启动、日志、信息查询、IP 设置、时间同步、多雷达升级和调试点云等 sample:

cd Livox-SDK2/build
./samples/livox_lidar_quick_start/livox_lidar_quick_start \
  ../samples/livox_lidar_quick_start/<config.json>

日志配置包括 lidar_log_enable、lidar_log_cache_size_MB 和 lidar_log_path。升级固件会让设备停止工作,必须使用与设备匹配的固件文件并在现场操作前确认网络稳定。

Livox ROS Driver 2

ROS2 launch 文件位于 ws_livox/src/livox_ros_driver2/launch_ROS2/:

Launch 用途
msg_HAP_launch.py HAP,自定义消息
msg_MID360_launch.py 单 Mid-360,自定义消息
msg_MID360s_launch.py 多 Mid-360,自定义消息
rviz_HAP_launch.py、rviz_MID360_launch.py 启动驱动并打开 RViz
rviz_mixed.py 混合型号可视化

需要修改的参数位于 launch 文件开头和 config/:xfer_format、multi_topic、publish_freq、frame_id、user_config_path 以及设备 IP。配置文件使用 JSON,不能包含注释或 ... 占位符。

FAST-LIO2 ROS2

FAST-LIO 将 LiDAR 和 IMU 通过紧耦合迭代扩展卡尔曼滤波器融合,并使用 ikd-Tree 增量维护地图。配置重点:

参数 说明
common.lid_topic / imu_topic 输入话题,Mid-360 默认 /livox/lidar、/livox/imu
preprocess.lidar_type Livox=1、Velodyne=2、Ouster=3、通用点云=4
mapping.extrinsic_T/R LiDAR 在 IMU body frame 中的外参
mapping.extrinsic_est_en 是否在线估计外参;已有可靠外参时建议关闭
publish.* 点云、地图、路径和 body-frame 输出开关
pcd_save.* PCD 保存开关、间隔和输出路径

输出话题和服务由 src/laserMapping.cpp 实现,包含 /cloud_registered、/cloud_registered_body、/cloud_effected、/Laser_map、/Odometry、/path 和 /map_save。

Livox ROS1 LOAM 建图

livox_mapping 是旧 ROS1 catkin 包,依赖 livox_ros_driver、PCL、Eigen 和 OpenCV:

cd <catkin_ws>/src
ln -s /path/to/Go2_navigation/livox_mapping livox_mapping
cd ..
catkin_make
source devel/setup.bash
roslaunch livox_mapping mapping_mid.launch

Horizon 使用 mapping_horizon.launch,室外示例使用 mapping_outdoor.launch。需要保存地图时,在 launch 文件中设置 map_file_path。

Livox 双雷达外参自动标定

工具会先用基准雷达建立地图,再将目标雷达逐帧配准,最后用 fitline 拟合外参矩阵:

cd Livox_automatic_calibration-master
cmake -S . -B build
cmake --build build -j$(nproc)
cp run.sh build/
cd build
bash run.sh

运行前准备:

data/Base_LiDAR_Frames/100000.pcd       # 基准雷达,帧号递增
data/Target-LiDAR-Frames/100000.pcd     # 目标雷达,时间同步
data/Init_Matrix.txt                    # 粗配准初值

程序会生成 data/H-LiDAR-Map-data/H_LiDAR_Map.pcd、T_Matrix.txt 和 calib_data.txt 等中间结果。两路数据必须尽可能同步,采集时应缓慢运动、减少动态物体,并保证初始矩阵大致对齐。

Unitree ROS2 消息与示例

消息包包括:

  • unitree_api:请求、响应和租约消息。
  • unitree_go:Go2/B2 的低层、高层、IMU、BMS、LiDAR、遥控器和运动消息。
  • unitree_hg:G1/H1 等人形机器人的电机、手部和主板消息。

构建:

source /opt/ros/humble/setup.bash
cd unitree_ros2/cyclonedds_ws
colcon build --symlink-install
source install/setup.bash

cd ../example
colcon build --symlink-install
source install/setup.bash

常用示例:

ros2 run unitree_ros2_example read_motion_state
ros2 run unitree_ros2_example read_low_state
ros2 run unitree_ros2_example read_wireless_controller
ros2 run unitree_ros2_example go2_sport_client

Go2 高层运动通过 /api/sport/request 请求响应接口,低层控制通过 /lowcmd 发布 unitree_go/msg/LowCmd。低层控制会直接驱动关节,必须先确认机器人处于安全状态并准备急停。

Unitree SDK2

SDK2 的 CMake 工程按 CPU 架构从 lib/<CMAKE_SYSTEM_PROCESSOR>/ 查找预编译库:

cmake -S unitree_sdk2 -B unitree_sdk2/build -DCMAKE_BUILD_TYPE=Release
cmake --build unitree_sdk2/build -j$(nproc)

安装到系统或自定义前缀:

cmake --install unitree_sdk2/build --prefix /opt/unitree_robotics

自定义前缀需要加入 CMAKE_PREFIX_PATH。unitree_sdk2/example/ 中按 Go2、G1、H1、B2、B2W、H2 等型号提供高级控制、低级控制、状态读取、无线遥控和 JSON 示例。

Unitree SLAM 示例

unitree_slam/bin/ 内已有 ARM64 二进制和配套动态库;源码示例依赖已安装的 unitree_sdk2:

cmake -S unitree_slam/example -B /tmp/unitree_slam_example-build
cmake --build /tmp/unitree_slam_example-build -j$(nproc)
./unitree_slam/bin/keyDemo eth0

keyDemo 的按键流程:

按键 操作
q / w 开始 / 结束建图
a 开始重定位
s / d / f 添加位姿、执行任务、清空任务
z / x 暂停 / 恢复导航
其他键 停止 SLAM

示例源码将地图写入机器人 /home/unitree/test.pcd,重定位也从该路径读取。现场使用时应修改为机器人上实际存在的 PCD 文件,并检查 unitree_slam/config/ 中的网卡名、LiDAR 型号、DDS XML 路径和 rt/unitree/... 话题。

Unitree WebRTC UI

网页控制台支持 Go2、G1 和 R1,主要能力包括:实时视频和 3D 视图、双摇杆、运动模式、状态面板、错误解码、服务管理、账号、BLE 配网、BLE 遥控转发、Go2 3D LiDAR SLAM、地图缓存和自动充电。

生产构建:

cd unitree_ui/unitree_ui-main
npm run build
npm run preview

详细协议说明在 docs/,其中 connection.md、control.md、status.md、slam.md、lidar.md 和 remote-control.md 是排查连接与控制问题的首选文档。

配置说明

Livox MID360 JSON

ws_livox/src/livox_ros_driver2/config/MID360_config_backup.json 是完整的单雷达示例。当前 MID360_config.json 含有 ... 占位符,不是合法 JSON;在启动前必须补齐 lidar_configs 并通过解析检查:

python3 -m json.tool ws_livox/src/livox_ros_driver2/config/MID360_config_backup.json >/dev/null

关键字段包括 lidar_net_info、host_net_info、LiDAR IP、点云数据类型、扫描模式和外参。不要把示例 IP 直接用于现场设备。

FAST-LIO YAML

fast_lio_ws/src/FAST_LIO_ROS2/config/ 提供 avia.yaml、horizon.yaml、mid360.yaml、velodyne.yaml 和 ouster64.yaml。修改输入话题、扫描线、盲区、时间单位、外参、视场角和 PCD 输出路径后再启动。

FAST-LIO 的外参定义是 LiDAR 在 IMU body frame 中的位姿,不是 IMU 在 LiDAR frame 中的逆变换;方向弄反会导致点云扭曲和里程计发散。

Unitree SLAM YAML/JSON

unitree_slam/config/ 里包含 Mid-360、XT16、建图、重定位、规划、栅格地图、充电和 DDS 配置。以下内容通常必须按设备修改:

  • network 或 network_interface:实际以太网卡。
  • lidar_ysn、lidar_ip:LiDAR 序列号和地址。
  • odom_topic、imu_topic、lidar_topic:机器人实际 DDS 话题。
  • dds_config.json 与 dds_parameter.json 中的绝对路径。
  • 建图/重定位的 PCD、PGM、YAML 文件路径。

仓库中的部分配置仍保留 /home/unitree/...、/home/unitree/graph_pid_ws/... 等设备端历史路径,迁移到新机器前必须替换。

Go2 地图资产

go2_maps/ 中的地图通常由以下文件组成:

<name>.pcd                 # 3D 点云地图
<name>.pgm                 # 2D 占据栅格图
<name>.yaml                # 分辨率、原点、阈值和 PGM 引用
<name>.initial-pose        # 初始位姿
<name>.map-manifest.json   # 校准 ID、哈希、点数和版本

示例 YAML 使用 resolution: 0.05、mode: trinary、occupied_thresh: 0.65、free_thresh: 0.25。地图目录和 mapping log 默认被 .gitignore 忽略,发布或备份前请单独归档。

话题、服务与数据流

Livox LiDAR
    └─ livox_ros_driver2
        ├─ /livox/lidar + /livox/imu (CustomMsg / sensor_msgs)
        └─ fast_lio_mapping
            ├─ /cloud_registered, /Laser_map
            ├─ /Odometry, /path
            └─ /map_save

Unitree Go2 DDS
    ├─ /sportmodestate, /lowstate, /wirelesscontroller
    ├─ /api/sport/request, /lowcmd
    └─ rt/unitree/slam_*

Browser
    └─ WebRTC data channel/video + BLE FastAPI proxy

常用检查命令:

ros2 topic list
ros2 topic hz /livox/lidar
ros2 topic echo --once /livox/imu
ros2 service list | grep map_save
ros2 topic echo --once /Odometry

Unitree UI 的 SLAM 页面使用 rt/uslam/client_command 发送 mapping/...、localization/...、navigation/... 和 patrol/... 字符串命令,状态从 rt/uslam/server_log、点云、里程计和路径话题返回。地图文件通过 WebRTC 内部文件传输通道分块传输,并在浏览器 IndexedDB 中缓存。

地图与标定数据

建议采用以下现场流程:

  1. 固定 LiDAR 和 IMU,记录安装方向、坐标系和网卡地址。
  2. 用 Livox Driver 检查点云与 IMU 时间戳;FAST-LIO 要求每点时间可用。
  3. 采集缓慢、闭环且包含丰富几何特征的数据,避免人车等动态物体。
  4. 先用已知外参或机械测量值运行 FAST-LIO,保存 PCD 并检查重影、扭曲和闭环误差。
  5. 双雷达场景使用 Livox_automatic_calibration-master,保证两路 PCD 文件名和时间戳一一对应。
  6. 把确认后的外参写入 FAST-LIO YAML 或设备配置,再重新建图验证。
  7. Go2 官方 SLAM 地图同时保存 PCD、PGM、YAML、初始位姿和 manifest,不要只保留单个 PCD。

外参验证至少采用一种独立方法:重叠视场观察墙面是否错层,或把目标雷达变换后与基准点云融合再重新建图。若出现分层,优先检查时间同步、坐标方向、初值和地图质量。

验证清单

构建前

  • ROS 发行版、PCL、Eigen 和编译器版本符合目标模块。
  • 设备网卡、IP、LiDAR 序列号和端口已核对。
  • MID360_config.json 等 JSON 通过解析检查,没有 ... 占位符。
  • FAST-LIO 的 lid_topic、imu_topic 和外参已确认。
  • Unitree 配置中的绝对路径已替换为本机或机器人实际路径。

运行时

  • ros2 topic list 能看到 Livox 和 Unitree 话题。
  • /livox/lidar 与 /livox/imu 频率稳定,时间戳递增。
  • FAST-LIO 的 /Odometry 连续更新,点云无明显扭曲。
  • /map_save 成功生成 PCD,且文件可用 pcl_viewer 打开。
  • 控制机器人前已确认站立模式、急停和通信链路。

软件检查

# Shell 语法
bash -n ws_livox/src/livox_ros_driver2/build.sh
bash -n unitree_ui/unitree_ui-main/start.sh

# UI TypeScript/Vite 构建
cd unitree_ui/unitree_ui-main
npm run build

# ROS2 包索引(已 source 对应环境)
cd ../../fast_lio_ws
colcon list

当前仓库没有统一的根目录测试命令;C++/ROS 包主要通过构建、话题、服务和真实设备链路验证。go2-environment-setup-handoff.md 中提到的 robot/、dependencies/、Cargo workspace 和导航测试属于另一套 /home/limerly/go2_autonomous_inspection 仓库,不能在本仓库直接执行。

故障排查

现象 优先检查
liblivox_lidar_sdk_shared.so 找不到 SDK2 是否安装到 /usr/local/lib;运行 sudo ldconfig 或设置 LD_LIBRARY_PATH
Driver 能启动但没有点云 LiDAR/主机 IP、UDP 端口、防火墙、host_net_info 和网卡是否一致
FAST-LIO 报 Failed to find match for field 'time' 驱动是否发布 CustomMsg;rosbag 是否保留每点时间字段
FAST-LIO 发散或点云扭曲 IMU/LiDAR 时间同步、外参方向、time_sync_en 和 time_offset_lidar_to_imu
/map_save 返回 Map save disabled YAML 中 pcd_save.pcd_save_en 是否为 true,map_file_path 是否可写
Unitree ROS2 看不到话题 网卡名、RMW_IMPLEMENTATION=rmw_cyclonedds_cpp、DDS 配置和机器人网段
setup.sh 找不到环境 脚本中的 /opt/ros/foxy 或 $HOME/unitree_ros2 是否与实际安装一致
SDK2 CMake 找不到库 unitree_sdk2/lib/<架构>/ 是否有当前 CPU 的静态库,或 SDK 是否已安装
keyDemo 无法连接 运行参数必须是连接机器人且位于 123 网段的网卡,例如 eth0
UI BLE 服务启动失败 pip3 install -r server/requirements.txt,确认 websockets >= 11、蓝牙适配器和权限
UI 能开但无法连接机器人 Chrome、机器人 WebRTC 网络模式、STA-L/AP/Remote 配置和固件兼容性
地图重定位失败 PCD/YAML/PGM 是否成套,初始位姿坐标系、地图版本和 DDS 话题是否一致

文档索引

仓库级

Livox 与 FAST-LIO

Unitree

WebRTC UI

许可证

各子目录保留各自上游许可证,不能用一个根目录许可证替代:

  • Livox SDK2:Livox-SDK2/LICENSE.txt
  • Livox ROS Driver 2:ws_livox/src/livox_ros_driver2/LICENSE.txt
  • Livox Mapping:livox_mapping/LICENSE
  • Livox 自动标定:Livox_automatic_calibration-master/LICENSE
  • FAST-LIO ROS2:fast_lio_ws/src/FAST_LIO_ROS2/LICENSE(GPLv2)
  • Unitree ROS2:unitree_ros2/LICENSE(BSD 3-Clause)
  • Unitree SDK2:unitree_sdk2/LICENSE(BSD 3-Clause)
  • Unitree UI:unitree_ui/unitree_ui-main/LICENSE(MIT)

发布二进制、固件、地图或修改后的第三方源码时,请同时保留对应版权和许可证文本。