minghanchen
582 lines
21 KiB
Python
582 lines
21 KiB
Python
from __future__ import annotations
|
|
|
|
import json
|
|
import time
|
|
import unittest
|
|
from types import SimpleNamespace
|
|
from unittest.mock import patch
|
|
|
|
from cognitive_agent.infrastructure.can.gateway import CanFrame
|
|
from cognitive_agent.infrastructure.can.hardware import (
|
|
HardwareSafetyError,
|
|
VehicleMqttSettings,
|
|
)
|
|
from cognitive_agent.infrastructure.can.mqtt_transport import (
|
|
MQTT_CAN_SCHEMA,
|
|
MqttCanTransport,
|
|
parse_mqtt_can_payload,
|
|
)
|
|
|
|
|
|
class _PublishResult:
|
|
rc = 0
|
|
|
|
def wait_for_publish(self, timeout: float) -> bool:
|
|
return timeout > 0
|
|
|
|
|
|
class _FakeMqttClient:
|
|
def __init__(self) -> None:
|
|
self.on_connect = None
|
|
self.on_disconnect = None
|
|
self.on_message = None
|
|
self.subscriptions: list[tuple[str, int]] = []
|
|
self.published: list[tuple[str, str, int, bool]] = []
|
|
|
|
def reconnect_delay_set(self, **_kwargs) -> None:
|
|
return None
|
|
|
|
def connect_async(self, host: str, port: int, keepalive: int) -> None:
|
|
self.connection = (host, port, keepalive)
|
|
|
|
def loop_start(self) -> None:
|
|
self.on_connect(self, None, {}, 0, None)
|
|
|
|
def loop_stop(self) -> None:
|
|
return None
|
|
|
|
def disconnect(self) -> None:
|
|
return None
|
|
|
|
def subscribe(self, topic: str, qos: int):
|
|
self.subscriptions.append((topic, qos))
|
|
return (0, 1)
|
|
|
|
def publish(self, topic: str, payload: str, qos: int, retain: bool):
|
|
self.published.append((topic, payload, qos, retain))
|
|
return _PublishResult()
|
|
|
|
def emit(self, topic: str, payload: object) -> None:
|
|
message = SimpleNamespace(
|
|
topic=topic,
|
|
payload=json.dumps(payload).encode("utf-8"),
|
|
)
|
|
self.on_message(self, None, message)
|
|
|
|
|
|
def _settings():
|
|
values = {
|
|
"enabled": True,
|
|
"state_configured": True,
|
|
"configured": True,
|
|
"host": "127.0.0.1",
|
|
"port": 1883,
|
|
"condition_topic": "n7/n7-headunit/can/state",
|
|
"control_topic": "n7/can_control",
|
|
"qos": 1,
|
|
"keepalive_s": 30,
|
|
"username": None,
|
|
"password": None,
|
|
"tls_enabled": False,
|
|
"feedback_timeout_ms": 100,
|
|
}
|
|
values["public_info"] = lambda: {
|
|
"enabled": True,
|
|
"configured": True,
|
|
"state_configured": True,
|
|
"backend": "mqtt",
|
|
"condition_topic": "n7/n7-headunit/can/state",
|
|
"control_topic": "n7/can_control",
|
|
}
|
|
return SimpleNamespace(**values)
|
|
|
|
|
|
class MqttCanPayloadTests(unittest.TestCase):
|
|
def test_accepts_one_canonical_frame_and_common_receive_aliases(self) -> None:
|
|
canonical = parse_mqtt_can_payload(
|
|
json.dumps(
|
|
{
|
|
"schema": MQTT_CAN_SCHEMA,
|
|
"arbitration_id": 0x342,
|
|
"channel": 1,
|
|
"data_hex": "00 00 00 01 00 00 00 00",
|
|
"is_fd": True,
|
|
"bitrate_switch": True,
|
|
}
|
|
)
|
|
)[0]
|
|
alias = parse_mqtt_can_payload(
|
|
json.dumps({"canId": "0x4FA", "data": [0] * 8})
|
|
)[0]
|
|
self.assertEqual(canonical.arbitration_id, 0x342)
|
|
self.assertEqual(canonical.channel, 1)
|
|
self.assertTrue(canonical.is_fd)
|
|
self.assertEqual(alias.arbitration_id, 0x4FA)
|
|
|
|
def test_accepts_vehicle_n7_can_signals_envelope(self) -> None:
|
|
frames = parse_mqtt_can_payload(
|
|
json.dumps(
|
|
{
|
|
"schema": "n7-can-signals/v1",
|
|
"sequence": 335,
|
|
"timestamp": 1787037724090,
|
|
"online": True,
|
|
"signals": [
|
|
{
|
|
"channel": "can1",
|
|
"channelIndex": 1,
|
|
"canId": "0x388",
|
|
"canIdDecimal": 904,
|
|
"frameType": "fd",
|
|
"brs": True,
|
|
"dlc": 8,
|
|
"data": "00 00 00 02 01 00 59 00",
|
|
"bytes": [0, 0, 0, 2, 1, 0, 89, 0],
|
|
"timestamp": 1787037667665,
|
|
"direction": "rx",
|
|
}
|
|
],
|
|
"can1Vehicle": {
|
|
"parsed": {"speedKph": 0, "gear": 1}
|
|
},
|
|
}
|
|
)
|
|
)
|
|
self.assertEqual(len(frames), 1)
|
|
self.assertEqual(frames[0].arbitration_id, 0x388)
|
|
self.assertEqual(frames[0].channel, 1)
|
|
self.assertEqual(frames[0].data, bytes.fromhex("00 00 00 02 01 00 59 00"))
|
|
self.assertTrue(frames[0].is_fd)
|
|
self.assertTrue(frames[0].bitrate_switch)
|
|
self.assertEqual(frames[0].timestamp, 1787037667.665)
|
|
|
|
def test_accepts_nested_can1_vehicle_signals_when_top_level_is_absent(self) -> None:
|
|
frame = parse_mqtt_can_payload(
|
|
json.dumps(
|
|
{
|
|
"schema": "n7-can-signals/v1",
|
|
"can1Vehicle": {
|
|
"signals": [
|
|
{
|
|
"channel": "can1",
|
|
"canId": "0x42E",
|
|
"frameType": "fd",
|
|
"brs": True,
|
|
"bytes": [17, 100, 50, 3, 144, 2, 0, 198],
|
|
}
|
|
]
|
|
},
|
|
}
|
|
)
|
|
)[0]
|
|
self.assertEqual(frame.arbitration_id, 0x42E)
|
|
self.assertTrue(frame.is_fd)
|
|
|
|
def test_accepts_nested_can0_seat_signals_with_can0_default(self) -> None:
|
|
frame = parse_mqtt_can_payload(
|
|
json.dumps(
|
|
{
|
|
"schema": "n7-can-signals/v1",
|
|
"can0Seat": {
|
|
"signals": [
|
|
{
|
|
"canId": "0x302",
|
|
"frameType": "classic",
|
|
"bytes": [0] * 8,
|
|
}
|
|
]
|
|
},
|
|
}
|
|
)
|
|
)[0]
|
|
self.assertEqual(frame.arbitration_id, 0x302)
|
|
self.assertEqual(frame.channel, 0)
|
|
self.assertFalse(frame.is_fd)
|
|
|
|
def test_translates_seat_ota_info_responses_into_four_position_states(self) -> None:
|
|
values = {
|
|
1: (480 << 16) | 407,
|
|
2: (373 << 16) | 266,
|
|
3: (917 << 16) | 451,
|
|
4: (647 << 16) | 575,
|
|
5: 0x11,
|
|
6: 0x11,
|
|
7: 0x11,
|
|
8: 0x11,
|
|
}
|
|
signals = []
|
|
for selector, value in values.items():
|
|
sequence = 0x20 + selector
|
|
signals.extend(
|
|
(
|
|
{
|
|
"channel": "can0",
|
|
"canId": "0x6A0",
|
|
"frameType": "classic",
|
|
"direction": "tx",
|
|
"bytes": [0xC7, sequence, 0x60, selector, 0, 0, 0, 0],
|
|
},
|
|
{
|
|
"channel": "can0",
|
|
"canId": "0x6A2",
|
|
"frameType": "classic",
|
|
"direction": "rx",
|
|
"bytes": [0x7C, sequence, 0x60, 0, *value.to_bytes(4, "little")],
|
|
},
|
|
)
|
|
)
|
|
|
|
frames = parse_mqtt_can_payload(json.dumps({"signals": signals}))
|
|
positions = [frame for frame in frames if frame.arbitration_id == 0x302]
|
|
|
|
self.assertEqual([frame.data[0] for frame in positions], [0, 1, 2, 3])
|
|
self.assertEqual(
|
|
[int.from_bytes(frame.data[5:7], "little") for frame in positions],
|
|
[round(407 * 10_000 / 480), round(266 * 10_000 / 373),
|
|
round(451 * 10_000 / 917), round(575 * 10_000 / 647)],
|
|
)
|
|
self.assertTrue(all(frame.data[2] & (1 << 3) for frame in positions))
|
|
|
|
def test_rejects_can0_frames_outside_seat_firmware_format(self) -> None:
|
|
for label, frame in (
|
|
(
|
|
"fd",
|
|
{
|
|
"channel": "can0",
|
|
"canId": "0x302",
|
|
"frameType": "fd",
|
|
"bytes": [0] * 8,
|
|
},
|
|
),
|
|
(
|
|
"brs",
|
|
{
|
|
"channel": "can0",
|
|
"canId": "0x302",
|
|
"frameType": "classic",
|
|
"brs": True,
|
|
"bytes": [0] * 8,
|
|
},
|
|
),
|
|
(
|
|
"dlc",
|
|
{
|
|
"channel": "can0",
|
|
"canId": "0x302",
|
|
"frameType": "classic",
|
|
"bytes": [0] * 7,
|
|
},
|
|
),
|
|
):
|
|
with self.subTest(label=label), self.assertRaisesRegex(
|
|
ValueError, "classic CAN, no BRS, and DLC 8"
|
|
):
|
|
parse_mqtt_can_payload(json.dumps(frame))
|
|
|
|
def test_top_level_signals_can_mix_can0_and_can1_without_nested_duplicates(self) -> None:
|
|
frames = parse_mqtt_can_payload(
|
|
json.dumps(
|
|
{
|
|
"schema": "n7-can-signals/v1",
|
|
"signals": [
|
|
{"channel": "can0", "canId": "0x302", "bytes": [0] * 8},
|
|
{"channel": "can1", "canId": "0x342", "bytes": [0] * 8},
|
|
],
|
|
"can0Seat": {
|
|
"signals": [
|
|
{"channel": "can0", "canId": "0x302", "bytes": [0] * 8}
|
|
]
|
|
},
|
|
}
|
|
)
|
|
)
|
|
self.assertEqual(
|
|
[(frame.channel, frame.arbitration_id) for frame in frames],
|
|
[(0, 0x302), (1, 0x342)],
|
|
)
|
|
|
|
def test_rejects_decoded_state_invalid_channels_and_channel_conflicts(self) -> None:
|
|
with self.assertRaisesRegex(ValueError, "requires an ID and data"):
|
|
parse_mqtt_can_payload(json.dumps({"gear": "P", "speed_kmh": 0}))
|
|
with self.assertRaisesRegex(ValueError, "only CAN0 or CAN1"):
|
|
parse_mqtt_can_payload(
|
|
json.dumps({"id": 0x342, "channel": 2, "data": [0] * 8})
|
|
)
|
|
with self.assertRaisesRegex(ValueError, "declarations conflict"):
|
|
parse_mqtt_can_payload(
|
|
json.dumps(
|
|
{
|
|
"id": 0x342,
|
|
"channel": "can1",
|
|
"channelIndex": 0,
|
|
"data": [0] * 8,
|
|
}
|
|
)
|
|
)
|
|
|
|
def test_control_remains_locked_without_existing_hardware_approval(self) -> None:
|
|
settings = VehicleMqttSettings(
|
|
enabled=True,
|
|
approval_required=True,
|
|
approval_token="",
|
|
host="127.0.0.1",
|
|
port=1883,
|
|
condition_topic="n7/n7-headunit/can/state",
|
|
control_topic="n7/can_control",
|
|
qos=1,
|
|
keepalive_s=30,
|
|
username=None,
|
|
password=None,
|
|
tls_enabled=False,
|
|
feedback_timeout_ms=1_500,
|
|
)
|
|
with patch(
|
|
"cognitive_agent.infrastructure.can.hardware._module_available",
|
|
return_value=True,
|
|
):
|
|
self.assertTrue(settings.state_configured)
|
|
self.assertFalse(settings.configured)
|
|
with self.assertRaisesRegex(HardwareSafetyError, "not configured"):
|
|
settings.authorize(None)
|
|
|
|
|
|
class MqttCanTransportTests(unittest.TestCase):
|
|
def setUp(self) -> None:
|
|
self.client = _FakeMqttClient()
|
|
self.transport = MqttCanTransport(
|
|
_settings(),
|
|
client_factory=lambda: self.client,
|
|
)
|
|
self.transport.start()
|
|
|
|
def tearDown(self) -> None:
|
|
self.transport.close()
|
|
|
|
def test_condition_frames_use_existing_decoder_and_snapshot_contract(self) -> None:
|
|
self.client.emit(
|
|
"n7/n7-headunit/can/state",
|
|
{
|
|
"frames": [
|
|
{"id": 0x342, "data": [0, 0, 0, 1, 0, 0, 0, 0]},
|
|
{"id": 0x4EB, "data": [7, 0, 0, 0, 0, 0, 0, 0]},
|
|
{"id": 0x407, "data": [0] * 8},
|
|
{"id": 0x494, "data": [0] * 8},
|
|
{"id": 0x4FA, "data": [0] * 8},
|
|
{"id": 0x555, "data": [255] * 8},
|
|
]
|
|
},
|
|
)
|
|
snapshot = self.transport.snapshot()
|
|
self.assertEqual(
|
|
snapshot.observed_ids,
|
|
frozenset({0x342, 0x4EB, 0x407, 0x494, 0x4FA}),
|
|
)
|
|
self.assertTrue(
|
|
all(
|
|
state["verification"]["transport"] == "wireless_mqtt"
|
|
for state in snapshot.feedback_states
|
|
)
|
|
)
|
|
self.assertEqual(
|
|
self.client.subscriptions,
|
|
[("n7/n7-headunit/can/state", 1)],
|
|
)
|
|
|
|
def test_first_partial_live_vehicle_snapshot_does_not_wait_for_missing_ids(self) -> None:
|
|
self.client.emit(
|
|
"n7/n7-headunit/can/state",
|
|
{
|
|
"signals": [
|
|
{
|
|
"channel": "can1",
|
|
"canId": "0x388",
|
|
"frameType": "fd",
|
|
"brs": True,
|
|
"bytes": [0, 0, 0, 2, 1, 0, 89, 0],
|
|
}
|
|
]
|
|
},
|
|
)
|
|
|
|
started = time.perf_counter()
|
|
snapshot = self.transport.snapshot()
|
|
|
|
self.assertLess(time.perf_counter() - started, 0.05)
|
|
self.assertEqual(snapshot.observed_ids, frozenset({0x388}))
|
|
|
|
def test_stale_safety_signal_cannot_enter_snapshot_or_executor_bus(self) -> None:
|
|
bus = self.transport.bus(channel=1)
|
|
self.client.emit(
|
|
"n7/n7-headunit/can/state",
|
|
{
|
|
"schema": "n7-can-signals/v1",
|
|
"signals": [
|
|
{
|
|
"channel": "can1",
|
|
"channelIndex": 1,
|
|
"canId": "0x342",
|
|
"frameType": "fd",
|
|
"brs": True,
|
|
"data": "00 00 00 01 00 00 00 00",
|
|
"timestamp": round((time.time() - 60) * 1_000),
|
|
}
|
|
],
|
|
},
|
|
)
|
|
snapshot = self.transport.snapshot()
|
|
info = self.transport.public_info()["state_sync"]
|
|
self.assertEqual(info["capture_sequence"], 1)
|
|
self.assertEqual(info["stale_frames"], 1)
|
|
self.assertEqual(info["observed_frame_ids"], [])
|
|
self.assertEqual(snapshot.observed_ids, frozenset())
|
|
self.assertIsNone(bus.receive(0, include_fd=True))
|
|
|
|
def test_stale_non_safety_can1_frames_are_display_only(self) -> None:
|
|
bus = self.transport.bus(channel=1)
|
|
cached_timestamp = round((time.time() - 60) * 1_000)
|
|
self.client.emit(
|
|
"n7/n7-headunit/can/state",
|
|
{
|
|
"schema": "n7-can-signals/v1",
|
|
"signals": [
|
|
{
|
|
"channel": "can1",
|
|
"canId": frame_id,
|
|
"frameType": "fd",
|
|
"brs": True,
|
|
"data": data,
|
|
"timestamp": cached_timestamp,
|
|
}
|
|
for frame_id, data in (
|
|
("0x388", "00 00 00 02 01 00 59 00"),
|
|
("0x41A", "00 5C 85 08 20 42 C1 FD"),
|
|
("0x42E", "11 64 32 03 90 02 00 C6"),
|
|
("0x3FF", "5B 09 FF 0F 32 61 04 12"),
|
|
)
|
|
],
|
|
},
|
|
)
|
|
|
|
snapshot = self.transport.snapshot()
|
|
self.assertEqual(
|
|
snapshot.observed_ids,
|
|
frozenset({0x388, 0x3FF, 0x41A, 0x42E}),
|
|
)
|
|
for state in snapshot.feedback_states:
|
|
verification = state["verification"]
|
|
self.assertFalse(verification["fresh"])
|
|
self.assertTrue(verification["display_only"])
|
|
self.assertFalse(verification["control_eligible"])
|
|
self.assertGreater(verification["cache_age_ms"], 50_000)
|
|
self.assertIsNone(bus.receive(0, include_fd=True))
|
|
|
|
def test_bus_publishes_only_canonical_frame_contract(self) -> None:
|
|
bus = self.transport.bus()
|
|
sent = bus.send(
|
|
0x431,
|
|
bytes.fromhex("10 00 00 00 00 00 00 00"),
|
|
is_fd=True,
|
|
bitrate_switch=True,
|
|
)
|
|
self.assertEqual(sent.channel, 1)
|
|
self.assertEqual(len(self.client.published), 1)
|
|
topic, raw_payload, qos, retain = self.client.published[0]
|
|
payload = json.loads(raw_payload)
|
|
self.assertEqual(topic, "n7/can_control")
|
|
self.assertEqual(qos, 1)
|
|
self.assertFalse(retain)
|
|
self.assertEqual(payload["schema"], MQTT_CAN_SCHEMA)
|
|
self.assertEqual(payload["arbitration_id"], 0x431)
|
|
self.assertEqual(payload["data_hex"], "10 00 00 00 00 00 00 00")
|
|
self.assertTrue(payload["is_fd"])
|
|
self.assertTrue(payload["bitrate_switch"])
|
|
with self.assertRaisesRegex(Exception, "approved CAN1 control set"):
|
|
bus.send(0x555, bytes(8), is_fd=True)
|
|
self.assertEqual(len(self.client.published), 1)
|
|
|
|
def test_can0_bus_publishes_only_classic_passenger_seat_control(self) -> None:
|
|
bus = self.transport.bus(channel=0)
|
|
sent = bus.send(0x6A0, bytes.fromhex("C7 01 64 00 F4 01 00 00"))
|
|
payload = json.loads(self.client.published[-1][1])
|
|
self.assertEqual(sent.channel, 0)
|
|
self.assertEqual(payload["channel"], 0)
|
|
self.assertFalse(payload["is_fd"])
|
|
with self.assertRaisesRegex(Exception, "approved CAN0 control set"):
|
|
bus.send(0x431, bytes(8))
|
|
with self.assertRaisesRegex(Exception, "requires classic CAN"):
|
|
bus.send(0x6A0, bytes.fromhex("C7 01 64 00 F4 01 00 00"),
|
|
is_fd=True, bitrate_switch=True)
|
|
with self.assertRaisesRegex(Exception, "approved CAN0 control set"):
|
|
bus.send(0x300, bytes(8))
|
|
with self.assertRaisesRegex(Exception, "only STOP, MOVE, HEARTBEAT, or DYNAMIC"):
|
|
bus.send(0x6A0, bytes.fromhex("C7 02 61 00 00 00 00 00"))
|
|
with self.assertRaisesRegex(Exception, "target 0..1000"):
|
|
bus.send(0x6A0, bytes.fromhex("C7 03 64 00 E9 03 00 00"))
|
|
with self.assertRaisesRegex(Exception, "approved channel policy"):
|
|
self.transport.publish(
|
|
CanFrame(arbitration_id=0x431, data=bytes(8), channel=0)
|
|
)
|
|
|
|
def test_bus_receive_views_are_isolated_by_channel(self) -> None:
|
|
can0 = self.transport.bus(channel=0)
|
|
can1 = self.transport.bus(channel=1)
|
|
self.client.emit(
|
|
"n7/n7-headunit/can/state",
|
|
{
|
|
"signals": [
|
|
{"channel": "can0", "canId": "0x302", "bytes": [0] * 8},
|
|
{
|
|
"channel": "can1",
|
|
"canId": "0x342",
|
|
"frameType": "fd",
|
|
"bytes": [0] * 8,
|
|
},
|
|
]
|
|
},
|
|
)
|
|
self.assertEqual(can0.receive(0).arbitration_id, 0x302)
|
|
self.assertEqual(can1.receive(0, include_fd=True).arbitration_id, 0x342)
|
|
self.assertIsNone(can0.receive(0))
|
|
self.assertIsNone(can1.receive(0, include_fd=True))
|
|
|
|
def test_can0_status_bus_can_replay_latest_translated_ota_positions(self) -> None:
|
|
signals = []
|
|
for selector, value in {
|
|
1: (480 << 16) | 407,
|
|
2: (373 << 16) | 266,
|
|
3: (917 << 16) | 451,
|
|
4: (647 << 16) | 575,
|
|
5: 0x11,
|
|
6: 0x11,
|
|
7: 0x11,
|
|
8: 0x11,
|
|
}.items():
|
|
sequence = 0x40 + selector
|
|
signals.extend(
|
|
(
|
|
{"channel": "can0", "canId": "0x6A0", "bytes":
|
|
[0xC7, sequence, 0x60, selector, 0, 0, 0, 0]},
|
|
{"channel": "can0", "canId": "0x6A2", "bytes":
|
|
[0x7C, sequence, 0x60, 0, *value.to_bytes(4, "little")]},
|
|
)
|
|
)
|
|
self.client.emit("n7/n7-headunit/can/state", {"signals": signals})
|
|
|
|
bus = self.transport.bus(channel=0, replay_latest=True)
|
|
replayed = [bus.receive(0) for _ in range(4)]
|
|
|
|
self.assertEqual(
|
|
[frame.data[0] for frame in replayed if frame is not None],
|
|
[0, 1, 2, 3],
|
|
)
|
|
self.assertEqual(bus.receive(0).arbitration_id, 0x6A2)
|
|
self.assertIsNone(bus.receive(0))
|
|
self.assertEqual(
|
|
self.transport.public_info()["state_sync"]["observed_frame_ids_by_channel"]["0"],
|
|
["0x302", "0x6A2"],
|
|
)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|