feat: Implement RM75 joint feedback and fault recovery design

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -202,6 +202,60 @@ def _install_fake_sdk(monkeypatch, *, push_return=0, send_feedback=True):
     return SimpleNamespace(RoboticArm=FakeArm)
 
 
+def test_joint_degree_query_returns_validated_radians() -> None:
+    class FakeArm:
+        def rm_get_joint_degree(self):
+            return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
+
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._arm = FakeArm()
+
+    snapshot = adapter.read_joint_state()
+
+    assert snapshot.positions == pytest.approx(
+        [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
+    )
+    assert snapshot.read_duration_ms is not None
+
+
+@pytest.mark.parametrize(
+    "result",
+    [
+        (7, [0.0] * 7),
+        (0, [0.0] * 6),
+        (0, [0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]),
+    ],
+)
+def test_joint_degree_query_rejects_sdk_errors_and_invalid_values(result) -> None:
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._arm = SimpleNamespace(rm_get_joint_degree=lambda: result)
+
+    with pytest.raises((RuntimeError, ValueError)):
+        adapter.read_joint_state()
+
+
+def test_mock_joint_query_uses_current_mock_positions() -> None:
+    adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
+
+    snapshot = adapter.read_joint_state()
+
+    assert snapshot.positions == pytest.approx(
+        [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
+    )
+
+
 def test_udp_feedback_is_cached_in_radians(monkeypatch) -> None:
     monotonic = iter([10.0, 10.005])
     monkeypatch.setattr(realman_adapter.time, "monotonic", lambda: next(monotonic))
@@ -253,7 +307,12 @@ def test_invalid_udp_feedback_does_not_replace_snapshot(state) -> None:
 
     adapter._on_realtime_arm_state(state)
 
-    assert adapter.get_latest_joint_state() == before
+    after = adapter.get_latest_joint_state()
+    assert after is not None
+    assert before is not None
+    assert after.positions == before.positions
+    assert after.received_at == before.received_at
+    assert after.motion_ready is False
 
 
 def test_udp_joint_fault_marks_snapshot_unready() -> None:

xr_rm_teleop/test/test_joint_control.py

@@ -30,34 +30,177 @@ class FakeTime:
         return SimpleNamespace(nanoseconds=0)
 
 
-def test_missing_or_stale_feedback_does_not_enable_qp() -> None:
+def test_startup_joint_query_initializes_qp_and_command_history() -> None:
+    positions = [0.1] * 7
+    pose = np.eye(4)
     teleop = object.__new__(SingleArmVelocityTeleop)
-    teleop._command_timeout_sec = 0.12
-    teleop._adapter = SimpleNamespace(get_latest_joint_state=lambda: None)
-
-    assert teleop._fresh_joint_state() is None
-
+    teleop._arm_name = "right_rm75"
     teleop._adapter = SimpleNamespace(
-        get_latest_joint_state=lambda: JointStateSnapshot(
-            [0.0] * 7,
-            time.monotonic() - 1.0,
+        read_joint_state=lambda: JointStateSnapshot(
+            positions,
+            time.monotonic(),
         )
     )
-    assert teleop._fresh_joint_state() is None
+    teleop._ik_solver = SimpleNamespace(
+        update_joint_state=lambda joints: pose
+    )
+    teleop.get_logger = lambda: FakeLogger()
+
+    teleop._initialize_joint_state()
+
+    assert teleop._latest_joint_positions == positions
+    assert teleop._last_valid_joint_target == positions
+    assert teleop._last_joint_command_target == positions
+    assert teleop._last_joint_command_velocity == [0.0] * 7
+    assert teleop._last_current_pose is pose
 
 
-def test_disabled_joint_feedback_does_not_enable_qp() -> None:
+def test_startup_joint_query_failure_closes_adapter() -> None:
+    class FailingAdapter:
+        def __init__(self):
+            self.close_calls = 0
+
+        def read_joint_state(self):
+            raise RuntimeError("rm_get_joint_degree failed with code 7")
+
+        def close(self):
+            self.close_calls += 1
+
+    errors = []
     teleop = object.__new__(SingleArmVelocityTeleop)
-    teleop._command_timeout_sec = 0.12
-    teleop._adapter = SimpleNamespace(
-        get_latest_joint_state=lambda: JointStateSnapshot(
+    teleop._arm_name = "left_rm75"
+    teleop._adapter = FailingAdapter()
+    teleop.get_logger = lambda: SimpleNamespace(
+        error=lambda message: errors.append(message)
+    )
+
+    with pytest.raises(RuntimeError, match="code 7"):
+        teleop._initialize_joint_state()
+
+    assert teleop._adapter.close_calls == 1
+    assert "left_rm75" in errors[0]
+    assert "启动关节同步失败" in errors[0]
+
+
+def _timeout_teleop(adapter) -> SingleArmVelocityTeleop:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._adapter = adapter
+    teleop._arm_name = "right_rm75"
+    teleop._follow = False
+    teleop._active = True
+    teleop._joint_feedback_ready = True
+    teleop._grip_rearm_required = False
+    teleop._feedback_resync_attempted = False
+    teleop._control_fault_latched = False
+    teleop._last_joint_command_target = [0.1] * 7
+    teleop._last_joint_command_velocity = [0.0] * 7
+    teleop._latest_joint_positions = [0.1] * 7
+    teleop._last_valid_joint_target = [0.1] * 7
+    teleop._last_current_pose = np.eye(4)
+    teleop._controller_start = None
+    teleop._controller_orientation_start = None
+    teleop._robot_start_transform = None
+    teleop._filtered_target = None
+    teleop._filtered_orientation_target = None
+    teleop._last_sent_target = None
+    teleop._last_sent_orientation = None
+    teleop._last_command_time = None
+    teleop._ik_solver = SimpleNamespace(
+        update_joint_state=lambda joints: np.eye(4)
+    )
+    teleop._stop_sent = False
+    teleop._feedback_resync_timeout_sec = 0.5
+    teleop._publish_stop_debug = lambda: None
+    teleop.get_logger = lambda: FakeLogger()
+    return teleop
+
+
+def test_missing_or_disabled_joint_snapshot_is_not_motion_ready() -> None:
+    assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(None)
+    assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(
+        JointStateSnapshot(
             [0.0] * 7,
             time.monotonic(),
             motion_ready=False,
         )
     )
 
-    assert teleop._fresh_joint_state() is None
+
+def test_short_udp_timeout_repeats_last_limited_target_without_query() -> None:
+    sends = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: sends.append(
+            (list(joints), follow)
+        ),
+        read_joint_state=lambda: pytest.fail("query must not run"),
+        stop=lambda: pytest.fail("stop must not run"),
+    )
+    teleop = _timeout_teleop(adapter)
+
+    teleop._handle_stale_joint_feedback(0.2)
+
+    assert sends == [([0.1] * 7, False)]
+    assert teleop._last_joint_command_target == [0.1] * 7
+    assert teleop._grip_rearm_required
+
+
+def test_short_udp_timeout_without_active_target_stays_stopped() -> None:
+    stop_calls = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: pytest.fail(
+            "inactive control must not start CANFD output"
+        ),
+        read_joint_state=lambda: pytest.fail("query must not run"),
+        stop=lambda: stop_calls.append(True),
+    )
+    teleop = _timeout_teleop(adapter)
+    teleop._active = False
+
+    teleop._handle_stale_joint_feedback(0.2)
+
+    assert len(stop_calls) == 1
+
+
+def test_persistent_udp_timeout_queries_once_and_holds_actual_position() -> None:
+    sends = []
+    query_calls = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: sends.append(list(joints)),
+        read_joint_state=lambda: (
+            query_calls.append(True)
+            or JointStateSnapshot([0.2] * 7, time.monotonic())
+        ),
+        stop=lambda: None,
+    )
+    teleop = _timeout_teleop(adapter)
+
+    teleop._handle_stale_joint_feedback(0.5)
+    teleop._handle_stale_joint_feedback(0.6)
+
+    assert len(query_calls) == 1
+    assert sends == [[0.2] * 7, [0.2] * 7]
+    assert teleop._last_valid_joint_target == [0.2] * 7
+    assert teleop._last_joint_command_velocity == [0.0] * 7
+
+
+def test_persistent_udp_timeout_query_failure_latches_control() -> None:
+    stop_calls = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: pytest.fail(
+            "CANFD must stop after query failure"
+        ),
+        read_joint_state=lambda: (_ for _ in ()).throw(
+            RuntimeError("rm_get_joint_degree failed with code 7")
+        ),
+        stop=lambda: stop_calls.append(True),
+    )
+    teleop = _timeout_teleop(adapter)
+
+    teleop._handle_stale_joint_feedback(0.5)
+    teleop._handle_stale_joint_feedback(0.6)
+
+    assert teleop._control_fault_latched
+    assert len(stop_calls) == 1
 
 
 def test_joint_command_step_limits_acceleration_from_rest() -> None:
@@ -106,6 +249,8 @@ def test_feedback_fault_blocks_grip_until_release() -> None:
         update_joint_state=lambda joints: np.eye(4)
     )
     teleop._grip_rearm_required = True
+    teleop._control_fault_latched = False
+    teleop._feedback_resync_attempted = False
     teleop.get_clock = lambda: FakeClock()
     teleop.get_logger = lambda: FakeLogger()
     stopped = []
@@ -125,40 +270,6 @@ def test_feedback_fault_blocks_grip_until_release() -> None:
     assert len(entered) == 1
 
 
-def test_stale_feedback_stops_before_active_control() -> None:
-    stopped = []
-    entered = []
-    teleop = object.__new__(SingleArmVelocityTeleop)
-    teleop._adapter = SimpleNamespace(
-        get_latest_joint_state=lambda: JointStateSnapshot(
-            [0.0] * 7,
-            time.monotonic() - 1.0,
-        )
-    )
-    teleop._command_timeout_sec = 0.12
-    teleop._joint_feedback_ready = True
-    teleop._arm_name = "right_rm75"
-    teleop._last_msg = SimpleNamespace(
-        grip=True,
-        pose=SimpleNamespace(
-            position=SimpleNamespace(x=0.0, y=0.0, z=0.0),
-            orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0),
-        ),
-    )
-    teleop._last_msg_time = FakeTime()
-    teleop._active = False
-    teleop._enable_orientation_control = False
-    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
-    teleop.get_logger = lambda: FakeLogger()
-    teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
-    teleop._enter_active_control = lambda *args: entered.append(args)
-
-    teleop._control_tick()
-
-    assert stopped == [True]
-    assert entered == []
-
-
 def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
     class FakeSolver:
         def __init__(self) -> None:
@@ -318,34 +429,58 @@ def test_timing_stats_logs_summary_and_clears_window() -> None:
     assert all(not samples for samples in teleop._timing_samples.values())
 
 
-def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None:
-    class FailingAdapter:
-        def __init__(self) -> None:
+def test_canfd_error_stops_queries_and_requires_grip_rearm() -> None:
+    class RecoveringAdapter:
+        def __init__(self):
             self.stop_calls = 0
+            self.read_calls = 0
 
         def send_joint_target(self, joints, follow):
             del joints, follow
-            raise RuntimeError("send failed")
+            raise RuntimeError("rm_movej_canfd failed with code 9")
 
         def stop(self):
             self.stop_calls += 1
 
-    reset_calls = []
-    teleop = object.__new__(SingleArmVelocityTeleop)
-    teleop._adapter = FailingAdapter()
-    teleop._follow = False
-    teleop._arm_name = "left_rm75"
-    teleop._stop_sent = False
-    teleop._last_joint_command_target = [0.0] * 7
-    teleop._last_joint_command_velocity = [0.0] * 7
+        def read_joint_state(self):
+            self.read_calls += 1
+            return JointStateSnapshot([0.2] * 7, time.monotonic())
+
+    teleop = _timeout_teleop(RecoveringAdapter())
     teleop._joint_command_max_speed = math.radians(180.0)
     teleop._joint_command_max_acceleration = math.radians(300.0)
-    teleop._dt = 1.0 / 125.0
-    teleop.get_logger = lambda: FakeLogger()
-    teleop._safe_stop = lambda reset_active: reset_calls.append(reset_active)
+    teleop._dt = 1.0 / 90.0
 
-    sent = teleop._send_joint_target([0.1] * 7)
+    sent = teleop._send_joint_target([0.3] * 7)
 
     assert not sent
     assert teleop._adapter.stop_calls == 1
-    assert reset_calls == [True]
+    assert teleop._adapter.read_calls == 1
+    assert not teleop._control_fault_latched
+    assert teleop._grip_rearm_required
+    assert teleop._last_joint_command_target == [0.2] * 7
+
+
+def test_canfd_error_latches_when_joint_query_also_fails() -> None:
+    class FailingAdapter:
+        def __init__(self):
+            self.stop_calls = 0
+
+        def send_joint_target(self, joints, follow):
+            del joints, follow
+            raise RuntimeError("rm_movej_canfd failed with code 9")
+
+        def stop(self):
+            self.stop_calls += 1
+
+        def read_joint_state(self):
+            raise RuntimeError("rm_get_joint_degree failed with code 7")
+
+    teleop = _timeout_teleop(FailingAdapter())
+    teleop._joint_command_max_speed = math.radians(180.0)
+    teleop._joint_command_max_acceleration = math.radians(300.0)
+    teleop._dt = 1.0 / 90.0
+
+    assert not teleop._send_joint_target([0.3] * 7)
+    assert teleop._control_fault_latched
+    assert teleop._adapter.stop_calls == 1

xr_rm_teleop/test/test_orientation_control.py

@@ -179,6 +179,8 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
     teleop._last_valid_joint_target = None
     teleop._last_current_pose = None
     teleop._joint_feedback_ready = True
+    teleop._control_fault_latched = False
+    teleop._feedback_resync_attempted = False
     stopped = []
     teleop.get_clock = lambda: FakeClock()
     teleop.get_logger = lambda: FakeLogger()

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -59,6 +59,9 @@ class MockRealManAdapter:
             time.monotonic(),
         )
 
+    def read_joint_state(self) -> JointStateSnapshot:
+        return self.get_latest_joint_state()
+
     def send_joint_target(self, joints: list[float], follow: bool) -> None:
         del follow
         if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
@@ -228,6 +231,25 @@ class RealManAdapter:
                 self._latest_joint_state.motion_ready,
             )
 
+    def read_joint_state(self) -> JointStateSnapshot:
+        self._require_arm()
+        started_at = time.monotonic()
+        result = self._arm.rm_get_joint_degree()
+        finished_at = time.monotonic()
+        if not isinstance(result, tuple) or len(result) != 2:
+            raise RuntimeError(
+                f"rm_get_joint_degree returned invalid result: {result!r}"
+            )
+        self._check_return(result, "rm_get_joint_degree")
+        return JointStateSnapshot(
+            self._joint_positions_from_degrees(
+                result[1],
+                "rm_get_joint_degree",
+            ),
+            finished_at,
+            (finished_at - started_at) * 1000.0,
+        )
+
     def send_joint_target(self, joints: list[float], follow: bool) -> None:
         self._require_arm()
         if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
@@ -305,17 +327,10 @@ class RealManAdapter:
                 raise ValueError(
                     f"unexpected realtime feedback source: {arm_ip}"
                 )
-            degrees = list(data.joint_status.joint_position)
-            if (
-                len(degrees) != 7
-                or not all(isinstance(value, Number) for value in degrees)
-            ):
-                raise ValueError(
-                    "RM75 UDP feedback must contain 7 numeric joints"
-                )
-            positions = [math.radians(float(value)) for value in degrees]
-            if not all(math.isfinite(value) for value in positions):
-                raise ValueError("RM75 UDP feedback contains NaN/Inf")
+            positions = self._joint_positions_from_degrees(
+                data.joint_status.joint_position,
+                "RM75 UDP feedback",
+            )
             joint_enabled = [
                 bool(value) for value in data.joint_status.joint_en_flag
             ]
@@ -367,12 +382,44 @@ class RealManAdapter:
             self._feedback_fault_logged = False
             self._feedback_ready.set()
         except Exception as exc:
+            with self._joint_state_lock:
+                if self._latest_joint_state is not None:
+                    current = self._latest_joint_state
+                    self._latest_joint_state = JointStateSnapshot(
+                        list(current.positions),
+                        current.received_at,
+                        current.read_duration_ms,
+                        current.update_interval_ms,
+                        False,
+                    )
             if not self._feedback_fault_logged:
                 self._log_warn(
                     f"RealMan UDP realtime feedback invalid: {exc}"
                 )
                 self._feedback_fault_logged = True
 
+    @staticmethod
+    def _joint_positions_from_degrees(
+        values: Any,
+        source: str,
+    ) -> list[float]:
+        try:
+            degrees = list(values)
+        except TypeError as exc:
+            raise ValueError(
+                f"{source} must contain 7 numeric joints"
+            ) from exc
+        if len(degrees) != 7 or not all(
+            isinstance(value, Number) for value in degrees
+        ):
+            raise ValueError(
+                f"{source} must contain 7 numeric joints"
+            )
+        positions = [math.radians(float(value)) for value in degrees]
+        if not all(math.isfinite(value) for value in positions):
+            raise ValueError(f"{source} contains NaN/Inf")
+        return positions
+
     def _log_motion_status_transition(
         self,
         status: tuple[Any, ...],

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -177,8 +177,9 @@ class SingleArmVelocityTeleop(Node):
 
         self.declare_parameter("arm_name", "rm75")
         self.declare_parameter("controller_topic", "/xr/right_controller")
-        self.declare_parameter("control_rate_hz", 125.0)
+        self.declare_parameter("control_rate_hz", 90.0)
         self.declare_parameter("command_timeout_sec", 0.12)
+        self.declare_parameter("feedback_resync_timeout_sec", 0.5)
         self.declare_parameter("scale", 1.0)
         self.declare_parameter("deadband_m", 0.001)
         self.declare_parameter("target_filter_alpha", 0.65)
@@ -234,6 +235,9 @@ class SingleArmVelocityTeleop(Node):
             raise ValueError("control_rate_hz must be > 0")
         self._dt = 1.0 / control_rate_hz
         self._command_timeout_sec = float(self.get_parameter("command_timeout_sec").value)
+        self._feedback_resync_timeout_sec = float(
+            self.get_parameter("feedback_resync_timeout_sec").value
+        )
         self._scale = float(self.get_parameter("scale").value)
         self._deadband_m = float(self.get_parameter("deadband_m").value)
         self._target_filter_alpha = float(self.get_parameter("target_filter_alpha").value)
@@ -287,6 +291,8 @@ class SingleArmVelocityTeleop(Node):
         self._last_joint_command_velocity: list[float] | None = None
         self._joint_feedback_ready = False
         self._grip_rearm_required = False
+        self._feedback_resync_attempted = False
+        self._control_fault_latched = False
         self._stop_sent = True
         self._trigger_tool_open = True
         self._last_trigger_pressed: bool | None = None
@@ -321,6 +327,7 @@ class SingleArmVelocityTeleop(Node):
         )
         self._adapter = self._make_adapter()
         self._adapter.connect()
+        self._initialize_joint_state()
         self._setup_tool_control()
 
         debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
@@ -374,6 +381,31 @@ class SingleArmVelocityTeleop(Node):
             canfd_radio=int(self.get_parameter("canfd_radio").value),
         )
 
+    def _initialize_joint_state(self) -> None:
+        try:
+            self._reset_joint_state(self._adapter.read_joint_state())
+        except Exception as exc:
+            self.get_logger().error(
+                f"{self._arm_name} 启动关节同步失败：{exc}"
+            )
+            self._adapter.close()
+            raise
+
+    def _reset_joint_state(
+        self,
+        snapshot: JointStateSnapshot,
+    ) -> np.ndarray:
+        current_pose = self._ik_solver.update_joint_state(
+            snapshot.positions
+        )
+        positions = list(snapshot.positions)
+        self._latest_joint_positions = positions
+        self._last_current_pose = current_pose
+        self._last_valid_joint_target = list(positions)
+        self._last_joint_command_target = list(positions)
+        self._last_joint_command_velocity = [0.0] * 7
+        return current_pose
+
     def _setup_tool_control(self) -> None:
         peripheral_arm = self._peripheral_arm_name()
         if self._bool_parameter("configure_peripheral_on_connect"):
@@ -515,17 +547,32 @@ class SingleArmVelocityTeleop(Node):
             else (tick_started_ns - last_tick_started_ns) * 1e-6
         )
         now = self.get_clock().now()
-        snapshot = self._fresh_joint_state()
-        if snapshot is None:
+        if self._control_fault_latched:
+            return
+
+        snapshot = self._adapter.get_latest_joint_state()
+        if not self._joint_snapshot_is_motion_ready(snapshot):
             self._grip_rearm_required = True
             if self._joint_feedback_ready:
                 self.get_logger().warn(
-                    f"{self._arm_name} 关节反馈缺失、过期或机械臂未就绪，机械臂停止。",
+                    f"{self._arm_name} 关节反馈无效或机械臂未就绪，机械臂停止。",
                     throttle_duration_sec=1.0,
                 )
             self._joint_feedback_ready = False
             self._safe_stop(reset_active=True)
             return
+        assert snapshot is not None
+        feedback_age = time.monotonic() - snapshot.received_at
+        if feedback_age < 0.0:
+            self._grip_rearm_required = True
+            self._joint_feedback_ready = False
+            self._safe_stop(reset_active=True)
+            return
+        if feedback_age > self._command_timeout_sec:
+            self._handle_stale_joint_feedback(feedback_age)
+            return
+
+        self._feedback_resync_attempted = False
         try:
             current_pose = self._sync_joint_feedback(snapshot)
         except Exception as exc:
@@ -538,9 +585,17 @@ class SingleArmVelocityTeleop(Node):
             self._safe_stop(reset_active=True)
             return
         if not self._joint_feedback_ready:
-            self.get_logger().info(
-                f"{self._arm_name} 已收到首帧有效关节反馈，QP 可以启用。"
-            )
+            if self._grip_rearm_required:
+                message = (
+                    f"{self._arm_name} UDP关节反馈已恢复，"
+                    "等待Grip松开后重新使能。"
+                )
+            else:
+                message = (
+                    f"{self._arm_name} 已收到首帧有效关节反馈，"
+                    "QP可以启用。"
+                )
+            self.get_logger().info(message)
             self._joint_feedback_ready = True
 
         if self._last_msg is None or self._last_msg_time is None:
@@ -592,7 +647,7 @@ class SingleArmVelocityTeleop(Node):
             )
             return
 
-        feedback_age_ms = (time.monotonic() - snapshot.received_at) * 1000.0
+        feedback_age_ms = feedback_age * 1000.0
         assert self._controller_start is not None
         assert self._robot_start_transform is not None
 
@@ -978,20 +1033,102 @@ class SingleArmVelocityTeleop(Node):
             samples.clear()
         self.get_logger().info(message)
 
-    def _fresh_joint_state(self) -> JointStateSnapshot | None:
-        snapshot = self._adapter.get_latest_joint_state()
-        if snapshot is None:
-            return None
-        age = time.monotonic() - snapshot.received_at
-        if age < 0.0 or age > self._command_timeout_sec:
-            return None
+    def _handle_stale_joint_feedback(self, age: float) -> None:
+        if self._control_fault_latched:
+            return
+        self._grip_rearm_required = True
+        if self._joint_feedback_ready:
+            self.get_logger().warn(
+                f"{self._arm_name} UDP关节反馈超时，保持最后安全目标。"
+            )
+        self._joint_feedback_ready = False
+
         if (
-            len(snapshot.positions) != 7
-            or not all(math.isfinite(value) for value in snapshot.positions)
-            or not snapshot.motion_ready
+            age >= self._feedback_resync_timeout_sec
+            and not self._feedback_resync_attempted
         ):
-            return None
-        return snapshot
+            self._feedback_resync_attempted = True
+            self.get_logger().warn(
+                f"{self._arm_name} UDP关节反馈持续超时，"
+                "尝试rm_get_joint_degree重新同步。"
+            )
+            try:
+                self._reset_joint_state(
+                    self._adapter.read_joint_state()
+                )
+            except Exception as exc:
+                self._latch_control_fault(
+                    f"UDP关节反馈持续超时且重新同步失败：{exc}"
+                )
+                return
+            self.get_logger().info(
+                f"{self._arm_name} 已通过rm_get_joint_degree重新同步，"
+                "继续保持并等待UDP恢复。"
+            )
+
+        if self._active and self._last_joint_command_target is not None:
+            self._repeat_last_joint_target()
+        else:
+            self._safe_stop(reset_active=True)
+
+    def _repeat_last_joint_target(self) -> None:
+        target = self._last_joint_command_target
+        if target is None:
+            return
+        try:
+            self._adapter.send_joint_target(list(target), self._follow)
+            self._stop_sent = False
+        except Exception as exc:
+            self._recover_from_canfd_error(exc)
+
+    def _recover_from_canfd_error(
+        self,
+        send_error: Exception,
+    ) -> None:
+        self.get_logger().error(
+            f"{self._arm_name} rm_movej_canfd发送失败：{send_error}"
+        )
+        self._grip_rearm_required = True
+        self._send_stop_once()
+        self._safe_stop(reset_active=True)
+        try:
+            self._reset_joint_state(
+                self._adapter.read_joint_state()
+            )
+        except Exception as query_error:
+            self._latch_control_fault(
+                "CANFD错误后关节同步失败："
+                f"send={send_error}; query={query_error}"
+            )
+            return
+        self.get_logger().info(
+            f"{self._arm_name} CANFD错误后已同步实际关节角，"
+            "等待UDP恢复及Grip重新使能。"
+        )
+
+    def _latch_control_fault(self, message: str) -> None:
+        if self._control_fault_latched:
+            return
+        self._control_fault_latched = True
+        self._grip_rearm_required = True
+        self.get_logger().error(
+            f"{self._arm_name} 控制故障已锁存：{message}"
+        )
+        self._safe_stop(reset_active=True)
+
+    @staticmethod
+    def _joint_snapshot_is_motion_ready(
+        snapshot: JointStateSnapshot | None,
+    ) -> bool:
+        return (
+            snapshot is not None
+            and len(snapshot.positions) == 7
+            and all(
+                math.isfinite(value)
+                for value in snapshot.positions
+            )
+            and snapshot.motion_ready
+        )
 
     def _sync_joint_feedback(
         self,
@@ -1086,12 +1223,7 @@ class SingleArmVelocityTeleop(Node):
         try:
             self._adapter.send_joint_target(limited_target, self._follow)
         except Exception as exc:
-            self.get_logger().error(
-                f"{self._arm_name} 发送关节透传命令失败：{exc}",
-                throttle_duration_sec=1.0,
-            )
-            self._send_stop_once()
-            self._safe_stop(reset_active=True)
+            self._recover_from_canfd_error(exc)
             return False
         self._last_joint_command_target = limited_target
         self._last_joint_command_velocity = limited_velocity
@@ -1205,6 +1337,11 @@ class SingleArmVelocityTeleop(Node):
     def _validate_parameters(self) -> None:
         if self._command_timeout_sec <= 0.0:
             raise ValueError("command_timeout_sec must be > 0")
+        if self._feedback_resync_timeout_sec <= self._command_timeout_sec:
+            raise ValueError(
+                "feedback_resync_timeout_sec must be greater than "
+                "command_timeout_sec"
+            )
         if self._deadband_m < 0.0:
             raise ValueError("deadband_m must be >= 0")
         if not 0.0 <= self._target_filter_alpha <= 1.0: