feat: Implement absolute scheduling for feedback loop and enhance tool frame configuration

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -2,9 +2,10 @@ import math
 
 import pytest
 
+from xr_rm_teleop import realman_adapter
 from xr_rm_teleop.realman_adapter import RealManAdapter
 from xr_rm_teleop.realman_adapter import MockRealManAdapter
-from xr_rm_teleop.fun_peripheral import PeripheralConfig
+from xr_rm_teleop.fun_peripheral import PeripheralConfig, _configure_tool_frame
 
 
 def test_initial_pose_uses_joint_move_only() -> None:
@@ -38,21 +39,152 @@ def test_peripheral_config_exposes_selected_tool() -> None:
     assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
 
 
-def test_joint_feedback_is_cached_in_radians() -> None:
+@pytest.mark.parametrize(
+    ("existing", "expected_operation"),
+    [(False, "create"), (True, "update")],
+)
+def test_tool_frame_is_created_or_updated(existing, expected_operation) -> None:
+    class FakeArm:
+        def __init__(self) -> None:
+            self.calls = []
+
+        def rm_get_total_tool_frame(self):
+            self.calls.append(("get",))
+            names = ["omnipic"] if existing else []
+            return {"return_code": 0, "tool_names": names}
+
+        def rm_set_manual_tool_frame(self, *, frame):
+            self.calls.append(("create", frame))
+            return 0
+
+        def rm_update_tool_frame(self, *, frame):
+            self.calls.append(("update", frame))
+            return 0
+
+        def rm_change_tool_frame(self, tool_name):
+            self.calls.append(("change", tool_name))
+            return 0
+
+    arm = FakeArm()
+    frame = object()
+
+    _configure_tool_frame(arm, frame, "omnipic")
+
+    assert arm.calls == [
+        ("get",),
+        (expected_operation, frame),
+        ("change", "omnipic"),
+    ]
+
+
+@pytest.mark.parametrize(
+    ("existing", "failure", "operation"),
+    [
+        (False, "query", "rm_get_total_tool_frame"),
+        (False, "create", "rm_set_manual_tool_frame"),
+        (True, "update", "rm_update_tool_frame"),
+        (False, "change", "rm_change_tool_frame"),
+    ],
+)
+def test_tool_frame_sdk_failures_are_reported(existing, failure, operation) -> None:
+    class FakeArm:
+        def rm_get_total_tool_frame(self):
+            names = ["omnipic"] if existing else []
+            return {
+                "return_code": 1 if failure == "query" else 0,
+                "tool_names": names,
+            }
+
+        def rm_set_manual_tool_frame(self, *, frame):
+            del frame
+            return 1 if failure == "create" else 0
+
+        def rm_update_tool_frame(self, *, frame):
+            del frame
+            return 1 if failure == "update" else 0
+
+        def rm_change_tool_frame(self, tool_name):
+            del tool_name
+            return 1 if failure == "change" else 0
+
+    with pytest.raises(RuntimeError, match=operation):
+        _configure_tool_frame(FakeArm(), object(), "omnipic")
+
+
+def test_joint_feedback_is_cached_in_radians(monkeypatch) -> 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]
 
+    perf_counter_ns = iter(
+        [1_000_000_000, 1_002_000_000, 2_000_000_000, 2_003_000_000]
+    )
+    monotonic = iter([10.0, 10.011])
+    monkeypatch.setattr(
+        realman_adapter,
+        "time",
+        type(
+            "FakeTime",
+            (),
+            {
+                "perf_counter_ns": staticmethod(lambda: next(perf_counter_ns)),
+                "monotonic": staticmethod(lambda: next(monotonic)),
+            },
+        ),
+    )
     adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
     adapter._arm = FakeArm()
 
     adapter._read_joint_state_once()
-    snapshot = adapter.get_latest_joint_state()
+    first = adapter.get_latest_joint_state()
+    adapter._read_joint_state_once()
+    second = adapter.get_latest_joint_state()
 
-    assert snapshot is not None
-    assert snapshot.positions == pytest.approx(
+    assert first is not None
+    assert first.positions == pytest.approx(
         [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
     )
+    assert first.read_duration_ms == pytest.approx(2.0)
+    assert first.update_interval_ms is None
+    assert second is not None
+    assert second.read_duration_ms == pytest.approx(3.0)
+    assert second.update_interval_ms == pytest.approx(11.0)
+
+
+def test_feedback_loop_uses_absolute_schedule_without_catch_up(monkeypatch) -> None:
+    class FakeStopEvent:
+        def __init__(self) -> None:
+            self.checks = 0
+            self.waits = []
+
+        def is_set(self):
+            self.checks += 1
+            return self.checks > 3
+
+        def wait(self, timeout):
+            self.waits.append(timeout)
+            return False
+
+    monotonic = iter([0.0, 0.005, 0.018, 0.018, 0.023])
+    monkeypatch.setattr(
+        realman_adapter,
+        "time",
+        type(
+            "FakeTime",
+            (),
+            {"monotonic": staticmethod(lambda: next(monotonic))},
+        ),
+    )
+    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.008)
+    stop_event = FakeStopEvent()
+    reads = []
+    adapter._feedback_stop = stop_event
+    adapter._read_joint_state_once = lambda: reads.append(None)
+
+    adapter._feedback_loop()
+
+    assert len(reads) == 3
+    assert stop_event.waits == pytest.approx([0.003, 0.003])
 
 
 def test_joint_target_uses_movej_canfd_in_degrees() -> None:

xr_rm_teleop/test/test_joint_control.py

@@ -194,33 +194,47 @@ def test_timing_stats_logs_summary_and_clears_window() -> None:
     teleop = object.__new__(SingleArmVelocityTeleop)
     teleop._arm_name = "right_rm75"
     teleop._dt = 0.008
-    teleop._timing_stats_window = 2
+    teleop._timing_stats_window = 3
     teleop._timing_samples = {
         name: []
-        for name in ("period", "total", "qp", "send", "feedback_age")
+        for name in (
+            "period",
+            "total",
+            "qp",
+            "send",
+            "feedback_age",
+            "feedback_read",
+            "feedback_interval",
+        )
     }
+    teleop._last_timing_feedback_received_at = None
     teleop.get_logger = lambda: SimpleNamespace(
         info=lambda message: messages.append(message)
     )
+    first_feedback = JointStateSnapshot([0.0] * 7, 10.0, 2.0, None)
+    second_feedback = JointStateSnapshot([0.0] * 7, 10.011, 3.0, 11.0)
 
-    teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0)
+    teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0, first_feedback)
+    teleop._record_timing_sample(8.0, 8.0, 1.5, 0.6, 3.5, first_feedback)
     assert messages == []
 
-    teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0)
+    teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0, second_feedback)
 
     assert len(messages) == 1
-    assert "right_rm75 timing n=2 deadline=8.000 ms" in messages[0]
+    assert "right_rm75 timing n=3 deadline=8.000 ms" in messages[0]
     assert (
-        "period[n=2 mean=8.000 p95=8.900 p99=8.980 "
+        "period[n=3 mean=8.000 p95=8.900 p99=8.980 "
         "max=9.000 ms overruns=1]"
     ) in messages[0]
     assert (
-        "total[n=2 mean=8.000 p95=9.800 p99=9.960 "
+        "total[n=3 mean=8.000 p95=9.800 p99=9.960 "
         "max=10.000 ms overruns=1]"
     ) in messages[0]
-    assert "qp[n=2" in messages[0]
-    assert "send[n=2" in messages[0]
-    assert "feedback_age[n=2" in messages[0]
+    assert "qp[n=3" in messages[0]
+    assert "send[n=3" in messages[0]
+    assert "feedback_age[n=3" in messages[0]
+    assert "feedback_read[n=2" in messages[0]
+    assert "feedback_interval[n=1" in messages[0]
     assert all(not samples for samples in teleop._timing_samples.values())
 
 

xr_rm_teleop/xr_rm_teleop/fun_peripheral.py

@@ -102,6 +102,40 @@ def _tool_name_for_index(tools_in_ee: dict[str, list[list[float]]], scissorgripp
     return list(tools_in_ee.keys())[scissorgripper]
 
 
+def _check_sdk_return(result: Any, operation: str) -> None:
+    if result != 0:
+        raise RuntimeError(f"{operation} failed with code {result}: {result!r}")
+
+
+def _configure_tool_frame(robot, tool_frame, tool_name: str) -> None:
+    frames = robot.rm_get_total_tool_frame()
+    if not isinstance(frames, dict):
+        raise RuntimeError(
+            f"rm_get_total_tool_frame returned invalid data: {frames!r}"
+        )
+    _check_sdk_return(
+        frames.get("return_code"),
+        "rm_get_total_tool_frame",
+    )
+    tool_names = frames.get("tool_names")
+    if not isinstance(tool_names, (list, tuple)):
+        raise RuntimeError(
+            f"rm_get_total_tool_frame returned invalid tool_names: {tool_names!r}"
+        )
+
+    if tool_name in tool_names:
+        operation = "rm_update_tool_frame"
+        result = robot.rm_update_tool_frame(frame=tool_frame)
+    else:
+        operation = "rm_set_manual_tool_frame"
+        result = robot.rm_set_manual_tool_frame(frame=tool_frame)
+    _check_sdk_return(result, operation)
+    _check_sdk_return(
+        robot.rm_change_tool_frame(tool_name),
+        "rm_change_tool_frame",
+    )
+
+
 def cal_tool_frame(handle, scissorgripper, tools_in_ee):
     """根据末端工具配置生成 RealMan 工具坐标系。"""
     from Robotic_Arm.rm_robot_interface import rm_frame_t
@@ -160,8 +194,7 @@ def peripheral_cfg(
 
     time.sleep(0.2)
     tool_frame, tool_name = cal_tool_frame(robot, scissorgripper, tools_in_ee)
-    robot.rm_set_manual_tool_frame(frame=tool_frame)
-    robot.rm_change_tool_frame(tool_name)
+    _configure_tool_frame(robot, tool_frame, tool_name)
 
     if scissorgripper == 0:
         # 剪刀夹爪通过工具板数字输出控制。

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -30,6 +30,8 @@ class ArmPose:
 class JointStateSnapshot:
     positions: list[float]
     received_at: float
+    read_duration_ms: float | None = None
+    update_interval_ms: float | None = None
 
 
 class MockRealManAdapter:
@@ -161,6 +163,8 @@ class RealManAdapter:
             return JointStateSnapshot(
                 list(self._latest_joint_state.positions),
                 self._latest_joint_state.received_at,
+                self._latest_joint_state.read_duration_ms,
+                self._latest_joint_state.update_interval_ms,
             )
 
     def send_joint_target(self, joints: list[float], follow: bool) -> None:
@@ -232,6 +236,7 @@ class RealManAdapter:
             raise RuntimeError("睿尔曼机械臂尚未连接")
 
     def _feedback_loop(self) -> None:
+        next_read_at = time.monotonic()
         while not self._feedback_stop.is_set():
             try:
                 self._read_joint_state_once()
@@ -240,11 +245,18 @@ class RealManAdapter:
                 if not self._feedback_fault_logged:
                     self._log_warn(f"RealMan 关节反馈读取失败：{exc}")
                     self._feedback_fault_logged = True
-            self._feedback_stop.wait(self._feedback_period)
+            next_read_at += self._feedback_period
+            remaining = next_read_at - time.monotonic()
+            if remaining <= 0.0:
+                next_read_at = time.monotonic()
+                continue
+            self._feedback_stop.wait(remaining)
 
     def _read_joint_state_once(self) -> None:
         self._require_arm()
+        read_started_ns = time.perf_counter_ns()
         result = self._arm.rm_get_joint_degree()
+        read_duration_ms = (time.perf_counter_ns() - read_started_ns) * 1e-6
         self._check_return(result, "rm_get_joint_degree")
         if not isinstance(result, tuple) or len(result) < 2:
             raise RuntimeError(f"rm_get_joint_degree 返回格式错误：{result!r}")
@@ -258,9 +270,19 @@ class RealManAdapter:
         positions = [math.radians(float(value)) for value in degrees]
         if not all(math.isfinite(value) for value in positions):
             raise RuntimeError("RM75 关节反馈包含 NaN/Inf")
-        snapshot = JointStateSnapshot(positions, time.monotonic())
+        received_at = time.monotonic()
         with self._joint_state_lock:
-            self._latest_joint_state = snapshot
+            update_interval_ms = (
+                None
+                if self._latest_joint_state is None
+                else (received_at - self._latest_joint_state.received_at) * 1000.0
+            )
+            self._latest_joint_state = JointStateSnapshot(
+                positions,
+                received_at,
+                read_duration_ms,
+                update_interval_ms,
+            )
 
     def _apply_safety_limits(self) -> None:
         # 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -283,9 +283,18 @@ class SingleArmVelocityTeleop(Node):
         self._timing_stats_window = max(1, int(round(5.0 / self._dt)))
         self._timing_samples = {
             name: []
-            for name in ("period", "total", "qp", "send", "feedback_age")
+            for name in (
+                "period",
+                "total",
+                "qp",
+                "send",
+                "feedback_age",
+                "feedback_read",
+                "feedback_interval",
+            )
         }
         self._last_control_tick_started_ns: int | None = None
+        self._last_timing_feedback_received_at: float | None = None
 
         peripheral_arm = self._peripheral_arm_name()
         config_file = str(self.get_parameter("peripheral_config_file").value)
@@ -602,6 +611,7 @@ class SingleArmVelocityTeleop(Node):
                 qp_ms,
                 send_ms,
                 feedback_age_ms,
+                snapshot,
             )
         except Exception as exc:
             self.get_logger().warn(
@@ -882,6 +892,7 @@ class SingleArmVelocityTeleop(Node):
         qp_ms: float,
         send_ms: float,
         feedback_age_ms: float,
+        snapshot: JointStateSnapshot,
     ) -> None:
         if period_ms is not None:
             self._timing_samples["period"].append(period_ms)
@@ -889,6 +900,16 @@ class SingleArmVelocityTeleop(Node):
         self._timing_samples["qp"].append(qp_ms)
         self._timing_samples["send"].append(send_ms)
         self._timing_samples["feedback_age"].append(feedback_age_ms)
+        if snapshot.received_at != self._last_timing_feedback_received_at:
+            self._last_timing_feedback_received_at = snapshot.received_at
+            if snapshot.read_duration_ms is not None:
+                self._timing_samples["feedback_read"].append(
+                    snapshot.read_duration_ms
+                )
+            if snapshot.update_interval_ms is not None:
+                self._timing_samples["feedback_interval"].append(
+                    snapshot.update_interval_ms
+                )
         if len(self._timing_samples["total"]) < self._timing_stats_window:
             return
 
@@ -912,6 +933,11 @@ class SingleArmVelocityTeleop(Node):
                 self._timing_samples["feedback_age"],
             ),
         ]
+        for name in ("feedback_read", "feedback_interval"):
+            if self._timing_samples[name]:
+                summaries.append(
+                    self._timing_summary(name, self._timing_samples[name])
+                )
         message = (
             f"{self._arm_name} timing n={sample_count} "
             f"deadline={deadline_ms:.3f} ms | "