这是一个面向小型室内环境的三维建图与导航 baseline。它直接使用宇树 G1 官方雷达服务发布的 Mid-360 点云和 IMU，不启动另一套雷达驱动；FAST-LIO 负责建图和实时激光惯性里程计，PCL + OctoMap 把保存的 PCD 转为 Nav2 静态占据地图，Nav2 负责全局规划和局部实时避障。建图仍可使用 Viser，导航使用 Canvas + rosbridge 网页。
Mid-360 官方服务（eth0 / DDS）
├─ /utlidar/cloud_livox_mid360 ─┐
└─ /utlidar/imu_livox_mid360 ───┴─> FAST-LIO
├─ /cloud_registered（当前注册帧）
├─ /Laser_map（持续增长的全局地图）
├─ /Odometry（雷达 IMU 位姿）、/path
└─ maps/fastlio_map.pcd（完整地图）
rt/lowstate（eth0 / Unitree SDK2）──> 官方 mode=6 URDF 腰部链
└─ /Odometry_base（平面导航 base_nav）
/cloud_registered + /Laser_map + /Odometry_base
└─> Viser 0.0.0.0:8080（远程主机经 wlan0 访问）
- 系统：Ubuntu 20.04 / ROS2 Foxy / aarch64。
- 雷达：Livox Mid-360，使用宇树官方 LiDAR service DDS 数据。
- DDS 内部链路：
eth0，CycloneDDS 配置默认为/home/unitree/cyclonedds_ws/cyclonedds_eth0.xml。 - 网页链路：
wlan0，Viser 监听0.0.0.0:8080。当前机器的 Wi-Fi 地址为http://172.16.12.245:8080，DHCP 变化时以启动脚本打印的地址为准。 - 输入有效距离：
0.5–10.0 m。preprocess.max_range=10.0会真实丢弃 10 m 外点，mapping.det_range=10.0同时控制 FAST-LIO 局部地图移动阈值。 - Mid-360 官方 PointCloud2 字段：
x/y/z/intensity/ring/time；time是单帧内纳秒偏移，已转换为 FAST-LIO 使用的毫秒。 - 最终
fastlio_map.pcd与/Laser_map使用同一份持续增长的实时建图层；Viser 的 0.10 m 体素和 25 万点上限只影响网页预览，不影响最终 PCD。 - 世界坐标为
map：+Z由启动静止阶段的重力确定，+X由官方 Mid360 安装姿态确定为启动时机器人腰部朝前方向。 - 本机实测
mode_machine=6，自动选择官方g1_29dof_lock_waist_rev_1_0.urdf；腰 yaw 使用LowState.motor_state[12].q，roll/pitch 按官方固定关节锁为 0。
参数文件在 fast_lio_mid360_indoor.yaml 和 viser.yaml。extrinsic_T=[-0.011,-0.02329,0.04412] 是宇树 LiDAR 文档所给“雷达系中的内置 IMU 位移”的逆变换，不是猜测的机身安装值。
本机采用下面这条动态链。base_link 仍是腰部 pelvis 的真实三维姿态；不伪造腿部、足端或固定 pelvis 高度。二维 AMCL/Nav2 另外使用同原点、仅保留 yaw 的 base_nav，防止人形机器人站立和行走时的 roll/pitch 污染 map→odom：
map → odom → base_nav（yaw-only）→ base_link (= pelvis，保留 roll/pitch) → 官方 URDF → MID-360
└─ waist_yaw_joint q = motor_state[12].q
└─ waist_roll_joint fixed, xyz=(-0.0039635, 0, 0.044)
└─ waist_pitch_joint fixed
└─ torso_link
└─ mid360_link / livox_frame
xyz=(0.0002835, 0.00003, 0.428434)
rpy=(pi, 0.0511206938, 0)
└─ livox_imu xyz=(0.011, 0.02329, -0.04412)
FAST-LIO 仍只用 Mid360 点云与雷达内置 IMU 做紧耦合估计；关节数据不进入滤波器，只在每帧用上面的官方链把 /Odometry 的 livox_imu 位姿换算成腰部 /Odometry_base，同时发布对应 TF 和 /joint_states。SDK2 与 ROS 2 Foxy 自带的 CycloneDDS ABI 不同，因此 g1_lowstate_udp_bridge 单独在 eth0 读取 rt/lowstate，只通过本机回环 UDP 把关节角交给 ROS 节点。
首次构建：
cd /home/unitree/uinitree_fast_lio_nav2
./build.sh开始建图：
./run_mapping.sh开始导航：
./run_navigation.sh浏览器打开脚本打印的 http://<G1-Wi-Fi-IP>:8080；rosbridge 默认监听 ws://<G1-Wi-Fi-IP>:9090。
导航链路如下：
所选 PCD ──PCL 地面 RANSAC + OctoMap 投影──> map.pgm/map.yaml ──> /map
/cloud_registered_body ──base_nav + 地面相对结构高度切片（8 m）──> /scan_localization
/map + /scan_localization + FAST-LIO odom ──Nav2 AMCL（omnidirectional）──> map→odom TF
/map ──static_layer + inflation_layer──> Global Costmap ──NavFn──> 全局路径
/cloud_registered_body ──base_nav + PCL 实时地面/本体过滤──> /cloud_obstacles
└─ pointcloud_to_laserscan ──> /scan
└─ obstacle_layer + inflation_layer ──DWB──> Local Costmap / 局部避障
Nav2 /cmd_vel ──安全门 + 限速 + 300 ms watchdog──> Unitree SDK2 G1 LocoClient
网页严格执行以下顺序，未完成的按钮不可用：
- 选择
maps/下保存的 PCD，设置地面/障碍高度后生成并加载精细二维地图。 - 在 Canvas 上按住并拖拽，指定机器人初始位置和朝向；等待官方 Nav2 AMCL 输出稳定。
- 人工确认机器人图标与实际环境一致，再开启 G1 运动安全门。
- 在 Canvas 上拖拽目标位置和朝向并发送，Nav2 同时执行全局路径与局部避障。
- “取消/急停”会取消 action 并立即关闭运动安全门；点云、里程计或导航中的定位超过 1.5 s 未更新也会自动关闭。AMCL 位姿跳变或协方差连续超限同样会取消 action 并关闭运动门。
首次启用真机控制时，先用宇树官方遥控器/应用让 G1 进入可行走的高层站立平衡模式，并由一人手持实体急停；本项目不会自动执行 StandUp 或切换 FSM。启动导航、加载地图和重定位后，先用可移动障碍验证 Local Costmap 能在约 1 秒内出现并消失，再点击“启用 G1 运动安全门”。第一次只发送前方 0.5–1.0 m 的短目标，随时准备按网页“停止/取消导航”和实体急停。运动门打开只允许 /cmd_vel 进入 SDK2，机器人实际速度仍由限速和 300 ms watchdog 约束。
默认地图是 cf.pcd，转换结果位于 maps/generated/cf。当前自动分析结果为：原始范围 x=[-8.651,19.220] m、y=[-10.332,12.255] m、z=[-2.372,5.091] m，地面平面在原点约为 z=-1.272 m；精细地图为 964×788、0.03 m/cell，障碍轮廓半径 0.04 m、地面补洞半径 0.15 m。PGM 使用白色可通行、黑色占据、灰色未知，未知格不会被 Nav2 当成自由空间；机器人安全距离仍由 Nav2 inflation layer 独立处理。
需要在真机低速测试前确认的参数：
- PCD 障碍高度切片：地面以上
0.15–1.80 m，地面带宽0.15 m。 - 实时障碍物按每帧 PCL RANSAC 地面平面计算，与离线地图统一保留地面以上
0.15–1.80 m，并过滤 pelvis 周围0.60 m的机器人本体区域；不再使用会把倾斜地面误认为障碍物的固定 Z 切片。Local Costmap 是机器人周围8×8 m的滚动窗口：实时/scan最远4.0 m，3.5 m内标障碍、4.0 m内射线清障；inf_is_valid=true使移走物体后的无回波射线能够删除旧障碍，Local Costmap 与网页位图均按最高约8 Hz更新，远距离结构只来自静态 Global Costmap。 - G1 足迹：约
0.62×0.50 m，全局/局部膨胀半径0.65 m；手臂姿态变化后应按最大包络调整。 - AMCL 是二维定位，不需要填写 pelvis 高度；pelvis 的真实三维高度和姿态继续由 FAST-LIO 与官方 URDF 在
base_nav→base_link链中提供。AMCL、Nav2、Costmap 和两路 LaserScan 统一使用 yaw-onlybase_nav。定位与避障共享实时 PCL RANSAC 地面相对高度过滤，避免上坡地面被固定 Z 切片误认为墙体；/scan_localization保留 8 m 内结构点供 AMCL 持续匹配，/scan限制在 4 m 内供 Local Costmap 实时标记和清除障碍。Canvas 默认显示后者，并同时报告 8 m 定位扫描点数和静态地图命中率。 - 最大命令：
vx=0.35 m/s、vy=0.20 m/s、yaw=0.50 rad/s；首次测试应在围挡区域进一步降速。
导航与 AMCL 参数在 nav2_foxy_g1.yaml，完整启动文件是 g1_navigation.launch.py。切换 PCD 时 map_server 会装载新 YAML，随后启动一份全新的、未修改的官方 Foxy AMCL 二进制，避免旧粒子和 Foxy lifecycle 重载缺陷进入新地图。初始位姿后会持续调用官方 no-motion update 做扫描匹配，满足至少 5 次输出、近期位姿稳定、协方差和扫描点质量全部合格后才显示 READY；30 秒仍不收敛则拒绝开放运动门。Canvas 导航页提供 RViz 风格的 Displays 图层树、米制网格、平移/滚轮缩放、2D 初始位姿、Nav2 Goal、Fixed Frame/位姿/雷达状态栏，以及独立局部雷达窗；拖拽初始位姿时，实时扫描会同步叠加到静态地图上辅助匹配墙角和走廊。
宇树 LiDAR DDS 消息采用设备/发布端时间，本机实测其绝对时间约比 ROS 主机慢 7.4–7.5 s，但点云与内置 IMU 保留共同的源时间供 FAST-LIO 做帧内去畸变和紧耦合融合。导航不能直接使用这条旧时间链，因此本项目只在 FAST-LIO 输出边界把 /Odometry_base、odom→base_nav、/cloud_nav 和后续两路 LaserScan 重打为 ROS 当前时间；不会改写 FAST-LIO 输入消息。/Odometry_base.twist 由连续 yaw-only 导航位姿差分并低通生成，不再向 DWB 固定报告零速度。
单台 Mid-360 与其内置 IMU不需要为了 FAST-LIO 先增加 GPS 时钟盒。若以后需要多雷达、多相机或绝对 UTC 的亚毫秒硬同步，Mid-360 官方协议支持 IEEE 1588v2 PTP/gPTP，也支持 GPS/PPS；这属于传感器硬同步的升级项，不能代替 IMU 零偏、外参、重力初始化和振动隔离标定。
脚本会强制 ROS2/CycloneDDS 在 eth0 接收宇树数据，同时打印 Wi-Fi Viser URL。在同一 Wi-Fi 局域网内的远程电脑浏览器打开该 URL。端口已经监听所有 IPv4 接口；如果系统以后启用 UFW，还需允许 8080/tcp。
可覆盖启动参数，例如：
./run_mapping.sh viser_port:=8081 map_output:=/home/unitree/maps/room.pcdViser 不拼接点云，也不修改 FAST-LIO 位姿：
- 黄色“当前帧”对应 RViz 的
/cloud_registered。 - 彩色“全局地图”对应 RViz 的
/Laser_map，在建图过程中持续扩展。 - 机器人坐标轴和轨迹来自腰部
/Odometry_base。 - 实时网页层以 2 Hz 渲染，使用 0.10 m 体素并限制在 25 万点；这些限制只节省 Jetson 到远程浏览器之间的 Wi-Fi/WebSocket 带宽。
- 点击“保存实时建图层并渐进显示最终 PCD”会把
/Laser_map的完整底层点云保存下来，随后每批加载 5 万点，直到同一页面显示最终地图的全部点。 - 点击“返回实时全局地图”可重新查看正在增长的轻量实时层。
停止建图时按一次 Ctrl-C。FAST-LIO 也会把完整地图写到：
/home/unitree/uinitree_fast_lio_nav2/maps/fastlio_map.pcd
也可从命令行在线保存：
source /opt/ros/foxy/setup.bash
source install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=/home/unitree/cyclonedds_ws/cyclonedds_eth0.xml
ros2 service call /map_save std_srvs/srv/Trigger '{}'默认不会再把每一个 10 Hz 注册帧重复追加到最终 PCD。若科研时确实需要保留逐帧高密度档案，可将 pcd_save.registered_frames_archive 改为 true；此模式会快速增加内存和文件大小，不用于 Viser 的常规最终地图。
如果 FAST-LIO 没有输出，先确认宇树官方服务：
source /opt/ros/foxy/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI=/home/unitree/cyclonedds_ws/cyclonedds_eth0.xml
ros2 topic list -t | grep utlidar
ros2 topic hz /utlidar/cloud_livox_mid360
ros2 topic hz /utlidar/imu_livox_mid360正常情况下，Mid-360 点云约 10 Hz，IMU 约 200 Hz。网页打不开时检查 ip -4 addr show wlan0、远程主机是否在同一 Wi-Fi，以及 ss -ltn | grep 8080。
当前 baseline 使用 Mid-360 点云和雷达内置 IMU，由 FAST-LIO 输出 /Odometry，再经官方腰部/Mid360 URDF 链生成 /Odometry_base。腰关节只用于刚体坐标换算，不作为 FAST-LIO 观测；腿部、足端接触和足端里程计暂不参与。
FAST-LIO 本身是紧耦合激光惯性里程计和增量 scan-to-map，不包含回环检测或全局位姿图优化。因此重访区域可能仍有累计漂移。若后续需要回环，应在保持本 baseline 前端的基础上另接回环/图优化后端，并让 Viser显示后端校正后的地图；当前实现不会用网页端的累计逻辑伪装成闭环。
- Ericsii/FAST_LIO_ROS2，ROS2 分支，支持 Foxy 和 ARM。
- 宇树 G1 LiDAR service interface。
- 宇树官方 G1 URDF（unitree_ros），本机使用
mode_machine=6对应的锁腰 Rev 1.0 文件。 - 宇树 unitree_sdk2，用于读取
rt/lowstate。 - Viser。
- ROS Navigation 2，本机使用官方 Foxy
0.4.7二进制包。 - PointCloud Library 与 OctoMap，保持系统上游算法不变，只由本项目的离线转换器组合调用。
- 官方 Nav2
nav2_amcl，使用 PCD 投影地图与实时/scan_localization发布map→odom；旧0.1.0-foxyPCD/NDT 兼容源码仍保留在工作区，但导航 launch 不再启动它。 - ros-perception/pointcloud_to_laserscan，将当前点云投影成 Nav2 obstacle layer 的
/scan。 - RobotWebTools/rosbridge_suite，Canvas 前端通过 WebSocket 订阅地图、代价地图、路径和状态。
src/livox_ros_driver2 仅保留为该 ROS2 FAST-LIO 上游源码的 CustomMsg 编译依赖。本 baseline 的 launch 不会启动 Livox driver，运行数据来自宇树官方服务。