feat: add session-gated TG3 data collection

tg3_local_teleop/test_session_gate.py

@@ -30,6 +30,7 @@ for package, names in {
     "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})
@@ -42,6 +43,7 @@ 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:
@@ -321,6 +323,158 @@ class RobotSessionGateTest(unittest.TestCase):
         bridge._tick_locomotion(1.4, old_wrong_binding)
         self.assertEqual(published[-1], (0.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()