自主导航最容易写成“先定位，再规划，最后控制”。这个项目确实有这三层，但真正的工作量在三层之间的边界：什么时候允许出发，什么时候必须停，定位掉了怎么办，点云断了怎么办，到了目标点怎么才算真正对准。

navigate 是一个组合 Skill

nav.navigate 不是把所有逻辑都写在同一个类里。它通过 ROS2 Action 组合平台里已经存在的能力：

localization.pose 提供 /skill/pose。
motion.gait 负责切步态。
motion.move 和共享 MotionController 负责最终下发轴指令。
perception.freespace 提供前方障碍、负障和扇区距离。
live 点云作为避障兜底。

Skill 之间不直接调内部实现，只走 Action 和 topic。这样导航、站立、检测都暴露成同一种契约，将来 VLM 或上层编排层也用同一套接口驱动。

出发前：前置检查要可行动

invoke() 开始工作前，会先检查几件事。

定位必须有新鲜的 /skill/pose。所谓新鲜，不是“曾经有过位姿”，而是这个位姿还能反映当前状态。位姿一旦陈旧，控制回路宁可停，也不会照着旧位置继续开。

本体状态必须是站立。机器人不站立就发运动指令，轻则走不动，重则有安全风险。

里程计也会做一次体检。但这条前置有个重要取舍：它只决定“这一趟要不要出发”，不会在导航中途把正在跑的任务掐掉。因为导航中途重启里程计，会造成一段没有位姿来源的空窗。

还有一个容易忽略的帧守卫：导航当前地图必须和定位当前地图一致。如果定位用的是室内图，导航拿的是室外图，两个坐标系根本对不上，A* 规划的路径会直接撞墙。这种错误不报异常，只能靠前置显式拒绝。

两种执行模式

navigate 的 worker 线程按是否有 path 分成两个分支。

有 path 时进入路网模式：不读点云、不建 Costmap、不跑 A*，只沿着给定的中心线走。这个模式对推算态更友好，因为不需要知道全局地图坐标，只需要知道狗在这条边上的哪一段。

没有 path 时进入 A* 模式：按当前位置和目标点开一个局部窗口，读点云分片，建 Costmap，跑全局规划。这个模式要求真实位姿，推算态下会被拒绝。

PurePursuit：10Hz 的跟踪控制

PurePursuit 把折线转换成归一化轴指令 (vx, vyaw)，范围都是 [-1, 1]。

它先找到机器人在路径上的投影点，再往前找 lookahead 距离处的目标点，最后根据目标点方位角计算转向量。

控制输出有两个针对这台机器狗的重要修正。

第一是转向指令变化率限幅。位姿大约 5.6Hz，控制回路 10Hz，存在相位滞后。如果转向指令一拍拍满，会出现来回摆头的极限环。所以每拍转向指令最大变化量会被限制：

1
2
3

VYAW_SLEW = 0.12         # 每拍转向指令最大变化量(时间阻尼)
LOOKAHEAD = 1.5          # m, pure-pursuit carrot distance
ARRIVE_ARC_M = 1.2       # 到达判据的"沿路剩余弧长"上限

第二是前爬弧转。这台狗的原地自转死区很高。纯自转时，vyaw=0.5 持续 25 秒只转 5 度。给一点前进速度后，自转变成小半径弧转，实际执行效果才稳定。

转向控制还区分了“跟随阶段”和“终点对齐阶段”：

跟随阶段使用 VYAW_MIN=0.50 保证能转起来。
终点对齐阶段使用更低的 VYAW_ALIGN_MIN=0.36，因为狗在往前爬，转弯弧线能让更低转速生效。
对齐需要连续多拍都在容差内，避免单拍陈旧位姿导致“看起来对齐了就停下”。

到达判断也不只看直线距离。路网可能绕回自身，两个点直线距离是 0，沿路距离却还有几十米。所以同时检查剩余弧长，只有真正走到路径末端附近才算到。

安全证据链：宁可不走，不能盲走

导航循环里，速度不是 PurePursuit 一个人决定的，还要过几层安全判断。

前方障碍距离来自 freespace 契约。远则全速，近则减速，进入停车线就停。如果 freespace 暂时不可用，本地会用点云做一次走廊检测作为回落，但速度要打折。长时间断开就直接停，因为回落计算更贵，而且看不到台阶和负障。

视觉感知是第二条证据链。激光回答“地面能不能走”，视觉回答“那里有没有人、车”。视觉结论只能让速度更保守：如果视觉说停，即使激光没看到近处障碍，也不能用激光结果解除停车。

转向时会看转向那一侧的扇区。以前只检查正前方，结果桌子在侧前方，狗转着转着蹭上去。现在扇区会随着转向方向张开，直行时退化成窄扇区，不会因为两侧正常墙体而一步都动不了。

点云断流也必须停。实时避障依赖 live 点云，如果点云静默返回空，控制器会以为前方一路畅通。导航因此加了点云看门狗：断流先 hold，持续断流就 abort。

位姿跳变也有独立看门狗。定位可能单帧跳到十几米外，如果继续照着这个位置控制，狗会按幻觉路径猛走。导航按物理上限判断：这台狗最快也就零点几米每秒，单拍位移超过阈值且折算速度不合理，就认为位姿跳了。

区域漫游：没有地图和路网时怎么走

nav.roam 解决的是另一个问题：给定一个区域，没有先验地图，没有目标点，让狗自己到处走。

它的运动执行复用 navigate，每一段都发一个两点路径。这样避障和安全逻辑不用重写。

漫游决策抽成纯函数，只回答三件事：边界在哪、下一步往哪走、什么时候回家。

选点是刻意做得简单的随机方向加边界约束。为什么不做弓字形覆盖？因为弓字形需要精确知道自己在哪，没有 RTK 时这个前提不成立。硬做出来只会按漂移后的位置画格子。

边界有三层：

平台给意图边界，例如半径或矩形。
端侧给能力边界，按推算误差和里程预算决定还能走多远。
感知给物理边界，处理障碍和负障。

返航是“预算过半就回家”，不是“预算耗尽再停”。因为回家本身也在走路，也会继续产生漂移。如果等误差耗尽，狗就停在离家几十米的地方，等于走丢：

def should_return(*, elapsed_s, duration_s, dr, max_err_m, max_dist_m,
                  home_dist_m, stuck_rounds, max_stuck):
    """该回家了吗。返回原因（None=接着逛）。
    推算那两条按比例判而不是判耗尽:预算耗尽时停在离家 40m 处等于走丢。
    另一半预算是留给回家那段路的 —— 回家也要走、也在漂。
    """
    if duration_s > 0 and elapsed_s >= duration_s:
        return f"到时间({elapsed_s:.0f}s)"
    if stuck_rounds >= max_stuck:
        return f"连续 {stuck_rounds} 段都走不动(可能被围住了)"
    if dr and dr.get("active"):
        err = float(dr.get("est_err_m") or 0.0)
        dist = float(dr.get("dist_m") or 0.0)
        if err >= max_err_m * RETURN_ERR_FRAC:
            return (f"推算误差 {err:.2f}m 已用掉一半预算(上限 {max_err_m:.2f}m)"
                    f",留另一半回家")
        if dist >= max_dist_m * RETURN_DIST_FRAC:
            return f"推算里程 {dist:.0f}m 已用掉一半预算,留另一半回家"
        k = float((dr.get("drift") or {}).get("k_dist_m_per_m") or 0.02)
        if err + k * home_dist_m >= max_err_m:
            return (f"再走下去连家都回不到了(现在 ±{err:.2f}m,"
                    f"回家 {home_dist_m:.0f}m 还会涨 ±{k * home_dist_m:.2f}m)")
    return None

位姿刚跳变时，漫游不允许立刻返航。进入点坐标是跳变前记录的，跳变后的位姿和它不在同一个参照下。这时候最安全的动作是原地停，不是沿着一条更长的新路径去追一个已经失效的“家”。

关键代码示例

看门狗：不是“检查一次”，而是每拍重新确认安全前提

navigate 的控制循环里，位姿、点云、定位健康都在每拍重新检查：

_POSE_WARN_S = 1.0
_POSE_LOST_S = 5.0
_CLOUD_WARN_S = 1.0
_CLOUD_GIVEUP_S = 3.0
_POSE_JUMP_M = 0.35
_POSE_JUMP_SPEED = 1.2

位姿陈旧时先停，不盲走：

pose, age = self._pose_now()
if pose is None or age > _POSE_LOST_S:
    self._controller.stop()
    return self._fail(h, f"位姿丢失 {age:.1f}s...")

if age > _POSE_WARN_S:
    self._controller.stop()
    self._set(h, message=f"位姿陈旧 {age:.1f}s,暂停")

front=None 有两种含义：“看清楚、没东西”和“还没看见”。起步阶段不能把后者当前者：

if fsrc is None and front is None and front_drop is None:
    if seen_obst_src == 0.0:
        seen_obst_src = -1.0
    self._controller.stop()
    self._set(h, message="还没看清前方(障碍源尚未出数),先不动 —— "
                         "「看不见」不等于「没障碍」")
    time.sleep(period)
    continue

位姿跳变按物理上限判，而不是只信任定位自己的保护

定位可能单帧跳到十几米外，而控制回路会照这个幻觉位姿继续发指令：

jump = math.hypot(x - last_pose_xy[0], y - last_pose_xy[1])
jump_v = jump / dt_pose

if jump > _POSE_JUMP_M and jump / dt_pose > _POSE_JUMP_SPEED:
    if pose_jump_since == 0.0:
        pose_jump_since = now
    self._controller.stop()

    if now - pose_jump_since > _POSE_JUMP_GIVEUP_S:
        return self._fail(h, f"位姿在跳({jump:.1f}m/拍,狗走不了这么快)...")

这台狗最快也就零点几米每秒，一拍不可能挪那么远。用物理上限做判据，比等定位模块自己报警更直接，也能拦住 0.5m 级的小跳变。

PurePursuit 的转向变化率限幅和末端对齐

位姿约 5.6Hz，控制回路 10Hz，纯比例转向会自激摆头。变化率限幅是时间阻尼：

def _slew(self, vyaw: float) -> float:
    """转向指令变化率限幅(时间阻尼)。位姿滞后下纯比例会自激摆头。"""
    d = vyaw - self._vyaw_prev
    if d > VYAW_SLEW:
        vyaw = self._vyaw_prev + VYAW_SLEW
    elif d < -VYAW_SLEW:
        vyaw = self._vyaw_prev - VYAW_SLEW
    self._vyaw_prev = vyaw
    return vyaw

末端对齐时，前爬把原地自转变成小半径弧转；对齐增益不能拿原地死区 VYAW_MIN 当下限，否则减速会被满速吃掉：

# 绝不能拿 VYAW_MIN 当下限——它等于 VYAW_MAX,会把整段减速吃成满速砰砰控制
mag = min(VYAW_MAX, max(VYAW_ALIGN_MIN, K_ALIGN * abs(herr)))
vyaw = self._slew(YAW_CMD_SIGN * math.copysign(mag, herr))
# 前爬随误差收敛而减小:远时用弧转(平稳),近时几乎原地,免得爬出目标点
creep = ALIGN_VX * min(1.0, abs(herr) / (3 * YAW_TOL))

到达也不只看直线距离。路网中心线可能绕回自身，所以同时检查沿路剩余弧长：

def _remaining_arc(self, x: float, y: float) -> float:
    """从当前投影点沿路径到终点的剩余弧长。
    到达判据用它,避免环路上"直线到了但路没走完"。"""
    pts = self.path
    if len(pts) < 2:
        return 0.0
    i = min(self._cursor, len(pts) - 2)
    ax, ay = pts[i]
    bx, by = pts[i + 1]
    dx, dy = bx - ax, by - ay
    L2 = dx * dx + dy * dy
    t = 0.0 if L2 < 1e-9 else max(0.0, min(1.0, ((x - ax) * dx + (y - ay) * dy) / L2))
    px, py = ax + t * dx, ay + t * dy
    rem = math.hypot(bx - px, by - py)
    for j in range(i + 1, len(pts) - 1):
        rem += math.hypot(pts[j + 1][0] - pts[j][0], pts[j + 1][1] - pts[j][1])
    return rem

near_end = self._remaining_arc(x, y) < ARRIVE_ARC_M
if not self._arrived and near_end and \
        (dgoal < POS_TOL or (behind and dgoal < ARRIVE_BEHIND_R)):
    self._arrived = True