问题看起来像是 SLAM 坏了
一开始，机器狗的建图和导航并不稳定。原来的 patrol 程序能够启动一些模块，但重定位和导航调用经常卡住，偶尔还会因为程序问题中止。我们最初把注意力放在 SLAM 本身，怀疑地图没有生成，或者 Mid-360 没有正常提供数据。
后来才发现，服务端和客户端需要分开看。
先确认官方服务到底能不能工作
Go2 的 SLAM 服务由宇树提供，核心进程是 unitree_slam，激光雷达使用 Livox Mid-360。官方还提供了一个 keyDemo 客户端，用来调用建图、重定位和导航接口。
我们把官方文档和 keyDemo 对照起来，先绕开自己的 patrol 程序做验证。结果很明确，官方服务端可以正常响应，keyDemo 的调用也能够返回。问题已经从“SLAM 系统不能工作”缩小成了“我们自己的导航客户端存在问题”。
手动启动服务时还有一个容易忽略的顺序。Mid-360 驱动需要先启动，等待雷达准备好以后，再启动 unitree_slam。反过来启动时，SLAM 服务可能在 YSN 检查阶段退出。
地图先保存成 PCD，再重新显示出来
SLAM 建图完成后，地图以 PCD 点云文件保存。为了在自己的 Dashboard 和 RViz 环境里观察地图，我们把 PCD 文件重新读取并发布成 PointCloud2，再在 RViz 中设置地图坐标系显示。
这样做的价值很直接。地图是不是完整，不能只看程序有没有返回成功。把点云打开以后，可以看到实验区域的范围、墙体和植物等结构，也能发现某些位置是否扫描不足。
直线导航先跑通
官方的位姿导航接口接收一个目标位姿。机器狗从当前点走向目标点，基础行为是直线运动。我们记录当前位置作为目标点，再让机器狗离开原地，最后调用导航接口返回目标位置。
这一步验证通过以后，移动平台才真正具备了“走到抓取区域附近”的基础能力。
但直线导航并不等于复杂巡航。手机 App 中能走转弯路线，依赖的是拓扑节点和边。patrol 程序里也有采集节点、保存节点和执行拓扑导航的代码，只是原来的 POSE_NAV 调用仍然存在卡住问题。
当前实验采用的办法比较朴素。在拐弯位置增加中间导航点，让机器狗逐段走直线。对于抓取实验来说，机器狗只需要从起点移动到一个合适的停靠位置，这个方案暂时够用。
卡住以后，日志比猜测有用
排查 POSE_NAV 时，日志停在任务列表已经生成的位置，后续调用没有返回。继续查看 patrol 的 slamCore 和 slamhandler，发现客户端里还涉及导航状态表、DDS 请求和任务完成判断。
其中一个问题是导航状态表在不同线程中访问，却没有明确的同步保护。另一个问题是导航任务完成状态如果没有正确更新，外层循环就会一直等待。程序看上去像是在等机器狗，实际上等的是一个没有被写回的状态。
这类问题很难靠反复重启解决。官方服务正常以后，就应该把客户端请求参数、返回值处理和状态更新拆开逐项验证。
当前结论
Go2 的 Mid-360 建图、地图保存、点云渲染和基础直线导航已经验证。patrol 程序仍然需要继续修复，复杂拓扑导航也还没有完成稳定测试。
对这次抓取实验来说，导航部分的目标可以先收窄。机器狗停到一个固定区域，视觉系统再寻找目标，已经足够支撑下一步联调。等机械臂和视觉链路稳定以后，再回头处理更复杂的巡航路线。