#!/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()