218 lines
7.2 KiB
Python
218 lines
7.2 KiB
Python
#!/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()
|