meiqi/TG3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: harden TG3 teleop and add right-B point gesture

Browse Source
This commit is contained in:
LengedZhao
2026-08-08 15:51:21 +08:00
parent eab60273bc
commit 1a79fca36f
17 changed files with 1725 additions and 196 deletions
Download Patch File Download Diff File Expand all files Collapse all files

344
tg3_omnisocket_transport/test_session_gate.py
View File

@@ -1,13 +1,13 @@
#!/usr/bin/env python3
"""Offline tests for the EAI teleoperation session gate."""
from __future__ import annotations
import argparse
import copy
import importlib.util
import json
from pathlib import Path
from types import ModuleType, SimpleNamespace
import sys
from types import ModuleType
import unittest
@@ -22,75 +22,109 @@ spec = importlib.util.spec_from_file_location("omnisocket_xtele_sender", module_
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)
XteleSender = sender_module.XteleSender
def buttons(pressed: bool) -> dict[str, object]:
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": [False, False, pressed],
"right": [False, False, pressed],
},
"hand": {"position": {"left": 0.0, "right": 0.0}},
"left": [0, 0, value],
"right": [0, 0, value],
}
}
class SessionGateTest(unittest.TestCase):
def setUp(self) -> None:
args = SimpleNamespace(
start_stop_hold_s=3.0,
start_marker_frames=50,
source_timeout_s=0.25,
max_feedback_age_ms=500.0,
max_pending_frames=100,
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))
)
self.sender = XteleSender(args)
def test_start_and_stop_each_require_a_new_continuous_hold(self) -> None:
# Boot requires a release, so a button held across service restart
# cannot start a session.
self.assertIsNone(self.sender._update_teleop_gate(0.0, buttons(True)))
self.assertIsNone(self.sender._update_teleop_gate(0.1, buttons(False)))
self.assertIsNone(self.sender._update_teleop_gate(1.0, buttons(True)))
self.assertIsNone(self.sender._update_teleop_gate(3.99, buttons(True)))
self.assertEqual(
self.sender._update_teleop_gate(4.01, buttons(True)), "start"
finished_at = started_at + 0.51
self.assertIsNone(
self.sender._update_teleop_gate(finished_at, frame(False))
)
first_id = self.sender.teleop_session_id
self.assertTrue(self.sender.teleop_active)
self.assertIsNotNone(first_id)
self.assertFalse(self.sender.require_combo_release)
return finished_at
# Keeping the same hold cannot immediately toggle the new session off.
self.assertIsNone(self.sender._update_teleop_gate(8.0, buttons(True)))
self.assertTrue(self.sender.teleop_active)
self.assertIsNone(self.sender._update_teleop_gate(8.1, buttons(False)))
self.assertIsNone(self.sender._update_teleop_gate(9.0, buttons(True)))
self.assertIsNone(self.sender._update_teleop_gate(11.99, buttons(True)))
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(12.01, buttons(True)), "stop"
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)
self.assertEqual(self.sender.teleop_session_id, first_id)
def test_releasing_during_hold_resets_the_timer(self) -> None:
self.sender._update_teleop_gate(0.0, buttons(False))
self.sender._update_teleop_gate(1.0, buttons(True))
self.sender._update_teleop_gate(2.0, buttons(False))
self.sender._update_teleop_gate(3.0, buttons(True))
self.assertIsNone(self.sender._update_teleop_gate(5.9, buttons(True)))
self.assertEqual(
self.sender._update_teleop_gate(6.01, buttons(True)), "start"
)
def test_transport_metadata_is_overwritten(self) -> None:
data = buttons(False)
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,
@@ -98,34 +132,230 @@ class SessionGateTest(unittest.TestCase):
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: int, pending: int) -> None:
self.feedback_age = feedback_age
def __init__(self, feedback_age_ms: int, pending: int) -> None:
self.feedback_age_ms = feedback_age_ms
self.pending = pending
def stats(self) -> dict[str, int]:
@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,
"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())
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())
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__":