363 lines
12 KiB
Python
363 lines
12 KiB
Python
#!/usr/bin/env python3
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import copy
|
|
import importlib.util
|
|
import json
|
|
from pathlib import Path
|
|
import sys
|
|
from types import ModuleType
|
|
import unittest
|
|
|
|
|
|
fake_omnisocket = ModuleType("omnisocket")
|
|
fake_omnisocket.CONTROL_DEFAULTS = {}
|
|
fake_omnisocket.MSG_TYPE_ERROR = 5
|
|
fake_omnisocket.Session = object
|
|
sys.modules.setdefault("omnisocket", fake_omnisocket)
|
|
|
|
module_path = Path(__file__).with_name("omnisocket_xtele_sender.py")
|
|
spec = importlib.util.spec_from_file_location("omnisocket_xtele_sender", module_path)
|
|
assert spec is not None and spec.loader is not None
|
|
sender_module = importlib.util.module_from_spec(spec)
|
|
spec.loader.exec_module(sender_module)
|
|
|
|
|
|
def args() -> argparse.Namespace:
|
|
return argparse.Namespace(
|
|
server="127.0.0.1:14049",
|
|
peer_id="sender",
|
|
target_peer="robot",
|
|
zmq_endpoint="tcp://127.0.0.1:5003",
|
|
cmd_zmq_endpoint="",
|
|
cmd_max_age_s=0.25,
|
|
source_timeout_s=0.25,
|
|
max_feedback_age_ms=500.0,
|
|
max_pending_frames=100,
|
|
start_stop_hold_s=3.0,
|
|
combo_release_s=0.5,
|
|
start_marker_frames=50,
|
|
status_file="/tmp/tg3_sender_test_status.json",
|
|
)
|
|
|
|
|
|
def frame(pressed: bool) -> dict[str, object]:
|
|
value = 1 if pressed else 0
|
|
return {
|
|
"button": {
|
|
"left": [0, 0, value],
|
|
"right": [0, 0, value],
|
|
}
|
|
}
|
|
|
|
|
|
class SessionGateTest(unittest.TestCase):
|
|
def setUp(self) -> None:
|
|
self.sender = sender_module.XteleSender(args())
|
|
|
|
def stable_release(self, started_at: float) -> float:
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(started_at, frame(False))
|
|
)
|
|
finished_at = started_at + 0.51
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(finished_at, frame(False))
|
|
)
|
|
self.assertFalse(self.sender.require_combo_release)
|
|
return finished_at
|
|
|
|
def test_boot_requires_stable_release_before_start(self) -> None:
|
|
self.assertIsNone(self.sender._update_teleop_gate(0.0, frame(True)))
|
|
self.assertIsNone(self.sender._update_teleop_gate(4.0, frame(True)))
|
|
released_at = self.stable_release(5.0)
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(released_at + 0.01, frame(True))
|
|
)
|
|
self.assertEqual(
|
|
self.sender._update_teleop_gate(released_at + 3.02, frame(True)),
|
|
"start",
|
|
)
|
|
self.assertTrue(self.sender.teleop_active)
|
|
|
|
def test_single_false_frame_cannot_rearm_stop(self) -> None:
|
|
released_at = self.stable_release(0.0)
|
|
self.sender._update_teleop_gate(released_at + 0.01, frame(True))
|
|
self.assertEqual(
|
|
self.sender._update_teleop_gate(released_at + 3.02, frame(True)),
|
|
"start",
|
|
)
|
|
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(released_at + 3.03, frame(False))
|
|
)
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(released_at + 3.04, frame(True))
|
|
)
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(released_at + 7.00, frame(True))
|
|
)
|
|
self.assertTrue(self.sender.teleop_active)
|
|
self.assertTrue(self.sender.require_combo_release)
|
|
|
|
def test_stable_release_allows_separate_stop_hold(self) -> None:
|
|
released_at = self.stable_release(0.0)
|
|
self.sender._update_teleop_gate(released_at + 0.01, frame(True))
|
|
self.assertEqual(
|
|
self.sender._update_teleop_gate(released_at + 3.02, frame(True)),
|
|
"start",
|
|
)
|
|
second_release = self.stable_release(released_at + 3.03)
|
|
self.sender._update_teleop_gate(second_release + 0.01, frame(True))
|
|
self.assertEqual(
|
|
self.sender._update_teleop_gate(second_release + 3.02, frame(True)),
|
|
"stop",
|
|
)
|
|
self.assertFalse(self.sender.teleop_active)
|
|
|
|
def test_untrusted_transport_metadata_is_overwritten(self) -> None:
|
|
data = frame(False)
|
|
data["tg3_transport"] = {
|
|
"protocol_version": 999,
|
|
"session_id": "forged",
|
|
"session_seq": 999,
|
|
"session_state": "stop",
|
|
"stop_reason": "forged",
|
|
}
|
|
|
|
payload, merged = self.sender._build_payload(
|
|
data,
|
|
None,
|
|
"a" * 32,
|
|
7,
|
|
"start",
|
|
)
|
|
|
|
self.assertFalse(merged)
|
|
metadata = json.loads(payload)["tg3_transport"]
|
|
self.assertEqual(metadata["protocol_version"], 2)
|
|
self.assertEqual(metadata["session_id"], "a" * 32)
|
|
self.assertEqual(metadata["session_seq"], 7)
|
|
self.assertEqual(metadata["session_state"], "start")
|
|
self.assertNotIn("stop_reason", metadata)
|
|
|
|
def test_stale_or_backlogged_transport_is_rejected(self) -> None:
|
|
class FakeSession:
|
|
def __init__(self, feedback_age_ms: int, pending: int) -> None:
|
|
self.feedback_age_ms = feedback_age_ms
|
|
self.pending = pending
|
|
|
|
@staticmethod
|
|
def stats() -> dict[str, int]:
|
|
return {"connected": 1, "registered": 1}
|
|
|
|
def kcp_stats(self) -> dict[str, int]:
|
|
return {
|
|
"snd_queue": self.pending,
|
|
"snd_buffer": 0,
|
|
"last_feedback_age_ms": self.feedback_age_ms,
|
|
}
|
|
|
|
self.sender.session_connected_at = 0.0
|
|
self.sender.session = FakeSession(600, 0)
|
|
self.assertIn(
|
|
"feedback stale", self.sender._session_unhealthy_reason() or ""
|
|
)
|
|
self.sender.session = FakeSession(1, 101)
|
|
self.assertIn(
|
|
"pending queue", self.sender._session_unhealthy_reason() or ""
|
|
)
|
|
|
|
|
|
class ProcessedHandMergeTest(unittest.TestCase):
|
|
@staticmethod
|
|
def raw_frame(right_b: int) -> dict[str, object]:
|
|
return {
|
|
"button": {
|
|
"left": [0, 0, 0],
|
|
"right": [0, right_b, 0],
|
|
},
|
|
"trigger": {"left": 0.12, "right": 0.73},
|
|
"hand": {"position": {"left": 0.12, "right": 0.73}},
|
|
}
|
|
|
|
@staticmethod
|
|
def processed_command() -> dict[str, object]:
|
|
return {
|
|
"timestamp": 123.5,
|
|
"hand": {
|
|
"position": {
|
|
"left": [0.1, 0.2, 0.3, 0.4, 0.5, 0.6],
|
|
"right": [0.6, 0.5, 0.4, 0.3, 0.2, 0.1],
|
|
}
|
|
},
|
|
}
|
|
|
|
def test_processed_command_cannot_run_ahead_of_raw_timestamp(self) -> None:
|
|
command = self.processed_command()
|
|
self.assertFalse(
|
|
sender_module.XteleSender._command_aligned_with_raw(
|
|
command, {"timestamp": 123.4}
|
|
)
|
|
)
|
|
self.assertTrue(
|
|
sender_module.XteleSender._command_aligned_with_raw(
|
|
command, {"timestamp": 123.5}
|
|
)
|
|
)
|
|
self.assertTrue(
|
|
sender_module.XteleSender._command_aligned_with_raw(
|
|
command, {"timestamp": 124.0}
|
|
)
|
|
)
|
|
self.assertFalse(
|
|
sender_module.XteleSender._command_aligned_with_raw(
|
|
command, {"timestamp": "124.0"}
|
|
)
|
|
)
|
|
|
|
def build(
|
|
self,
|
|
data: dict[str, object],
|
|
suppress_processed_right: bool | None = None,
|
|
) -> tuple[dict[str, object], bool]:
|
|
encoded, merged = sender_module.XteleSender._build_payload(
|
|
data,
|
|
self.processed_command(),
|
|
"session-id",
|
|
7,
|
|
"active",
|
|
suppress_processed_right=suppress_processed_right,
|
|
)
|
|
return json.loads(encoded), merged
|
|
|
|
def test_right_b_preserves_raw_right_hand_and_trigger(self) -> None:
|
|
raw = self.raw_frame(right_b=1)
|
|
original = copy.deepcopy(raw)
|
|
|
|
payload, merged = self.build(raw)
|
|
|
|
self.assertTrue(merged)
|
|
self.assertEqual(
|
|
payload["hand"]["position"]["left"],
|
|
self.processed_command()["hand"]["position"]["left"],
|
|
)
|
|
self.assertEqual(payload["hand"]["position"]["right"], 0.73)
|
|
self.assertEqual(payload["trigger"], original["trigger"])
|
|
self.assertEqual(
|
|
payload["tg3_transport"]["processed_hand_sides"], ["left"]
|
|
)
|
|
self.assertTrue(
|
|
payload["tg3_transport"][
|
|
"processed_right_hand_suppressed_by_b"
|
|
]
|
|
)
|
|
self.assertEqual(raw, original, "payload building must not mutate raw 5003")
|
|
|
|
def test_right_b_release_restores_bilateral_processed_merge(self) -> None:
|
|
raw = self.raw_frame(right_b=0)
|
|
|
|
payload, merged = self.build(raw)
|
|
|
|
self.assertTrue(merged)
|
|
self.assertEqual(
|
|
payload["hand"]["position"],
|
|
self.processed_command()["hand"]["position"],
|
|
)
|
|
self.assertEqual(payload["trigger"]["right"], 0.73)
|
|
self.assertEqual(
|
|
payload["tg3_transport"]["processed_hand_sides"],
|
|
["left", "right"],
|
|
)
|
|
self.assertNotIn(
|
|
"processed_right_hand_suppressed_by_b",
|
|
payload["tg3_transport"],
|
|
)
|
|
|
|
def test_runtime_gate_suppresses_through_stable_b_release(self) -> None:
|
|
sender = sender_module.XteleSender(args())
|
|
pressed = self.raw_frame(right_b=1)
|
|
released = self.raw_frame(right_b=0)
|
|
baseline = self.processed_command()["hand"]["position"]["right"]
|
|
b_gesture = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98]
|
|
|
|
self.assertFalse(sender._update_right_b_merge_gate(0.0, released, baseline))
|
|
self.assertFalse(
|
|
sender._update_right_b_merge_gate(0.26, released, baseline)
|
|
)
|
|
self.assertTrue(
|
|
sender._update_right_b_merge_gate(1.0, pressed, b_gesture)
|
|
)
|
|
self.assertTrue(
|
|
sender._update_right_b_merge_gate(1.1, released, b_gesture)
|
|
)
|
|
self.assertTrue(
|
|
sender._update_right_b_merge_gate(1.61, released, b_gesture)
|
|
)
|
|
held_payload, _ = self.build(
|
|
released, suppress_processed_right=True
|
|
)
|
|
self.assertEqual(held_payload["hand"]["position"]["right"], 0.73)
|
|
|
|
# Stable B release is not enough: processed 5001 must also remain at
|
|
# a safe baseline for at least cmd_max_age_s before it is trusted.
|
|
self.assertTrue(
|
|
sender._update_right_b_merge_gate(1.62, released, baseline)
|
|
)
|
|
self.assertTrue(
|
|
sender._update_right_b_merge_gate(1.86, released, baseline)
|
|
)
|
|
self.assertFalse(
|
|
sender._update_right_b_merge_gate(1.88, released, baseline)
|
|
)
|
|
restored_payload, _ = self.build(
|
|
released, suppress_processed_right=False
|
|
)
|
|
self.assertEqual(
|
|
restored_payload["hand"]["position"]["right"],
|
|
self.processed_command()["hand"]["position"]["right"],
|
|
)
|
|
|
|
def test_malformed_b_cannot_clear_runtime_suppression(self) -> None:
|
|
sender = sender_module.XteleSender(args())
|
|
self.assertTrue(
|
|
sender._update_right_b_merge_gate(
|
|
1.0, self.raw_frame(right_b=1), None
|
|
)
|
|
)
|
|
malformed = self.raw_frame(right_b=0)
|
|
malformed["button"] = {"right": [0]}
|
|
self.assertTrue(
|
|
sender._update_right_b_merge_gate(2.0, malformed, 0.73)
|
|
)
|
|
self.assertIsNone(sender.right_b_release_started_at)
|
|
|
|
def test_one_early_processed_change_cannot_replace_pre_b_baseline(self) -> None:
|
|
sender = sender_module.XteleSender(args())
|
|
released = self.raw_frame(right_b=0)
|
|
pressed = self.raw_frame(right_b=1)
|
|
baseline = self.processed_command()["hand"]["position"]["right"]
|
|
b_gesture = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98]
|
|
|
|
sender._update_right_b_merge_gate(0.0, released, baseline)
|
|
sender._update_right_b_merge_gate(0.26, released, baseline)
|
|
self.assertEqual(sender.right_b_processed_baseline, baseline)
|
|
|
|
# Model 5001 being polled just before the corresponding raw B frame.
|
|
# It may become a candidate, but cannot immediately replace the last
|
|
# stable, B-false baseline.
|
|
sender._update_right_b_merge_gate(1.0, released, b_gesture)
|
|
self.assertEqual(sender.right_b_processed_baseline, baseline)
|
|
sender._update_right_b_merge_gate(1.01, pressed, b_gesture)
|
|
sender._update_right_b_merge_gate(1.02, released, b_gesture)
|
|
self.assertTrue(
|
|
sender._update_right_b_merge_gate(1.53, released, b_gesture)
|
|
)
|
|
self.assertTrue(
|
|
sender._update_right_b_merge_gate(2.0, released, b_gesture)
|
|
)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|