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: