feat: package TG3 TS1P OmniSocket teleoperation
This commit is contained in:
217
tg3_local_teleop/test_session_gate.py
Normal file
217
tg3_local_teleop/test_session_gate.py
Normal file
@@ -0,0 +1,217 @@
|
||||
#!/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.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,
|
||||
"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": {"right": [False, False, True]},
|
||||
"joystick": {"left": [1.0, 0.0]},
|
||||
},
|
||||
received_at=1.0,
|
||||
)
|
||||
released = ArmSnapshot(
|
||||
{
|
||||
"button": {"right": [False, False, False]},
|
||||
"joystick": {"left": [1.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))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Reference in New Issue
Block a user