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

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 但臂没动'，得写清楚"

src/flexiv_ros2/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp

@@ -123,6 +123,22 @@ private:
     bool position_controller_running_;
     bool velocity_controller_running_;
     bool torque_controller_running_;
+
+    // ===== 连接存活看门狗（2026-09-28 真机踩到）=====
+    //
+    // 现象：RDK 断连后 `operational()` 会**一直保持 true**，于是 read() 照跑却只
+    // 返回最后一帧缓存（关节状态永久冻结），write() 把命令发进虚空也不抛异常，
+    // 而 rizon_arm_controller 是本地插值、不校验反馈 —— 最终表现为
+    // **「控制器报 Goal reached/success、关节状态纹丝不动、臂一步没动」**。
+    //
+    // 实测：14:53:44 RDK 打印 "Disconnected from the robot, waiting for reconnection"，
+    // 此后 50 分钟里 /joint_states 的 robot_timestamp 推进量恒为 0，而 `op=1` 没变过。
+    // 根因是两台臂出厂 MAC 相同（88:88:88:88:87:88）在同一 L2 段抢转发，链路丢包 40~53%。
+    //
+    // 这里用 connected() + states().timestamp 双重判活：任一失效就返回 ERROR，
+    // 让 ros2_control 把栈停下来报错，而不是静默地"成功"。
+    int64_t last_state_stamp_ns_ = 0;   // 上一帧的机器人时间戳
+    int64_t last_state_change_ns_ = 0;  // 上一帧时间戳发生变化的本地时刻
 };
 
 } /* namespace flexiv_hardware */

src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp

@@ -9,6 +9,7 @@
 #include <algorithm>
 #include <chrono>
 #include <cmath>
+#include <cstdint>
 #include <exception>
 #include <string>
 #include <thread>
@@ -28,6 +29,12 @@ namespace {
 constexpr double kServoJointVelocity = 0.50;
 constexpr double kServoJointAcceleration = 1.00;
 
+// 状态帧停滞多久就判为「状态流已死」。
+//
+// RDK 的状态流约 1 kHz，正常丢包也不可能 500 ms 一帧都到不了，所以这个阈值
+// 留了很大余量；宁可晚 0.5 s 报错，也不要在运动中误判把栈停掉。
+constexpr int64_t kStateStaleTimeoutNs = 500'000'000LL;
+
 struct NrtSendGate
 {
     std::vector<double> last_sent;
@@ -441,7 +448,17 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_deactivate(
 {
     RCLCPP_INFO(getLogger(), "Stopping... please wait...");
 
+    // 看门狗判到断连时正是把我们送到这里，此时 Stop() 已经发不出去（RDK 不在会话里）
+    // 且大概率抛异常 —— 不能让异常抛穿 controller_manager 的下线流程。
+    if (!robot_->connected()) {
+        RCLCPP_WARN(getLogger(), "跳过 robot->Stop()：RDK 已断连（看门狗触发下线）");
+    } else {
+        try {
             robot_->Stop();
+        } catch (const std::exception& e) {
+            RCLCPP_WARN(getLogger(), "robot->Stop() 抛异常，忽略并继续下线: %s", e.what());
+        }
+    }
 
     RCLCPP_INFO(getLogger(), "System successfully stopped!");
 
@@ -451,10 +468,45 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_deactivate(
 hardware_interface::return_type FlexivHardwareInterface::read(
     const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
 {
+    // 与 write() 里同一约定：THROTTLE 宏需要一个 rclcpp::Clock 对象。
+    static rclcpp::Clock steady_clock(RCL_STEADY_TIME);
+
+    // 看门狗之一：RDK 的连接标志。
+    // 必须放在 operational() **之前**判 —— 断连后 operational() 会一直保持 true，
+    // 只看它会把"已断连"误判成"正常"，这正是当初臂不动却一路报 success 的原因。
+    if (!robot_->connected()) {
+        RCLCPP_ERROR_THROTTLE(getLogger(), steady_clock, 2000,
+            "RDK 与机器人的连接已断开 —— 关节状态只是最后一帧缓存，下发的命令不会生效。"
+            "检查网络后重启本栈（两台臂出厂 MAC 相同时会在同一 L2 段抢转发，"
+            "表现为 ping 通但丢包、RDK 时连时断）。");
+        return hardware_interface::return_type::ERROR;
+    }
+
     if (robot_->operational()) {
 
         hw_flexiv_robot_states_ = robot_->states();
 
+        // 看门狗之二：状态帧时间戳。
+        // 机器人把**自己的时钟**打在每一帧上，正常时每帧都在推进；卡住不动就说明
+        // 状态流已经停了。这一条兜住 connected() 仍乐观报 true 的情况。
+        const auto stamp = hw_flexiv_robot_states_.timestamp;
+        const int64_t stamp_ns =
+            static_cast<int64_t>(stamp.first) * 1000000000LL + static_cast<int64_t>(stamp.second);
+        const int64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+                                   std::chrono::steady_clock::now().time_since_epoch())
+                                   .count();
+        if (stamp_ns != last_state_stamp_ns_) {
+            last_state_stamp_ns_ = stamp_ns;
+            last_state_change_ns_ = now_ns;
+        } else if (last_state_change_ns_ != 0
+                   && now_ns - last_state_change_ns_ > kStateStaleTimeoutNs) {
+            RCLCPP_ERROR_THROTTLE(getLogger(), steady_clock, 2000,
+                "机器人状态帧已停滞 %.1f s —— 关节状态是陈旧值，下发命令不会生效。"
+                "检查网络/RDK 连接后重启本栈。",
+                static_cast<double>(now_ns - last_state_change_ns_) / 1e9);
+            return hardware_interface::return_type::ERROR;
+        }
+
         // Read joint states
         // Map RDK states (RDK order) to Hardware Interface states (ROS order)
         for (size_t rdk_idx = 0; rdk_idx < robot_->info().DoF; ++rdk_idx) {