minghanchen
245 lines
9.1 KiB
Python
245 lines
9.1 KiB
Python
"""Persistent read-only CH1 vehicle-state receiver for the Linux ZLG backend."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import threading
|
|
import time
|
|
from collections.abc import Callable
|
|
from copy import deepcopy
|
|
from dataclasses import dataclass, replace
|
|
from datetime import UTC, datetime
|
|
from typing import Any
|
|
|
|
from cognitive_agent.application.native_can_feedback import decode_feedback
|
|
from cognitive_agent.infrastructure.can.gateway import CanFrame, CanGatewayError
|
|
from cognitive_agent.infrastructure.can.hardware import VehicleCanSettings
|
|
|
|
REQUIRED_SEAT_IDS = frozenset({0x4FA, 0x494})
|
|
REQUIRED_BODY_IDS = frozenset({0x342, 0x4EB, 0x407})
|
|
REQUIRED_UI_IDS = REQUIRED_SEAT_IDS | REQUIRED_BODY_IDS
|
|
OBSERVABLE_IDS = REQUIRED_UI_IDS | frozenset(
|
|
{0x388, 0x38A, 0x3F3, 0x3FF, 0x404, 0x41A, 0x42E}
|
|
)
|
|
PERSISTENT_MONITOR_POLL_INTERVAL_MS = 500
|
|
|
|
|
|
def _now() -> str:
|
|
return datetime.now(UTC).isoformat().replace("+00:00", "Z")
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class VehicleStateSnapshot:
|
|
feedback_states: tuple[dict[str, Any], ...]
|
|
observed_ids: frozenset[int]
|
|
captured_at: str | None
|
|
capture_sequence: int
|
|
|
|
|
|
class VehicleStateMonitor:
|
|
"""Keep one monitor-only adapter owner online and cache fresh decoded state."""
|
|
|
|
def __init__(
|
|
self,
|
|
settings: VehicleCanSettings,
|
|
*,
|
|
decoder: Callable[[CanFrame], dict[str, Any]] = decode_feedback,
|
|
reconnect_delay_s: float = 1.0,
|
|
) -> None:
|
|
self.settings = settings
|
|
self.decoder = decoder
|
|
self.reconnect_delay_s = max(0.01, float(reconnect_delay_s))
|
|
self.initial_snapshot_timeout_s = max(0, settings.feedback_timeout_ms) / 1000
|
|
self.freshness_s = max(3.0, self.initial_snapshot_timeout_s * 2)
|
|
|
|
self._condition = threading.Condition(threading.RLock())
|
|
self._stop_event = threading.Event()
|
|
self._thread: threading.Thread | None = None
|
|
self._bridge: Any | None = None
|
|
self._latest: dict[int, tuple[float, dict[str, Any]]] = {}
|
|
self._baseline_ready = False
|
|
self._capture_sequence = 0
|
|
self._state = "idle"
|
|
self._last_error: str | None = None
|
|
|
|
def start(self) -> None:
|
|
with self._condition:
|
|
if self._thread is not None and self._thread.is_alive():
|
|
return
|
|
self._stop_event.clear()
|
|
self._state = "connecting"
|
|
self._thread = threading.Thread(
|
|
target=self._run,
|
|
name="zlg-linux-vehicle-state-monitor",
|
|
daemon=True,
|
|
)
|
|
self._thread.start()
|
|
|
|
def close(self) -> None:
|
|
self._stop_event.set()
|
|
with self._condition:
|
|
self._condition.notify_all()
|
|
thread = self._thread
|
|
if thread is not None and thread is not threading.current_thread():
|
|
thread.join(timeout=2.0)
|
|
with self._condition:
|
|
bridge = self._bridge
|
|
if thread is not None and thread.is_alive() and bridge is not None:
|
|
bridge.close()
|
|
thread.join(timeout=1.0)
|
|
with self._condition:
|
|
if self._thread is None or not self._thread.is_alive():
|
|
self._state = "stopped"
|
|
self._thread = None
|
|
self._condition.notify_all()
|
|
|
|
def snapshot(self) -> VehicleStateSnapshot:
|
|
"""Return cached fresh state, waiting only for the first required baseline."""
|
|
|
|
self.start()
|
|
deadline = time.monotonic() + self.initial_snapshot_timeout_s
|
|
with self._condition:
|
|
while not self._baseline_ready:
|
|
fresh_ids = self._fresh_ids(time.monotonic())
|
|
if REQUIRED_UI_IDS <= fresh_ids:
|
|
self._baseline_ready = True
|
|
break
|
|
if self._state == "error" and not fresh_ids:
|
|
raise CanGatewayError(
|
|
self._last_error or "Linux vehicle-state monitor is unavailable"
|
|
)
|
|
remaining_s = deadline - time.monotonic()
|
|
if remaining_s <= 0 or self._stop_event.is_set():
|
|
break
|
|
self._condition.wait(timeout=remaining_s)
|
|
|
|
now = time.monotonic()
|
|
observed_ids = self._fresh_ids(now)
|
|
if self._state == "error" and not observed_ids:
|
|
raise CanGatewayError(
|
|
self._last_error or "Linux vehicle-state monitor is unavailable"
|
|
)
|
|
feedback_states = tuple(
|
|
deepcopy(self._latest[frame_id][1])
|
|
for frame_id in sorted(observed_ids)
|
|
)
|
|
captured_at = max(
|
|
(
|
|
str(state.get("captured_at"))
|
|
for state in feedback_states
|
|
if state.get("captured_at")
|
|
),
|
|
default=None,
|
|
)
|
|
return VehicleStateSnapshot(
|
|
feedback_states=feedback_states,
|
|
observed_ids=frozenset(observed_ids),
|
|
captured_at=captured_at,
|
|
capture_sequence=self._capture_sequence,
|
|
)
|
|
|
|
def public_info(self) -> dict[str, Any]:
|
|
with self._condition:
|
|
return {
|
|
"mode": "persistent_monitor",
|
|
"state": self._state,
|
|
"read_only": True,
|
|
"poll_interval_ms": PERSISTENT_MONITOR_POLL_INTERVAL_MS,
|
|
"snapshot_freshness_ms": round(self.freshness_s * 1000),
|
|
"capture_sequence": self._capture_sequence,
|
|
"observed_frame_ids": [
|
|
f"0x{frame_id:03X}"
|
|
for frame_id in sorted(self._fresh_ids(time.monotonic()))
|
|
],
|
|
"last_error": self._last_error,
|
|
}
|
|
|
|
def _fresh_ids(self, now: float) -> set[int]:
|
|
return {
|
|
frame_id
|
|
for frame_id, (captured_monotonic, _) in self._latest.items()
|
|
if now - captured_monotonic <= self.freshness_s
|
|
}
|
|
|
|
def _record(self, frame: CanFrame) -> None:
|
|
if (
|
|
frame.channel != self.settings.channel
|
|
or frame.arbitration_id not in OBSERVABLE_IDS
|
|
or len(frame.data) < 8
|
|
):
|
|
return
|
|
decoded = self.decoder(frame)
|
|
if not any(
|
|
key in decoded
|
|
for key in ("actuator_state", "domain_state", "vehicle_safety")
|
|
):
|
|
# 0x4EB is multiplexed; only subtype 0x07 contains verified speed.
|
|
return
|
|
frame_id = str(decoded.get("frame_id", ""))
|
|
decoded.update(
|
|
{
|
|
"valid": True,
|
|
"feedback_frame_id": frame_id,
|
|
"captured_at": _now(),
|
|
"verification": {
|
|
"authoritative": True,
|
|
"channel": 1,
|
|
"simulated": False,
|
|
"read_only": True,
|
|
"frame_id": frame_id,
|
|
"payload_size": len(frame.data),
|
|
},
|
|
}
|
|
)
|
|
with self._condition:
|
|
self._capture_sequence += 1
|
|
self._latest[frame.arbitration_id] = (time.monotonic(), decoded)
|
|
if REQUIRED_UI_IDS <= self._fresh_ids(time.monotonic()):
|
|
self._baseline_ready = True
|
|
self._condition.notify_all()
|
|
|
|
def _run(self) -> None:
|
|
while not self._stop_event.is_set():
|
|
bridge = None
|
|
try:
|
|
with self._condition:
|
|
self._state = "connecting"
|
|
self._condition.notify_all()
|
|
channel_config = replace(
|
|
self.settings.channel_config(),
|
|
receive_ids=tuple(sorted(OBSERVABLE_IDS)),
|
|
)
|
|
bridge = self.settings.bridge(
|
|
[channel_config],
|
|
monitor_only=True,
|
|
)
|
|
bus = bridge.channel_bus(self.settings.channel)
|
|
with self._condition:
|
|
self._bridge = bridge
|
|
self._latest.clear()
|
|
self._baseline_ready = False
|
|
self._last_error = None
|
|
self._state = "online"
|
|
self._condition.notify_all()
|
|
while not self._stop_event.is_set():
|
|
frame = bus.receive(100, include_fd=True)
|
|
if frame is not None:
|
|
self._record(frame)
|
|
except Exception as exc: # noqa: BLE001 - normalize vendor/runtime failures
|
|
if not self._stop_event.is_set():
|
|
with self._condition:
|
|
self._state = "error"
|
|
self._last_error = str(exc)
|
|
self._condition.notify_all()
|
|
finally:
|
|
with self._condition:
|
|
if self._bridge is bridge:
|
|
self._bridge = None
|
|
if bridge is not None:
|
|
bridge.close()
|
|
if not self._stop_event.wait(self.reconnect_delay_s):
|
|
continue
|
|
|
|
with self._condition:
|
|
self._state = "stopped"
|
|
self._condition.notify_all()
|