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

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())