996 lines
38 KiB
Python
996 lines
38 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 threading import Event
|
|
from types import ModuleType
|
|
import unittest
|
|
from unittest import mock
|
|
|
|
|
|
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):
|
|
class FakeSession:
|
|
def __init__(
|
|
self,
|
|
responses: list[tuple[str, int, bytes]] | None = None,
|
|
) -> None:
|
|
self.responses = list(responses or [])
|
|
self.sent: list[tuple[str, bytes]] = []
|
|
self.closed = False
|
|
|
|
@staticmethod
|
|
def stats() -> dict[str, int]:
|
|
return {"connected": 1, "registered": 1}
|
|
|
|
@staticmethod
|
|
def kcp_stats() -> dict[str, int]:
|
|
return {
|
|
"snd_queue": 0,
|
|
"snd_buffer": 0,
|
|
"last_feedback_age_ms": 0,
|
|
}
|
|
|
|
def send(self, *, to: str, data: bytes) -> None:
|
|
self.sent.append((to, data))
|
|
|
|
def recv(self, timeout_ms: int) -> tuple[str, int, bytes] | None:
|
|
self.assert_zero_timeout(timeout_ms)
|
|
if self.responses:
|
|
return self.responses.pop(0)
|
|
return None
|
|
|
|
@staticmethod
|
|
def assert_zero_timeout(timeout_ms: int) -> None:
|
|
if timeout_ms != 0:
|
|
raise AssertionError(f"unexpected receive timeout: {timeout_ms}")
|
|
|
|
def close(self) -> None:
|
|
self.closed = True
|
|
|
|
class BlockingConnectSession(FakeSession):
|
|
def __init__(
|
|
self,
|
|
entered: Event,
|
|
release: Event,
|
|
failure: OSError | None = None,
|
|
) -> None:
|
|
super().__init__()
|
|
self.entered = entered
|
|
self.release = release
|
|
self.failure = failure
|
|
self.connect_args: tuple[str, str, dict[str, object]] | None = None
|
|
|
|
def connect(
|
|
self,
|
|
*,
|
|
server_addr: str,
|
|
peer_id: str,
|
|
**options: object,
|
|
) -> None:
|
|
self.connect_args = (server_addr, peer_id, options)
|
|
self.entered.set()
|
|
if not self.release.wait(timeout=1.0):
|
|
raise RuntimeError("test did not release blocked connect")
|
|
if self.failure is not None:
|
|
raise self.failure
|
|
|
|
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 create_pending_start(self, started_at: float = 0.0) -> float:
|
|
released_at = self.stable_release(started_at)
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(released_at + 0.01, frame(True))
|
|
)
|
|
pending_at = released_at + 3.02
|
|
self.assertEqual(
|
|
self.sender._update_teleop_gate(pending_at, frame(True)),
|
|
"start_pending",
|
|
)
|
|
self.assertTrue(self.sender.start_pending)
|
|
self.assertFalse(self.sender.teleop_active)
|
|
return pending_at
|
|
|
|
def cancel_pending_from_latest_frame(self) -> None:
|
|
if (
|
|
self.sender.start_pending
|
|
and self.sender.latest_combo_state is not True
|
|
):
|
|
reason = (
|
|
"released"
|
|
if self.sender.latest_combo_state is False
|
|
else "malformed"
|
|
)
|
|
self.sender._cancel_pending_start(reason)
|
|
|
|
def attach_session(
|
|
self,
|
|
responses: list[tuple[str, int, bytes]] | None = None,
|
|
) -> FakeSession:
|
|
session = self.FakeSession(responses)
|
|
self.sender.session = session
|
|
self.sender.session_connected_at = 100.0
|
|
self.sender.counters["connected"] = 1
|
|
return session
|
|
|
|
def test_boot_requires_stable_release_before_pending_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_pending",
|
|
)
|
|
self.assertTrue(self.sender.start_pending)
|
|
self.assertFalse(self.sender.teleop_active)
|
|
self.assertEqual(self.sender.counters["teleop_start_requests"], 1)
|
|
self.assertEqual(self.sender.counters["teleop_starts"], 0)
|
|
self.assertIsNotNone(self.sender.teleop_session_id)
|
|
self.assertTrue(
|
|
self.sender._pending_connect_ready(released_at + 3.02)
|
|
)
|
|
|
|
def test_boot_malformed_buttons_cannot_clear_release_requirement(self) -> None:
|
|
malformed = frame(False)
|
|
malformed["button"] = {
|
|
"left": [0, 0, 0],
|
|
"right": [0, 0, "0"],
|
|
}
|
|
|
|
self.assertIsNone(self.sender._update_teleop_gate(0.0, frame(False)))
|
|
self.assertEqual(self.sender.combo_release_started_at, 0.0)
|
|
self.assertIsNone(self.sender._update_teleop_gate(0.49, malformed))
|
|
self.assertTrue(self.sender.require_combo_release)
|
|
self.assertIsNone(self.sender.combo_release_started_at)
|
|
self.assertIsNone(self.sender.latest_combo_state)
|
|
|
|
# The release timer must restart after malformed input; time before the
|
|
# malformed frame cannot be accumulated toward re-arming.
|
|
self.assertIsNone(self.sender._update_teleop_gate(0.50, frame(False)))
|
|
self.assertIsNone(self.sender._update_teleop_gate(0.99, frame(False)))
|
|
self.assertTrue(self.sender.require_combo_release)
|
|
self.assertIsNone(self.sender._update_teleop_gate(1.01, frame(False)))
|
|
self.assertFalse(self.sender.require_combo_release)
|
|
|
|
def test_start_stop_parser_accepts_only_explicit_boolean_buttons(self) -> None:
|
|
self.assertTrue(self.sender._start_stop_pressed(frame(True)))
|
|
self.assertFalse(self.sender._start_stop_pressed(frame(False)))
|
|
for invalid in (1.0, "1", None, 2, -1):
|
|
with self.subTest(value=invalid):
|
|
malformed = frame(True)
|
|
malformed["button"] = {
|
|
"left": [0, 0, invalid],
|
|
"right": [0, 0, 1],
|
|
}
|
|
self.assertIsNone(
|
|
self.sender._start_stop_pressed(malformed)
|
|
)
|
|
|
|
def test_single_false_frame_cannot_rearm_stop(self) -> None:
|
|
pending_at = self.create_pending_start()
|
|
self.sender._commit_pending_start_sent(pending_at + 0.01)
|
|
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(pending_at + 0.02, frame(False))
|
|
)
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(pending_at + 0.03, frame(True))
|
|
)
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(pending_at + 4.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:
|
|
pending_at = self.create_pending_start()
|
|
self.sender._commit_pending_start_sent(pending_at + 0.01)
|
|
second_release = self.stable_release(pending_at + 0.02)
|
|
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)
|
|
self.assertEqual(self.sender.counters["teleop_starts"], 1)
|
|
self.assertEqual(self.sender.counters["teleop_stops"], 1)
|
|
|
|
def test_cancelled_start_requires_release_before_a_new_hold(self) -> None:
|
|
pending_at = self.create_pending_start()
|
|
first_session_id = self.sender.teleop_session_id
|
|
self.sender._cancel_pending_start("Hub offline")
|
|
|
|
self.assertFalse(self.sender.start_pending)
|
|
self.assertFalse(self.sender.teleop_active)
|
|
self.assertTrue(self.sender.require_combo_release)
|
|
self.assertEqual(self.sender.last_start_failure, "Hub offline")
|
|
self.assertIsNone(self.sender.teleop_session_id)
|
|
|
|
# A consumed hold cannot silently become another pending request while
|
|
# the operator keeps Z+C pressed, regardless of elapsed time.
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(
|
|
pending_at + 10.0, frame(True)
|
|
)
|
|
)
|
|
self.assertFalse(self.sender.start_pending)
|
|
self.assertFalse(self.sender._pending_connect_ready(pending_at + 10.0))
|
|
|
|
# A stable release followed by a new complete hold creates a distinct
|
|
# logical session which can make one new connection attempt.
|
|
released_at = self.stable_release(pending_at + 10.01)
|
|
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_pending",
|
|
)
|
|
self.assertTrue(self.sender.start_pending)
|
|
self.assertIsNotNone(self.sender.teleop_session_id)
|
|
self.assertNotEqual(self.sender.teleop_session_id, first_session_id)
|
|
self.assertEqual(self.sender.counters["teleop_start_requests"], 2)
|
|
self.assertEqual(self.sender.counters["teleop_starts"], 0)
|
|
|
|
def test_pending_start_release_cancels_and_closes_session(self) -> None:
|
|
pending_at = self.create_pending_start()
|
|
session = self.attach_session()
|
|
self.sender.start_wait_fresh_after_connect = True
|
|
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(pending_at + 0.01, frame(False))
|
|
)
|
|
self.cancel_pending_from_latest_frame()
|
|
|
|
self.assertFalse(self.sender.start_pending)
|
|
self.assertFalse(self.sender.teleop_active)
|
|
self.assertIsNone(self.sender.teleop_session_id)
|
|
self.assertIsNone(self.sender.session)
|
|
self.assertTrue(session.closed)
|
|
self.assertEqual(session.sent, [])
|
|
self.assertEqual(self.sender.counters["start_pending_cancels"], 1)
|
|
self.assertTrue(self.sender.require_combo_release)
|
|
self.assertFalse(self.sender._pending_connect_ready(pending_at + 10.0))
|
|
|
|
def test_pending_start_malformed_button_state_cancels(self) -> None:
|
|
pending_at = self.create_pending_start()
|
|
malformed = frame(True)
|
|
malformed["button"] = {"left": [0, 0], "right": [0, 0, 1]}
|
|
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(pending_at + 0.01, malformed)
|
|
)
|
|
self.cancel_pending_from_latest_frame()
|
|
|
|
self.assertFalse(self.sender.start_pending)
|
|
self.assertIsNone(self.sender.latest_combo_state)
|
|
self.assertEqual(self.sender.last_start_failure, "malformed")
|
|
self.assertEqual(self.sender.counters["start_pending_cancels"], 1)
|
|
|
|
def test_pending_freshness_waits_then_accepts_advancing_timestamp(self) -> None:
|
|
self.create_pending_start()
|
|
self.sender.start_wait_fresh_after_connect = True
|
|
first = frame(True)
|
|
first["timestamp"] = 100.0
|
|
second = frame(True)
|
|
second["timestamp"] = 100.01
|
|
|
|
self.assertEqual(
|
|
self.sender._check_pending_start_freshness(first),
|
|
("wait", ""),
|
|
)
|
|
self.assertEqual(self.sender.start_fresh_barrier_timestamp, 100.0)
|
|
self.assertTrue(self.sender.start_wait_fresh_after_connect)
|
|
self.assertFalse(self.sender.teleop_active)
|
|
|
|
self.assertEqual(
|
|
self.sender._check_pending_start_freshness(second),
|
|
("ready", ""),
|
|
)
|
|
self.assertIsNone(self.sender.start_fresh_barrier_timestamp)
|
|
self.assertFalse(self.sender.start_wait_fresh_after_connect)
|
|
self.assertTrue(self.sender.start_pending)
|
|
self.assertEqual(self.sender.counters["teleop_starts"], 0)
|
|
|
|
def test_pending_freshness_missing_timestamp_cancels(self) -> None:
|
|
self.create_pending_start()
|
|
session = self.attach_session()
|
|
self.sender.start_wait_fresh_after_connect = True
|
|
|
|
outcome, reason = self.sender._check_pending_start_freshness(
|
|
frame(True)
|
|
)
|
|
self.assertEqual(outcome, "cancel")
|
|
self.assertIn("fresh xTELE timestamp", reason)
|
|
self.sender._cancel_pending_start(reason)
|
|
|
|
self.assertFalse(self.sender.start_pending)
|
|
self.assertFalse(self.sender.teleop_active)
|
|
self.assertTrue(session.closed)
|
|
self.assertEqual(session.sent, [])
|
|
|
|
def test_pending_freshness_repeated_timestamp_waits_for_later_advance(
|
|
self,
|
|
) -> None:
|
|
self.create_pending_start()
|
|
self.sender.start_wait_fresh_after_connect = True
|
|
first = frame(True)
|
|
first["timestamp"] = 100.0
|
|
repeated = frame(True)
|
|
repeated["timestamp"] = 100.0
|
|
advanced = frame(True)
|
|
advanced["timestamp"] = 100.01
|
|
|
|
self.assertEqual(
|
|
self.sender._check_pending_start_freshness(first),
|
|
("wait", ""),
|
|
)
|
|
self.assertEqual(
|
|
self.sender._check_pending_start_freshness(repeated),
|
|
("wait", ""),
|
|
)
|
|
self.assertEqual(self.sender.start_fresh_barrier_timestamp, 100.0)
|
|
self.assertTrue(self.sender.start_wait_fresh_after_connect)
|
|
self.assertTrue(self.sender.start_pending)
|
|
self.assertEqual(
|
|
self.sender._check_pending_start_freshness(advanced),
|
|
("ready", ""),
|
|
)
|
|
self.assertIsNone(self.sender.start_fresh_barrier_timestamp)
|
|
self.assertFalse(self.sender.start_wait_fresh_after_connect)
|
|
self.assertTrue(self.sender.start_pending)
|
|
|
|
def test_pending_freshness_backwards_timestamp_cancels(self) -> None:
|
|
self.create_pending_start()
|
|
session = self.attach_session()
|
|
self.sender.start_wait_fresh_after_connect = True
|
|
first = frame(True)
|
|
first["timestamp"] = 100.0
|
|
backwards = frame(True)
|
|
backwards["timestamp"] = 99.99
|
|
|
|
self.assertEqual(
|
|
self.sender._check_pending_start_freshness(first),
|
|
("wait", ""),
|
|
)
|
|
outcome, reason = self.sender._check_pending_start_freshness(backwards)
|
|
self.assertEqual(outcome, "cancel")
|
|
self.assertIn("moved backwards", reason)
|
|
self.sender._cancel_pending_start(reason)
|
|
|
|
self.assertFalse(self.sender.start_pending)
|
|
self.assertTrue(session.closed)
|
|
self.assertEqual(session.sent, [])
|
|
|
|
def test_background_connect_success_is_adopted_as_pending(self) -> None:
|
|
self.create_pending_start()
|
|
entered = Event()
|
|
release = Event()
|
|
session = self.BlockingConnectSession(entered, release)
|
|
|
|
with (
|
|
mock.patch.object(sender_module, "Session", return_value=session),
|
|
mock.patch.object(self.sender, "write_status"),
|
|
):
|
|
self.assertTrue(self.sender._start_connect_attempt())
|
|
self.assertTrue(entered.wait(timeout=1.0))
|
|
self.assertTrue(self.sender._connect_inflight())
|
|
thread = self.sender.connect_thread
|
|
self.assertIsNotNone(thread)
|
|
release.set()
|
|
thread.join(timeout=1.0) # type: ignore[union-attr]
|
|
self.assertFalse(thread.is_alive()) # type: ignore[union-attr]
|
|
self.assertEqual(
|
|
self.sender._poll_connect_result(50.0), "connected"
|
|
)
|
|
|
|
self.assertIs(self.sender.session, session)
|
|
self.assertFalse(session.closed)
|
|
self.assertTrue(self.sender.start_pending)
|
|
self.assertFalse(self.sender.teleop_active)
|
|
self.assertTrue(self.sender.start_wait_fresh_after_connect)
|
|
self.assertIsNone(self.sender.start_fresh_barrier_timestamp)
|
|
self.assertEqual(self.sender.counters["connected"], 1)
|
|
self.assertEqual(self.sender.counters["reconnects"], 1)
|
|
self.assertEqual(
|
|
session.connect_args,
|
|
(self.sender.args.server, self.sender.args.peer_id, {}),
|
|
)
|
|
self.sender.close_session()
|
|
|
|
def test_release_during_connect_discards_and_closes_late_result(self) -> None:
|
|
pending_at = self.create_pending_start()
|
|
entered = Event()
|
|
release = Event()
|
|
session = self.BlockingConnectSession(entered, release)
|
|
|
|
with mock.patch.object(sender_module, "Session", return_value=session):
|
|
self.assertTrue(self.sender._start_connect_attempt())
|
|
self.assertTrue(entered.wait(timeout=1.0))
|
|
thread = self.sender.connect_thread
|
|
self.assertIsNotNone(thread)
|
|
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(
|
|
pending_at + 0.01, frame(False)
|
|
)
|
|
)
|
|
self.cancel_pending_from_latest_frame()
|
|
self.assertFalse(self.sender.start_pending)
|
|
|
|
release.set()
|
|
thread.join(timeout=1.0) # type: ignore[union-attr]
|
|
self.assertFalse(thread.is_alive()) # type: ignore[union-attr]
|
|
self.assertEqual(
|
|
self.sender._poll_connect_result(pending_at + 0.02),
|
|
"discarded",
|
|
)
|
|
|
|
self.assertTrue(session.closed)
|
|
self.assertIsNone(self.sender.session)
|
|
self.assertFalse(self.sender.teleop_active)
|
|
self.assertIsNone(self.sender.teleop_session_id)
|
|
self.assertEqual(self.sender.counters["connected"], 0)
|
|
self.assertEqual(self.sender.counters["teleop_starts"], 0)
|
|
self.assertEqual(self.sender.counters["start_pending_cancels"], 1)
|
|
|
|
def test_background_connect_failure_consumes_hold_without_retry(self) -> None:
|
|
pending_at = self.create_pending_start()
|
|
first_session_id = self.sender.teleop_session_id
|
|
entered = Event()
|
|
release = Event()
|
|
session = self.BlockingConnectSession(
|
|
entered,
|
|
release,
|
|
OSError("connection refused"),
|
|
)
|
|
|
|
with (
|
|
mock.patch.object(sender_module, "Session", return_value=session),
|
|
mock.patch.object(self.sender, "write_status"),
|
|
):
|
|
# run() owns this counter; mirror its one increment around the
|
|
# lower-level worker call used by this unit test.
|
|
self.sender.counters["start_connect_attempts"] += 1
|
|
self.assertTrue(self.sender._start_connect_attempt())
|
|
self.assertTrue(entered.wait(timeout=1.0))
|
|
thread = self.sender.connect_thread
|
|
self.assertIsNotNone(thread)
|
|
release.set()
|
|
thread.join(timeout=1.0) # type: ignore[union-attr]
|
|
self.assertFalse(thread.is_alive()) # type: ignore[union-attr]
|
|
self.assertEqual(self.sender._poll_connect_result(20.0), "failed")
|
|
|
|
self.assertTrue(session.closed)
|
|
self.assertEqual(session.sent, [])
|
|
self.assertIsNone(self.sender.session)
|
|
self.assertFalse(self.sender.start_pending)
|
|
self.assertFalse(self.sender.teleop_active)
|
|
self.assertIsNone(self.sender.teleop_session_id)
|
|
self.assertTrue(self.sender.require_combo_release)
|
|
self.assertEqual(self.sender.counters["start_connect_failures"], 1)
|
|
self.assertEqual(self.sender.counters["start_connect_attempts"], 1)
|
|
self.assertEqual(self.sender.counters["teleop_starts"], 0)
|
|
self.assertEqual(self.sender.counters["frames_sent"], 0)
|
|
self.assertIn("connection refused", self.sender.last_start_failure)
|
|
self.assertFalse(self.sender._pending_connect_ready(100.0))
|
|
|
|
# Keeping the failed Z+C hold pressed cannot create another request or
|
|
# connection attempt after the Hub later becomes available.
|
|
self.assertIsNone(
|
|
self.sender._update_teleop_gate(25.0, frame(True))
|
|
)
|
|
self.assertFalse(self.sender.start_pending)
|
|
self.assertEqual(self.sender.counters["teleop_start_requests"], 1)
|
|
self.assertEqual(self.sender.counters["start_connect_attempts"], 1)
|
|
|
|
released_at = self.stable_release(25.01)
|
|
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_pending",
|
|
)
|
|
self.assertTrue(self.sender.start_pending)
|
|
self.assertNotEqual(self.sender.teleop_session_id, first_session_id)
|
|
self.assertEqual(self.sender.counters["teleop_start_requests"], 2)
|
|
self.assertTrue(self.sender._pending_connect_ready(released_at + 3.02))
|
|
|
|
def test_session_constructor_failure_consumes_hold_without_retry(self) -> None:
|
|
self.create_pending_start()
|
|
constructor_called = Event()
|
|
|
|
def broken_session_constructor() -> object:
|
|
constructor_called.set()
|
|
raise RuntimeError("Session constructor failed")
|
|
|
|
with (
|
|
mock.patch.object(
|
|
sender_module,
|
|
"Session",
|
|
side_effect=broken_session_constructor,
|
|
),
|
|
mock.patch.object(self.sender, "write_status"),
|
|
):
|
|
self.assertTrue(self.sender._start_connect_attempt())
|
|
self.assertTrue(constructor_called.wait(timeout=1.0))
|
|
thread = self.sender.connect_thread
|
|
self.assertIsNotNone(thread)
|
|
thread.join(timeout=1.0) # type: ignore[union-attr]
|
|
self.assertFalse(thread.is_alive()) # type: ignore[union-attr]
|
|
self.assertEqual(self.sender._poll_connect_result(30.0), "failed")
|
|
|
|
self.assertIsNone(self.sender.session)
|
|
self.assertFalse(self.sender.start_pending)
|
|
self.assertFalse(self.sender.teleop_active)
|
|
self.assertIsNone(self.sender.teleop_session_id)
|
|
self.assertTrue(self.sender.require_combo_release)
|
|
self.assertEqual(self.sender.counters["start_connect_failures"], 1)
|
|
self.assertIn("Session constructor failed", self.sender.last_start_failure)
|
|
self.assertEqual(self.sender.counters["teleop_starts"], 0)
|
|
self.assertFalse(self.sender._pending_connect_ready(300.0))
|
|
|
|
def test_successful_start_commit_activates_once(self) -> None:
|
|
pending_at = self.create_pending_start()
|
|
self.sender._commit_pending_start_sent(pending_at + 0.01)
|
|
|
|
self.assertFalse(self.sender.start_pending)
|
|
self.assertTrue(self.sender.teleop_active)
|
|
self.assertEqual(self.sender.counters["teleop_starts"], 1)
|
|
self.assertEqual(self.sender.counters["teleop_start_requests"], 1)
|
|
|
|
def test_unknown_target_is_an_ordinary_remote_error_and_aborts(self) -> None:
|
|
pending_at = self.create_pending_start()
|
|
self.sender._commit_pending_start_sent(pending_at + 0.01)
|
|
session = self.attach_session(
|
|
[("hub", fake_omnisocket.MSG_TYPE_ERROR, b"unknown target: robot")]
|
|
)
|
|
|
|
self.sender._drain_responses()
|
|
|
|
self.assertTrue(session.closed)
|
|
self.assertIsNone(self.sender.session)
|
|
self.assertFalse(self.sender.start_pending)
|
|
self.assertFalse(self.sender.teleop_active)
|
|
self.assertIsNone(self.sender.teleop_session_id)
|
|
self.assertTrue(self.sender.require_combo_release)
|
|
self.assertEqual(self.sender.counters["teleop_aborts"], 1)
|
|
self.assertEqual(self.sender.counters["teleop_starts"], 1)
|
|
self.assertIn("unknown target: robot", self.sender.last_error)
|
|
|
|
def test_other_remote_error_also_aborts(self) -> None:
|
|
pending_at = self.create_pending_start()
|
|
self.sender._commit_pending_start_sent(pending_at + 0.01)
|
|
session = self.attach_session(
|
|
[("hub", fake_omnisocket.MSG_TYPE_ERROR, b"route unavailable")]
|
|
)
|
|
|
|
self.sender._drain_responses()
|
|
|
|
self.assertTrue(session.closed)
|
|
self.assertFalse(self.sender.start_pending)
|
|
self.assertFalse(self.sender.teleop_active)
|
|
self.assertEqual(self.sender.counters["teleop_aborts"], 1)
|
|
self.assertIn("route unavailable", self.sender.last_error)
|
|
|
|
def test_source_clock_allows_adjacent_repeat_under_timeout(self) -> None:
|
|
first = frame(False)
|
|
first["timestamp"] = 2507
|
|
repeated = frame(False)
|
|
repeated["timestamp"] = 2507
|
|
|
|
self.assertEqual(
|
|
self.sender._check_source_clock(10.0, first),
|
|
(True, ""),
|
|
)
|
|
self.assertEqual(
|
|
self.sender._check_source_clock(10.24, repeated),
|
|
(True, ""),
|
|
)
|
|
self.assertEqual(self.sender.last_source_timestamp, 2507.0)
|
|
self.assertEqual(self.sender.last_source_timestamp_advanced_at, 10.0)
|
|
|
|
def test_source_clock_rejects_persistent_freeze_over_timeout(self) -> None:
|
|
first = frame(False)
|
|
first["timestamp"] = 2517
|
|
repeated = frame(False)
|
|
repeated["timestamp"] = 2517
|
|
|
|
self.assertEqual(
|
|
self.sender._check_source_clock(10.0, first),
|
|
(True, ""),
|
|
)
|
|
self.assertEqual(
|
|
self.sender._check_source_clock(10.24, repeated),
|
|
(True, ""),
|
|
)
|
|
clock_ok, reason = self.sender._check_source_clock(10.251, repeated)
|
|
self.assertFalse(clock_ok)
|
|
self.assertIn("stopped advancing", reason)
|
|
self.assertEqual(self.sender.last_source_timestamp, 2517.0)
|
|
|
|
def test_source_clock_rejects_backwards_timestamp_while_active(self) -> None:
|
|
first = frame(False)
|
|
first["timestamp"] = 3000
|
|
backwards = frame(False)
|
|
backwards["timestamp"] = 2999
|
|
self.assertEqual(
|
|
self.sender._check_source_clock(10.0, first),
|
|
(True, ""),
|
|
)
|
|
self.sender.teleop_active = True
|
|
|
|
clock_ok, reason = self.sender._check_source_clock(10.01, backwards)
|
|
|
|
self.assertFalse(clock_ok)
|
|
self.assertIn("moved backwards", reason)
|
|
self.assertEqual(self.sender.last_source_timestamp, 3000.0)
|
|
self.assertEqual(self.sender.counters["source_timestamp_resets"], 0)
|
|
|
|
def test_source_clock_idle_backwards_resets_and_requires_release(self) -> None:
|
|
first = frame(False)
|
|
first["timestamp"] = 4000
|
|
restarted = frame(True)
|
|
restarted["timestamp"] = 10
|
|
self.assertEqual(
|
|
self.sender._check_source_clock(10.0, first),
|
|
(True, ""),
|
|
)
|
|
self.sender.require_combo_release = False
|
|
self.sender.combo_started_at = 10.01
|
|
self.sender.combo_release_started_at = 10.02
|
|
self.sender.latest_combo_state = True
|
|
|
|
self.assertEqual(
|
|
self.sender._check_source_clock(10.03, restarted),
|
|
(True, ""),
|
|
)
|
|
|
|
self.assertEqual(self.sender.last_source_timestamp, 10.0)
|
|
self.assertEqual(self.sender.last_source_timestamp_advanced_at, 10.03)
|
|
self.assertTrue(self.sender.require_combo_release)
|
|
self.assertIsNone(self.sender.combo_started_at)
|
|
self.assertIsNone(self.sender.combo_release_started_at)
|
|
self.assertIsNone(self.sender.latest_combo_state)
|
|
self.assertEqual(self.sender.counters["source_timestamp_resets"], 1)
|
|
|
|
def test_source_clock_rejects_missing_timestamp(self) -> None:
|
|
clock_ok, reason = self.sender._check_source_clock(10.0, frame(False))
|
|
|
|
self.assertFalse(clock_ok)
|
|
self.assertIn("missing or malformed", reason)
|
|
self.assertIsNone(self.sender.last_source_timestamp)
|
|
|
|
def test_finish_stop_session_clears_state_and_closes_session(self) -> None:
|
|
session = self.attach_session()
|
|
self.sender.teleop_active = True
|
|
self.sender.teleop_session_id = "active-session"
|
|
self.sender.teleop_session_seq = 42
|
|
self.sender.start_markers_remaining = 7
|
|
|
|
self.sender._finish_stop_session()
|
|
|
|
self.assertFalse(self.sender.teleop_active)
|
|
self.assertIsNone(self.sender.teleop_session_id)
|
|
self.assertEqual(self.sender.teleop_session_seq, 0)
|
|
self.assertEqual(self.sender.start_markers_remaining, 0)
|
|
self.assertIsNone(self.sender.session)
|
|
self.assertTrue(session.closed)
|
|
self.assertEqual(session.sent, [])
|
|
self.assertEqual(self.sender.counters["connected"], 0)
|
|
|
|
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()
|