真机断连不再静默"成功"：加连接存活看门狗 + 说清"臂没跟随"

2026-09-28 真机实测：S05 精拍定位报 code=2（垂直化或悬停失败：工具轴没竖直、或悬停
高度到不了），看着像几何/标定问题。真因是 RDK 在 14:53:44 就与机器人断开了
（Disconnected from the robot, waiting for reconnection ...），此后 robot_->states()
只返回最后一帧缓存、robot_timestamp 推进量恒为 0（/joint_states 永久冻结），而
rizon_arm_controller 是本地插值、不校验硬件反馈 —— 于是控制器照报 Goal reached,
success!，臂却一步没动。

最要命的是整条链路没有一个环节报错：断连不报、写命令不报、控制器报成功，只有
"实测位移为 0"这一个真信号，而它还被归因成了"悬停高度到不了"。

- flexiv_hardware: read() 加双重看门狗 —— connected()（必须放在 operational()
  之前判：断连后 operational() 会一直保持 true）+ 状态帧时间戳停滞 500ms。
  任一失效即返回 ERROR，让 ros2_control 把栈停下来报错。
- flexiv_hardware: on_deactivate 里 robot_->Stop() 加保护 —— 看门狗正是把断连
  场景送到这里，此时 Stop() 发不出去且大概率抛异常，不能让异常抛穿
  controller_manager 的下线流程。
- stage2_locate: 实测位移为 0 时单独报一条指向"连接/关节状态"的错，不再混进
  "垂直化或悬停失败"里把人带去调标定。
- 新增 test_stage2_no_follow.py（4 例）。

验证：全量单测 1136 passed / 1 skipped；flexiv_hardware 单独编译通过。

Co-authored-by: Cursor <cursoragent@cursor.com>

src/arm_application/arm_application/stage2_locate_node.py

@@ -53,6 +53,15 @@ from arm_application.hole_detector import config_from_site, weights_from_site
 from arm_application.site_io import deep_merge, load_site_dict
 from arm_application.ros_node import TolerantNode
 
+# 一次 hop 之后实测关节变化小于这个值，就认为「臂根本没跟随指令」。
+#
+# 真机踩到过（2026-09-28）：一次 hover hop 计划要动 `max|Δq|=0.17378rad`，实测
+# `0.00000rad`，而控制器照样打印 "Goal reached, success!"。原因不在几何/对位，
+# 而是 RDK 断连后 `operational()` 仍为 true、`read()` 一直返回最后一帧缓存，
+# 关节状态永久冻结（`robot_timestamp` 推进量为 0），而控制器是本地插值、不校验反馈。
+# 真机上报成"垂直化或悬停失败"会把人往"倾角/悬停高度"上带，故单独拎出来报。
+_NO_FOLLOW_EPS_RAD = 1e-4
+
 
 def _as_bool(val: Any, default: bool = False) -> bool:
     if val is None:
@@ -718,12 +727,22 @@ class Stage2Locate(TolerantNode):
             if q2 is None or Tee2 is None:
                 self.get_logger().error("运动途中丢了 joint_states/TF")
                 return False, q, Tee
+            d_moved = float(np.max(np.abs(q2 - q)))
             self.get_logger().info(
-                f"  {tag} hop{hop + 1} 实际 max|Δq|={float(np.max(np.abs(q2 - q))):.5f}rad "
+                f"  {tag} hop{hop + 1} 实际 max|Δq|={d_moved:.5f}rad "
                 f"Δxy=({(Tee2[0, 3] - Tee[0, 3]) * 1000:+.2f},"
                 f"{(Tee2[1, 3] - Tee[1, 3]) * 1000:+.2f})mm "
                 f"Δz={float((Tee2[2, 3] - Tee[2, 3]) * 1000):+.2f}mm"
             )
+            if d_moved < _NO_FOLLOW_EPS_RAD:
+                # 轨迹发出去了、控制器也报成功，臂却一步没动 —— 别往几何/对位上找。
+                self.get_logger().error(
+                    f"  {tag} hop{hop + 1}：关节状态**毫无变化**（实测 max|Δq|={d_moved:.5f}rad，"
+                    f"指令计划见上一条 send 日志）—— 臂没有跟随指令。这**不是**倾角/悬停高度的问题，"
+                    "而是关节状态没在更新：RDK 断连后 operational() 仍为 true，read() 只会一直"
+                    "返回最后一帧缓存，而控制器是本地插值、不校验反馈，所以照样报 success。"
+                    "先查网络：ping 机器人 IP 是否丢包、两台臂是否同 MAC 抢 L2 转发。"
+                )
             q, Tee = q2, Tee2
         p = Tee[:3, 3]
         goal = np.array([p[0], p[1], target[2]]) if hold_xy else target

src/arm_application/test/test_stage2_no_follow.py

@@ -0,0 +1,168 @@
+"""臂没跟随指令时，报错必须指向**连接**，不能甩锅给几何。
+
+为什么单独钉住（2026-09-28 真机实测）
+--------------------------------------
+真机跑到 S05 精拍定位时反复失败，报的是
+``code=2（垂直化或悬停失败：工具轴没竖直、或悬停高度到不了）``，
+看着像标定/几何/悬停高度的问题。实际日志里是：
+
+    plan hover Δ=(+0,+0,-80)mm tilt=0.12deg
+    send hover: 114 pts T=4.37s ... max|Δq|=0.17378rad
+    hover hop1 实际 max|Δq|=0.00000rad Δz=+0.00mm
+    hover hop2 实际 max|Δq|=0.00000rad Δz=+0.00mm
+
+指令要求把法兰挪 0.17378 rad，实测**一动不动**。真因是 RDK 与机器人的连接在
+14:53:44 就断了（``Disconnected from the robot, waiting for reconnection``），
+此后 ``robot_->states()`` 只会返回最后一帧缓存、``robot_timestamp`` 推进量恒为 0
+（joint_states 因此永久冻结），而 ``rizon_arm_controller`` 是**本地插值**、
+不校验硬件反馈，所以照样打印 ``Goal reached, success!``。
+
+最坑的一点：整条链路**没有一个环节报错** —— 断连不报、写命令不报、控制器报成功。
+只有"实测位移为 0"这一个信号是真的。所以这个信号必须自己喊出来，并且喊对方向
+（去查网络/连接），否则下一次仍然会被"垂直化或悬停失败"带去调标定。
+
+顺带一提：``_goto`` 里那个 ``stall`` 逻辑本来就会因"连续 2 hop 无进展"而放弃，
+所以**不会**无限转下去；缺的只是**说清原因**。
+"""
+
+from __future__ import annotations
+
+from typing import Any, List
+
+import numpy as np
+
+from arm_application.stage2_locate_node import Stage2Locate
+
+_NO_FOLLOW_HINT = "臂没有跟随指令"
+_NO_FOLLOW_SIGN = "关节状态**毫无变化**"
+
+# 与真机那次同量级：悬停要往下 197mm，容差 0.7mm。
+_EE_Z = 0.444
+_TARGET_Z = 0.247
+_TOL_M = 0.0007
+_TOL_TILT = 0.01
+
+
+class _StubLogger:
+    def __init__(self, sink: List[str]) -> None:
+        self._sink = sink
+
+    def info(self, msg: str) -> None:
+        self._sink.append(msg)
+
+    def warn(self, msg: str) -> None:
+        self._sink.append(msg)
+
+    def error(self, msg: str) -> None:
+        self._sink.append(msg)
+
+
+class _StubStage2:
+    """``_goto`` 只用这几样东西；测试里不建任何 ROS 实体。
+
+    ``follows=False``：模拟"指令发了、关节状态冻住" —— ``_wait_state`` 每次都
+    回同一份 (q, Tee)，就像真机上读到的最后一帧缓存。
+    ``follows=True``：正常跟随，用于确认不会误报。
+    """
+
+    def __init__(self, follows: bool) -> None:
+        self.logs: List[str] = []
+        self.sends = 0
+        self._follows = bool(follows)
+        self._calls = 0
+
+    def _clamp_ws(self, target: Any) -> np.ndarray:
+        return np.array(target, dtype=float)
+
+    def _spin(self, _dt: float = 0.05) -> None:
+        return None
+
+    def _plan_and_send(self, *_a: Any, **_kw: Any) -> bool:
+        self.sends += 1
+        return True
+
+    def _wait_state(self, _timeout: float) -> Any:
+        self._calls += 1
+        Tee = np.eye(4)
+        Tee[0, 3] = 0.0
+        Tee[1, 3] = 0.0
+        q = np.zeros(7)
+        if self._follows:
+            # 每次调用都比上一次更靠近目标：模拟"真的在走"。
+            # 注意必须**逐次递进** —— 若每次都回同一份"动过"的状态，第二次相对
+            # 第一次就没变化了，会被判成没跟随（这正是打桩要避免的假象）。
+            Tee[2, 3] = _EE_Z - 0.03 * self._calls
+            q[1] = 0.05 * self._calls
+        else:
+            # 冻结在最后一帧缓存：每次都一样，就像真机断连后读到的那样。
+            Tee[2, 3] = _EE_Z
+        return q, Tee
+
+    def get_logger(self) -> Any:
+        return _StubLogger(self.logs)
+
+
+def _run(follows: bool) -> Any:
+    st = _StubStage2(follows)
+    Tee0 = np.eye(4)
+    Tee0[2, 3] = _EE_Z
+    ok, _q, _Tee = Stage2Locate._goto(  # type: ignore[arg-type]
+        st,
+        np.zeros(7),
+        Tee0,
+        np.array([0.0, 0.0, _TARGET_Z]),
+        False,  # verticalize：本用例不碰姿态，只看"臂动没动"
+        _TOL_M,
+        _TOL_TILT,
+        "hover",
+        0.022,
+        max_hops=2,
+        settle_s=0.0,
+        min_dur=0.0,
+    )
+    return ok, st
+
+
+def test_frozen_joint_state_is_reported_as_no_follow():
+    """实测位移为 0 → 必须出一条指向"连接/关节状态"的错，而不是只说容差没到。"""
+    ok, st = _run(follows=False)
+
+    assert st.sends == 2, "无进展会连着 hop 两次才放弃（stall 判据）"
+    assert ok is False, "臂没跟上就不该报成功"
+    assert any(_NO_FOLLOW_SIGN in m for m in st.logs), (
+        "要有一条明确说'关节状态毫无变化'的日志，否则查日志时会以为臂动过"
+    )
+    assert any(_NO_FOLLOW_HINT in m for m in st.logs), (
+        "错因必须指向'臂没有跟随指令'，不能只停在'悬停未达容差'"
+    )
+    hint = " ".join(m for m in st.logs if _NO_FOLLOW_HINT in m)
+    for kw in ("RDK", "断连", "ping", "MAC"):
+        assert kw in hint, f"提示里要给出可执行的排查方向（缺 {kw}）"
+
+
+def test_normal_follow_does_not_emit_the_no_follow_error():
+    """正常跟随时不能误报 —— 这条错一旦刷出来就会把人带去查网络。"""
+    _ok, st = _run(follows=True)
+
+    assert st.sends >= 1, "至少得真的发过轨迹"
+    assert not any(_NO_FOLLOW_HINT in m for m in st.logs), (
+        "臂动了还报'没跟随'就是误报，比不报更糟"
+    )
+
+
+def test_eps_is_small_enough_to_not_swallow_real_motion():
+    """阈值必须小于"真动了一下"的量级，否则真实的小位移会被误判成没动。"""
+    from arm_application.stage2_locate_node import _NO_FOLLOW_EPS_RAD
+
+    assert _NO_FOLLOW_EPS_RAD > 0.0
+    # 真机一次 hop 计划 0.17378rad；即使只走到 1%，也远大于阈值。
+    assert _NO_FOLLOW_EPS_RAD < 0.17378 * 0.01, (
+        "阈值取大了会把'动了一点点'误判成'完全没动'"
+    )
+
+
+def test_hint_mentions_the_controller_still_reports_success():
+    """要点破"控制器仍报 success"这个反直觉点，否则下一轮还会被日志骗。"""
+    _ok, st = _run(follows=False)
+    hint = " ".join(m for m in st.logs if _NO_FOLLOW_HINT in m)
+    assert "success" in hint, "真机现象就是'控制器报 success 但臂没动'，得写清楚"