524 lines
19 KiB
Python
524 lines
19 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_msgs.msg": ("String",),
|
|
"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
|
|
RecordingToggleGate = bridge_module.RecordingToggleGate
|
|
|
|
|
|
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.right_a_pose_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_network_sample_age_is_not_a_runtime_disarm_gate(self) -> None:
|
|
bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
|
|
bridge.cfg = {
|
|
"network": {
|
|
"expected_iarm_id": "IArm-test",
|
|
"expected_iarm_type": "TS1P",
|
|
"minimum_arm_frequency_hz": 30.0,
|
|
},
|
|
"robot": {
|
|
"arm_state_timeout_s": 0.25,
|
|
"state_timeout_s": 1.0,
|
|
"required_state": "HBWALK",
|
|
"required_status": "running",
|
|
},
|
|
"control": {},
|
|
}
|
|
bridge.hands_enabled = False
|
|
bridge.robot_arm_at = 100.0
|
|
bridge.robot_arm_errors = [0] * 14
|
|
bridge.rl_state_at = 100.0
|
|
bridge.rl_state = {
|
|
"current_state": "HBWALK",
|
|
"child_state": "HBWALK",
|
|
"status": "running",
|
|
}
|
|
bridge.foreign_source_seen = False
|
|
bridge.foreign_hand_source_seen = False
|
|
old_network_sample = ArmSnapshot(
|
|
{
|
|
"isomorphic_arm_id": "IArm-test",
|
|
"isomorphic_arm_type": "TS1P",
|
|
"servo_error": {"left": [0] * 7, "right": [0] * 7},
|
|
"joycan_error": [0, 0],
|
|
"freq": {"left": 80.0, "right": 80.0},
|
|
},
|
|
received_at=10.0,
|
|
)
|
|
|
|
reasons = bridge._safety_reasons(
|
|
100.0,
|
|
old_network_sample,
|
|
"",
|
|
check_iarm_target=False,
|
|
check_hands=False,
|
|
check_hand_feedback=False,
|
|
)
|
|
self.assertEqual(reasons, [])
|
|
|
|
def test_two_second_omnisocket_restart_watchdog_remains(self) -> None:
|
|
bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
|
|
bridge.cfg = {
|
|
"network": {
|
|
"transport": "omnisocket",
|
|
"omnisocket_restart_after_stale_s": 2.0,
|
|
}
|
|
}
|
|
active = sample("f" * 32, 1, "active")
|
|
self.assertIsNone(bridge._source_restart_reason(11.999, active))
|
|
self.assertIn(
|
|
"OmniSocket input stale",
|
|
bridge._source_restart_reason(12.0, active),
|
|
)
|
|
|
|
def test_locomotion_reacts_directly_to_sticks_without_buttons(self) -> None:
|
|
bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
|
|
bridge.locomotion_cfg = {
|
|
"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_active = False
|
|
bridge.walk_command = [0.0, 0.0]
|
|
bridge.walk_zero_frames_remaining = 0
|
|
bridge.walk_require_neutral = False
|
|
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, False],
|
|
},
|
|
"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": [0.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, False],
|
|
"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))
|
|
|
|
def test_locomotion_requires_one_neutral_sample_after_arming(self) -> None:
|
|
bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
|
|
bridge.locomotion_cfg = {
|
|
"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_active = False
|
|
bridge.walk_command = [0.0, 0.0]
|
|
bridge.walk_zero_frames_remaining = 0
|
|
bridge.walk_require_neutral = True
|
|
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)
|
|
deflected = ArmSnapshot(
|
|
{"joystick": {"left": [1.0, 0.0], "right": [0.0, 0.0]}},
|
|
received_at=2.0,
|
|
)
|
|
neutral = ArmSnapshot(
|
|
{"joystick": {"left": [0.0, 0.0], "right": [0.0, 0.0]}},
|
|
received_at=2.1,
|
|
)
|
|
bridge._tick_locomotion(2.0, deflected)
|
|
self.assertTrue(bridge.walk_require_neutral)
|
|
self.assertEqual(bridge.walk_command, [0.0, 0.0])
|
|
bridge._tick_locomotion(2.1, neutral)
|
|
self.assertFalse(bridge.walk_require_neutral)
|
|
bridge._tick_locomotion(2.2, deflected)
|
|
self.assertEqual(published[-1], (1.0, -0.0))
|
|
|
|
def test_l3_hold_reaches_nonblocking_capture_request(self) -> None:
|
|
bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
|
|
bridge.data_collection_enabled = True
|
|
bridge.data_collection_cfg = {
|
|
"button_input_timeout_s": 0.25,
|
|
"control_retry_seconds": 0.5,
|
|
"heartbeat_interval_seconds": 0.5,
|
|
}
|
|
bridge.data_collection_gate = RecordingToggleGate(1.0, 0.5)
|
|
bridge.data_collection_gate.new_session()
|
|
bridge.armed = True
|
|
bridge.data_pending_control = None
|
|
bridge.data_last_control_publish_at = 0.0
|
|
bridge.data_recorder_status = {}
|
|
bridge.data_recorder_status_at = 0.0
|
|
bridge.data_last_heartbeat_at = 0.0
|
|
bridge.data_last_iarm_received_at = 0.0
|
|
bridge.data_iarm_publisher = None
|
|
bridge.data_toggle_count = 0
|
|
requested: list[str] = []
|
|
bridge._request_data_capture = MethodType(
|
|
lambda _self, command, _reason: requested.append(command), bridge
|
|
)
|
|
|
|
def frame(now: float, pressed: int) -> object:
|
|
return ArmSnapshot(
|
|
{"button_joystick": {"left": pressed}}, received_at=now
|
|
)
|
|
|
|
bridge._tick_data_collection(10.0, frame(10.0, 0))
|
|
bridge._tick_data_collection(10.5, frame(10.5, 0))
|
|
bridge._tick_data_collection(10.6, frame(10.6, 1))
|
|
bridge._tick_data_collection(11.61, frame(11.61, 1))
|
|
self.assertEqual(requested, ["start"])
|
|
self.assertEqual(bridge.data_toggle_count, 1)
|
|
|
|
def test_rejected_recorder_start_clears_requested_active_state(self) -> None:
|
|
bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
|
|
bridge.data_collection_gate = RecordingToggleGate(1.0, 0.5)
|
|
bridge.data_collection_gate.new_session()
|
|
bridge.data_collection_gate.active = True
|
|
bridge.data_pending_control = {
|
|
"command": "start",
|
|
"request_id": "request-1",
|
|
"event_seq": 4,
|
|
}
|
|
bridge.data_recorder_status = {}
|
|
bridge.data_recorder_status_at = 0.0
|
|
bridge.data_capture_id = "capture-1"
|
|
bridge.data_last_transition = "start_requested"
|
|
bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge)
|
|
message = Dummy()
|
|
message.data = (
|
|
'{"version":1,"state":"failed","capture_id":"capture-1",'
|
|
'"ack_request_id":"request-1","ack_event_seq":4,'
|
|
'"ack_accepted":false,"ack_code":"preflight_failed",'
|
|
'"last_error":"required topic missing"}'
|
|
)
|
|
|
|
bridge._on_data_recorder_status(message)
|
|
self.assertIsNone(bridge.data_pending_control)
|
|
self.assertFalse(bridge.data_collection_gate.active)
|
|
self.assertIn("start_rejected", bridge.data_last_transition)
|
|
|
|
def test_supervisor_context_loss_clears_false_recording_state(self) -> None:
|
|
bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
|
|
bridge.data_collection_gate = RecordingToggleGate(1.0, 0.5)
|
|
bridge.data_collection_gate.new_session()
|
|
bridge.data_collection_gate.active = True
|
|
bridge.data_pending_control = None
|
|
bridge.data_recorder_status = {}
|
|
bridge.data_recorder_status_at = 0.0
|
|
bridge.data_capture_id = "capture-old"
|
|
bridge.data_last_transition = "recording"
|
|
bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge)
|
|
message = Dummy()
|
|
message.data = (
|
|
'{"version":1,"state":"idle","capture_id":null,'
|
|
'"ack_request_id":null,"ack_event_seq":null,'
|
|
'"ack_accepted":null,"last_error":""}'
|
|
)
|
|
|
|
bridge._on_data_recorder_status(message)
|
|
self.assertFalse(bridge.data_collection_gate.active)
|
|
self.assertEqual(
|
|
bridge.data_last_transition,
|
|
"recorder context was lost or replaced",
|
|
)
|
|
|
|
def test_missing_supervisor_ack_cancels_false_start(self) -> None:
|
|
bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
|
|
bridge.data_collection_enabled = True
|
|
bridge.data_collection_cfg = {
|
|
"control_retry_seconds": 0.5,
|
|
"ack_timeout_seconds": 5.0,
|
|
"heartbeat_interval_seconds": 0.5,
|
|
"status_stale_seconds": 4.0,
|
|
}
|
|
bridge.data_collection_gate = RecordingToggleGate(1.0, 0.5)
|
|
bridge.data_collection_gate.new_session()
|
|
bridge.data_collection_gate.active = True
|
|
bridge.armed = False
|
|
bridge.data_pending_control = {"command": "start"}
|
|
bridge.data_pending_control_since = 10.0
|
|
bridge.data_last_control_publish_at = 10.0
|
|
bridge.data_control_publisher = None
|
|
bridge.data_recorder_status = {}
|
|
bridge.data_recorder_status_at = 0.0
|
|
bridge.data_last_heartbeat_at = 0.0
|
|
bridge.data_last_iarm_received_at = 0.0
|
|
bridge.data_iarm_publisher = None
|
|
bridge.data_last_transition = "start_requested"
|
|
bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge)
|
|
|
|
bridge._tick_data_collection(15.0, None)
|
|
self.assertIsNone(bridge.data_pending_control)
|
|
self.assertFalse(bridge.data_collection_gate.active)
|
|
self.assertEqual(bridge.data_last_transition, "start_ack_timeout")
|
|
|
|
def test_stale_recorder_status_requests_capture_stop(self) -> None:
|
|
bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
|
|
bridge.data_collection_enabled = True
|
|
bridge.data_collection_cfg = {
|
|
"control_retry_seconds": 0.5,
|
|
"ack_timeout_seconds": 5.0,
|
|
"heartbeat_interval_seconds": 0.5,
|
|
"status_stale_seconds": 4.0,
|
|
}
|
|
bridge.data_collection_gate = RecordingToggleGate(1.0, 0.5)
|
|
bridge.data_collection_gate.new_session()
|
|
bridge.data_collection_gate.active = True
|
|
bridge.armed = False
|
|
bridge.data_pending_control = None
|
|
bridge.data_pending_control_since = 0.0
|
|
bridge.data_last_control_publish_at = 0.0
|
|
bridge.data_recorder_status = {"state": "recording"}
|
|
bridge.data_recorder_status_at = 10.0
|
|
bridge.data_last_heartbeat_at = 10.0
|
|
bridge.data_last_iarm_received_at = 0.0
|
|
bridge.data_iarm_publisher = None
|
|
bridge.data_last_transition = "recording"
|
|
bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge)
|
|
requested: list[str] = []
|
|
bridge._request_data_capture = MethodType(
|
|
lambda _self, command, _reason: requested.append(command), bridge
|
|
)
|
|
|
|
bridge._tick_data_collection(14.0, None)
|
|
self.assertFalse(bridge.data_collection_gate.active)
|
|
self.assertEqual(requested, ["stop"])
|
|
self.assertEqual(bridge.data_last_transition, "recorder_status_stale")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|