#!/usr/bin/env python3 """Offline protocol tests for the robot-side session gate.""" from __future__ import annotations import importlib.util from pathlib import Path from types import MethodType, ModuleType import sys import unittest class Dummy: pass def install_module(name: str, **attributes: object) -> ModuleType: module = ModuleType(name) for key, value in attributes.items(): setattr(module, key, value) sys.modules[name] = module return module install_module("rclpy") install_module("rclpy.node", Node=Dummy) for package, names in { "brainco_hand_msgs.msg": ("MotorStatus", "SetMotorMulti"), "diagnostic_msgs.msg": ("DiagnosticStatus",), "geometry_msgs.msg": ("TwistStamped",), "ros2_bridge_msgs.msg": ("ArmStatus",), "sensor_msgs.msg": ("JointState",), "std_srvs.srv": ("Trigger",), }.items(): install_module(package, **{name: Dummy for name in names}) module_path = Path(__file__).with_name("tg3_local_teleop.py") spec = importlib.util.spec_from_file_location("tg3_local_teleop_module", module_path) assert spec is not None and spec.loader is not None bridge_module = importlib.util.module_from_spec(spec) sys.modules[spec.name] = bridge_module spec.loader.exec_module(bridge_module) ArmSnapshot = bridge_module.ArmSnapshot LocalTeleopBridge = bridge_module.LocalTeleopBridge class NullLogger: def info(self, _message: str) -> None: pass def warning(self, _message: str) -> None: pass def error(self, _message: str) -> None: pass def sample(session_id: str, seq: int, state: str, reason: str = "") -> object: metadata: dict[str, object] = { "protocol_version": 2, "session_id": session_id, "session_seq": seq, "session_state": state, } if reason: metadata["stop_reason"] = reason return ArmSnapshot({"tg3_transport": metadata}, received_at=10.0) class RobotSessionGateTest(unittest.TestCase): def make_bridge(self) -> object: bridge = LocalTeleopBridge.__new__(LocalTeleopBridge) bridge.last_session_state = "inactive" bridge.last_session_stop_reason = "" bridge.active_session_id = None bridge.session_start_attempted_id = None bridge.armed = False bridge.returning_home = False bridge.allow_publish = True bridge.cfg = {"control": {"auto_home_on_stop": True}} bridge.hands_enabled = False bridge.right_point_gesture_enabled = False bridge.robot_arm_positions = [0.0] * 14 bridge.last_command = None bridge.last_publish_at = 0.0 bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge) bridge._safety_reasons = MethodType( lambda _self, _now, _sample, _error: [], bridge ) def disarm(self: object, _reason: str) -> None: self.armed = False bridge._disarm = MethodType(disarm, bridge) bridge.home_started = False def start_home( self: object, _now: float, _sample: object, _error: str, _reason: str, ) -> bool: self.home_started = True return True bridge._start_home_if_safe = MethodType(start_home, bridge) return bridge def test_start_active_stop_is_session_scoped(self) -> None: bridge = self.make_bridge() session_id = "a" * 32 bridge._update_session_gate(10.0, sample(session_id, 1, "start"), "") self.assertTrue(bridge.armed) self.assertEqual(bridge.active_session_id, session_id) bridge._update_session_gate(10.1, sample(session_id, 2, "active"), "") self.assertTrue(bridge.armed) bridge._update_session_gate( 10.2, sample("b" * 32, 3, "stop", "operator"), "" ) self.assertTrue(bridge.armed) bridge._update_session_gate( 10.3, sample(session_id, 4, "stop", "operator"), "" ) self.assertFalse(bridge.armed) self.assertIsNone(bridge.active_session_id) self.assertTrue(bridge.home_started) def test_active_without_start_never_arms(self) -> None: bridge = self.make_bridge() bridge._update_session_gate(10.0, sample("c" * 32, 1, "active"), "") self.assertFalse(bridge.armed) self.assertIsNone(bridge.active_session_id) def test_rejected_start_is_not_retried_in_same_session(self) -> None: bridge = self.make_bridge() calls = 0 def reject(_self: object, _now: float, _sample: object, _error: str) -> list[str]: nonlocal calls calls += 1 return ["blocked"] bridge._safety_reasons = MethodType(reject, bridge) session_id = "d" * 32 bridge._update_session_gate(10.0, sample(session_id, 1, "start"), "") bridge._update_session_gate(10.1, sample(session_id, 2, "start"), "") bridge._update_session_gate(10.2, sample(session_id, 3, "active"), "") self.assertEqual(calls, 1) self.assertFalse(bridge.armed) def test_metadata_validation(self) -> None: valid = { "tg3_transport": { "protocol_version": 2, "session_id": "e" * 32, "session_seq": 1, "session_state": "start", } } self.assertIsNotNone(LocalTeleopBridge._teleop_session_info(valid)) valid["tg3_transport"]["protocol_version"] = 1 self.assertIsNone(LocalTeleopBridge._teleop_session_info(valid)) def test_locomotion_reacts_immediately_after_repress(self) -> None: bridge = LocalTeleopBridge.__new__(LocalTeleopBridge) bridge.locomotion_cfg = { "hold_seconds": 0.0, "joystick_deadzone": 0.2, "joystick_expo": 2.0, "yaw_joystick_expo": 1.0, "max_forward_m_s": 1.0, "max_reverse_m_s": 0.8, "max_angular_rad_s": 0.8, "forward_axis_sign": 1.0, "yaw_axis_sign": -1.0, "zero_burst_frames": 10, } bridge.walk_combo_started_at = None bridge.walk_active = False bridge.walk_command = [0.0, 0.0] bridge.walk_zero_frames_remaining = 0 bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge) published: list[tuple[float, float]] = [] def publish( self: object, linear_x: float, angular_z: float, _now: float ) -> None: published.append((linear_x, angular_z)) self.walk_command = [linear_x, angular_z] bridge._publish_walk = MethodType(publish, bridge) moving = ArmSnapshot( { "button": { "left": [False, False, False], "right": [False, False, True], }, "joystick": { "left": [1.0, 0.0], "right": [0.0, 0.0], }, }, received_at=1.0, ) released = ArmSnapshot( { "button": { "left": [False, False, False], "right": [False, False, False], }, "joystick": { "left": [1.0, 0.0], "right": [0.0, 0.0], }, }, received_at=1.1, ) bridge._tick_locomotion(1.0, moving) self.assertEqual(published[-1], (1.0, -0.0)) bridge._tick_locomotion(1.1, released) self.assertEqual(published[-1], (0.0, 0.0)) bridge._tick_locomotion(1.2, moving) self.assertEqual(published[-1], (1.0, -0.0)) turning = ArmSnapshot( { "button": { "left": [False, False, True], "right": [False, False, False], }, "joystick": { "left": [0.0, 0.0], "right": [0.0, 1.0], }, }, received_at=1.3, ) bridge._tick_locomotion(1.3, turning) self.assertEqual(published[-1], (0.0, -0.8)) old_wrong_binding = ArmSnapshot( { "button": { "left": [False, False, False], "right": [False, False, True], }, "joystick": { "left": [0.0, 1.0], "right": [0.0, 0.0], }, }, received_at=1.4, ) bridge._tick_locomotion(1.4, old_wrong_binding) self.assertEqual(published[-1], (0.0, 0.0)) if __name__ == "__main__": unittest.main()