Добавление канонического графа M4.7

src/k1link/perception/graph.py

@@ -29,19 +29,25 @@ from .contracts import (
 )
 from .graph_contracts import (
     GRAPH_RESULT_SCHEMA,
+    GRAPH_RESULT_SCHEMA_V2,
     REFERENCE_GRAPH_ID,
+    REFERENCE_GRAPH_ID_V2,
     TERMINAL_OUTCOME_SCHEMA,
     DeliveredFrame,
     GraphExecutionError,
+    GraphRunMode,
     GraphRunResult,
+    GraphRunResultV2,
     GraphState,
     TerminalOutcome,
     TerminalOutcomeType,
     build_graph_run_result,
+    build_graph_run_result_v2,
 )
 from .graph_validation import (
     validate_observations,
     validate_proposals,
+    validate_rolling,
     validate_temporal,
     validate_threats,
 )
@@ -51,6 +57,8 @@ from .providers import (
     MotionProvider,
     ProviderRole,
     ReferencePerceptionGraphConfig,
+    ReferencePerceptionGraphConfigV2,
+    RollingMapProvider,
     SourcePacket,
     SourceProvider,
     TemporalStateProvider,
@@ -79,6 +87,15 @@ class _Temporal:
     obstacles: tuple[TemporalObstacle, ...]
 
 
+@dataclass(frozen=True, slots=True)
+class _Rolled:
+    packet: SourcePacket
+    proposals: tuple[ObjectProposal2D, ...]
+    associated_proposal_ids: frozenset[str]
+    obstacles: tuple[TemporalObstacle, ...]
+    retained: tuple[TemporalObstacle, ...]
+
+
 @dataclass(frozen=True, slots=True)
 class _StopSignal:
     pass
@@ -88,8 +105,9 @@ _STOP = _StopSignal()
 _DetectorItem = SourcePacket | _StopSignal
 _GeometryItem = _Detected | _StopSignal
 _TemporalItem = _Associated | _StopSignal
-_ThreatItem = _Temporal | _StopSignal
-_QueueItem = SourcePacket | _Detected | _Associated | _Temporal | _StopSignal
+_RollingItem = _Temporal | _StopSignal
+_ThreatItem = _Temporal | _Rolled | _StopSignal
+_QueueItem = SourcePacket | _Detected | _Associated | _Temporal | _Rolled | _StopSignal
 _QueueItemT = TypeVar("_QueueItemT", bound=_QueueItem)
 
 
@@ -114,6 +132,37 @@ class ReferencePerceptionGraphV1:
             raise GraphExecutionError("reference graph id is incompatible")
         if config.source_profile_id != BASELINE_PROFILE_ID:
             raise GraphExecutionError("reference source profile is incompatible")
+        self._initialize(
+            config=config,
+            source=source,
+            detector=detector,
+            geometry=geometry,
+            temporal=temporal,
+            motion=motion,
+            rolling=None,
+            threat=threat,
+            run_mode=GraphRunMode.SOURCE_PACED_LATEST_WINS,
+            telemetry_identity=telemetry_identity,
+            telemetry_sink=telemetry_sink,
+            clock_ns=clock_ns,
+        )
+
+    def _initialize(
+        self,
+        *,
+        config: ReferencePerceptionGraphConfig | ReferencePerceptionGraphConfigV2,
+        source: SourceProvider,
+        detector: DetectorProvider,
+        geometry: GeometryAssociationProvider,
+        temporal: TemporalStateProvider,
+        motion: MotionProvider,
+        rolling: RollingMapProvider | None,
+        threat: ThreatProvider,
+        run_mode: GraphRunMode,
+        telemetry_identity: PipelineTelemetryIdentity | None,
+        telemetry_sink: PipelineTelemetrySink | None,
+        clock_ns: Callable[[], int],
+    ) -> None:
         if (telemetry_identity is None) is not (telemetry_sink is None):
             raise GraphExecutionError("telemetry identity and sink must be configured together")
         self.config = config
@@ -122,7 +171,9 @@ class ReferencePerceptionGraphV1:
         self.geometry = geometry
         self.temporal = temporal
         self.motion = motion
+        self.rolling = rolling
         self.threat = threat
+        self.run_mode = run_mode
         self.telemetry_identity = telemetry_identity
         self.telemetry_sink = telemetry_sink
         self._clock_ns = clock_ns
@@ -135,6 +186,9 @@ class ReferencePerceptionGraphV1:
         self._deliveries: list[DeliveredFrame] = []
         self._admitted: dict[int, SourcePacket] = {}
         self._admitted_at_ns: dict[int, int] = {}
+        self._queue_high_watermarks: dict[str, int] = {
+            stage_id: 0 for stage_id in ("detector", "geometry", "temporal", "rolling", "threat")
+        }
         self._validate_provider_pins()
 
     @property
@@ -149,14 +203,17 @@ class ReferencePerceptionGraphV1:
     def cancel(self) -> None:
         self._cancel_event.set()
 
-    def run(self) -> GraphRunResult:
+    def run(self) -> GraphRunResult | GraphRunResultV2:
         self._begin_run()
         started_ns = self._now()
         detector_queue: Queue[_DetectorItem] = Queue(self._capacity("detector"))
         geometry_queue: Queue[_GeometryItem] = Queue(self._capacity("geometry"))
         temporal_queue: Queue[_TemporalItem] = Queue(self._capacity("temporal"))
         threat_queue: Queue[_ThreatItem] = Queue(self._capacity("threat"))
-        self._threads = [
+        rolling_queue: Queue[_RollingItem] | None = (
+            Queue(self._capacity("rolling")) if self.rolling is not None else None
+        )
+        threads = [
             Thread(
                 target=self._detector_loop,
                 args=(detector_queue, geometry_queue),
@@ -171,17 +228,29 @@ class ReferencePerceptionGraphV1:
             ),
             Thread(
                 target=self._temporal_loop,
-                args=(temporal_queue, threat_queue),
+                args=(temporal_queue, rolling_queue or threat_queue),
                 name="m4-temporal",
                 daemon=True,
             ),
+        ]
+        if rolling_queue is not None:
+            threads.append(
+                Thread(
+                    target=self._rolling_loop,
+                    args=(rolling_queue, threat_queue),
+                    name="m4-rolling",
+                    daemon=True,
+                )
+            )
+        threads.append(
             Thread(
                 target=self._threat_loop,
                 args=(threat_queue,),
                 name="m4-threat",
                 daemon=True,
-            ),
-        ]
+            )
+        )
+        self._threads = threads
         for thread in self._threads:
             thread.start()
         run_emitter = self._run_emitter()
@@ -244,6 +313,8 @@ class ReferencePerceptionGraphV1:
             self._deliveries.clear()
             self._admitted.clear()
             self._admitted_at_ns.clear()
+            for stage_id in self._queue_high_watermarks:
+                self._queue_high_watermarks[stage_id] = 0
 
     def _admit(self, packet: SourcePacket) -> bool:
         envelope = packet.envelope
@@ -283,6 +354,8 @@ class ReferencePerceptionGraphV1:
                     validate_proposals(item, proposals)
                     if stage is not None:
                         stage.output_count = len(proposals)
+                if self._expired(item, "detector"):
+                    continue
                 self._put_latest(outgoing, _Detected(item, proposals), "geometry")
             except Exception as exc:
                 if not isinstance(item, _StopSignal):
@@ -311,6 +384,8 @@ class ReferencePerceptionGraphV1:
                     validate_exclusive_point_ownership(observations)
                     if stage is not None:
                         stage.output_count = len(observations)
+                if self._expired(item.packet, "geometry"):
+                    continue
                 self._put_latest(
                     outgoing,
                     _Associated(item.packet, item.proposals, observations),
@@ -325,13 +400,16 @@ class ReferencePerceptionGraphV1:
     def _temporal_loop(
         self,
         incoming: Queue[_TemporalItem],
-        outgoing: Queue[_ThreatItem],
+        outgoing: Queue[_RollingItem] | Queue[_ThreatItem],
     ) -> None:
         while True:
             item = incoming.get()
             try:
                 if isinstance(item, _StopSignal):
-                    self._put_stop(outgoing, "threat")
+                    self._put_stop(
+                        cast(Queue[_QueueItem], outgoing),
+                        "rolling" if self.rolling is not None else "threat",
+                    )
                     return
                 if self._cancelled(item.packet):
                     continue
@@ -343,8 +421,10 @@ class ReferencePerceptionGraphV1:
                     validate_temporal(item.packet, obstacles)
                     if stage is not None:
                         stage.output_count = len(obstacles)
+                if self._expired(item.packet, "temporal"):
+                    continue
                 self._put_latest(
-                    outgoing,
+                    cast(Queue[_QueueItem], outgoing),
                     _Temporal(
                         item.packet,
                         item.proposals,
@@ -356,7 +436,7 @@ class ReferencePerceptionGraphV1:
                         ),
                         obstacles,
                     ),
-                    "threat",
+                    "rolling" if self.rolling is not None else "threat",
                 )
             except Exception as exc:
                 if not isinstance(item, _StopSignal):
@@ -364,6 +444,48 @@ class ReferencePerceptionGraphV1:
             finally:
                 incoming.task_done()
 
+    def _rolling_loop(
+        self,
+        incoming: Queue[_RollingItem],
+        outgoing: Queue[_ThreatItem],
+    ) -> None:
+        rolling = self.rolling
+        if rolling is None:
+            raise GraphExecutionError("rolling loop requires a rolling provider")
+        while True:
+            item = incoming.get()
+            try:
+                if isinstance(item, _StopSignal):
+                    self._put_stop(outgoing, "threat")
+                    return
+                if self._cancelled(item.packet):
+                    continue
+                if self._expired(item.packet, "rolling"):
+                    continue
+                with self._stage(item.packet, "rolling", len(item.obstacles)) as stage:
+                    retained = rolling.update(item.packet, item.obstacles)
+                    validate_rolling(item.obstacles, retained)
+                    if stage is not None:
+                        stage.output_count = len(retained)
+                if self._expired(item.packet, "rolling"):
+                    continue
+                self._put_latest(
+                    outgoing,
+                    _Rolled(
+                        item.packet,
+                        item.proposals,
+                        item.associated_proposal_ids,
+                        item.obstacles,
+                        retained,
+                    ),
+                    "threat",
+                )
+            except Exception as exc:
+                if not isinstance(item, _StopSignal):
+                    self._failed(item.packet, "rolling", exc)
+            finally:
+                incoming.task_done()
+
     def _threat_loop(self, incoming: Queue[_ThreatItem]) -> None:
         while True:
             item = incoming.get()
@@ -380,6 +502,8 @@ class ReferencePerceptionGraphV1:
                     validate_threats(obstacle_map, threats)
                     if stage is not None:
                         stage.output_count = len(threats)
+                if self._expired(item.packet, "threat"):
+                    continue
                 delivery = DeliveredFrame(
                     sequence=item.packet.envelope.sequence,
                     obstacle_map=obstacle_map,
@@ -399,14 +523,15 @@ class ReferencePerceptionGraphV1:
             finally:
                 incoming.task_done()
 
-    def _obstacle_map(self, item: _Temporal) -> LocalObstacleMap:
+    def _obstacle_map(self, item: _Temporal | _Rolled) -> LocalObstacleMap:
         packet = item.packet
         now_ns = self._now()
         with self._result_lock:
             admitted_at_ns = self._admitted_at_ns[packet.envelope.sequence]
-        occupied = tuple(
+        current = tuple(
             obstacle for obstacle in item.obstacles if obstacle.state is TemporalState.CURRENT
         )
+        occupied = (*current, *item.retained) if isinstance(item, _Rolled) else current
         unknown = tuple(
             obstacle for obstacle in item.obstacles if obstacle.state is not TemporalState.CURRENT
         )
@@ -484,9 +609,13 @@ class ReferencePerceptionGraphV1:
         item: _QueueItemT,
         stage_id: str,
     ) -> None:
+        if self.run_mode is GraphRunMode.LOSSLESS_REPLAY:
+            self._put_lossless(queue, item, stage_id)
+            return
         while True:
             try:
                 queue.put_nowait(item)
+                self._record_queue_depth(stage_id, queue.qsize())
                 return
             except Full:
                 pass
@@ -506,11 +635,60 @@ class ReferencePerceptionGraphV1:
             finally:
                 queue.task_done()
 
+    def _put_lossless(
+        self,
+        queue: Queue[_QueueItemT],
+        item: _QueueItemT,
+        stage_id: str,
+    ) -> None:
+        while True:
+            try:
+                queue.put(item, timeout=0.05)
+                self._record_queue_depth(stage_id, queue.qsize())
+                return
+            except Full:
+                if self._cancel_event.is_set() and not isinstance(item, _StopSignal):
+                    packet = _packet_from_item(item)
+                    self._terminal(
+                        packet,
+                        TerminalOutcomeType.FAILED,
+                        stage_id,
+                        "graph-cancelled",
+                    )
+                    return
+
+    def _record_queue_depth(self, stage_id: str, depth: int) -> None:
+        with self._result_lock:
+            self._queue_high_watermarks[stage_id] = max(
+                self._queue_high_watermarks.get(stage_id, 0),
+                depth,
+            )
+
     def _put_stop(self, queue: Queue[_QueueItemT], stage_id: str) -> None:
         timeout = self._terminal_timeout(stage_id) / 1_000_000_000
         try:
             queue.put(cast(_QueueItemT, _STOP), timeout=timeout)
+            self._record_queue_depth(stage_id, queue.qsize())
         except Full:
+            if self.run_mode is GraphRunMode.LOSSLESS_REPLAY:
+                self._cancel_event.set()
+                try:
+                    stranded = queue.get_nowait()
+                except Empty as exc:
+                    raise GraphExecutionError("lossless terminal queue timed out") from exc
+                try:
+                    if not isinstance(stranded, _StopSignal):
+                        self._terminal(
+                            _packet_from_item(stranded),
+                            TerminalOutcomeType.FAILED,
+                            stage_id,
+                            "terminal-queue-timeout",
+                        )
+                finally:
+                    queue.task_done()
+                queue.put_nowait(cast(_QueueItemT, _STOP))
+                self._record_queue_depth(stage_id, queue.qsize())
+                return
             self._put_latest(queue, cast(_QueueItemT, _STOP), stage_id)
 
     def _join_workers(self) -> None:
@@ -541,11 +719,27 @@ class ReferencePerceptionGraphV1:
             if set(self._admitted) != set(self._outcomes):
                 raise GraphExecutionError("terminal accounting did not close")
 
-    def _result(self, state: GraphState) -> GraphRunResult:
+    def _result(self, state: GraphState) -> GraphRunResult | GraphRunResultV2:
         with self._result_lock:
             outcomes = tuple(self._outcomes[key] for key in sorted(self._outcomes))
             deliveries = tuple(sorted(self._deliveries, key=lambda item: item.sequence))
             admitted_count = len(self._admitted)
+            queue_high_watermarks = tuple(
+                (stage_id, self._queue_high_watermarks[stage_id])
+                for stage_id in sorted(self._queue_high_watermarks)
+                if any(policy.stage_id == stage_id for policy in self.config.queues)
+            )
+        if self.rolling is not None:
+            return build_graph_run_result_v2(
+                graph_id=self.config.graph_id,
+                source_profile_id=self.config.source_profile_id,
+                run_mode=self.run_mode,
+                state=state,
+                admitted_count=admitted_count,
+                outcomes=outcomes,
+                deliveries=deliveries,
+                queue_high_watermarks=queue_high_watermarks,
+            )
         return build_graph_run_result(
             graph_id=self.config.graph_id,
             source_profile_id=self.config.source_profile_id,
@@ -598,6 +792,8 @@ class ReferencePerceptionGraphV1:
             ProviderRole.MOTION: self.motion.provider_id,
             ProviderRole.THREAT: self.threat.provider_id,
         }
+        if self.rolling is not None:
+            actual[ProviderRole.ROLLING] = self.rolling.provider_id
         pinned = {pin.role: pin.provider_id for pin in self.config.providers}
         if actual != pinned:
             raise GraphExecutionError("configured provider identities do not match runtime")
@@ -627,6 +823,56 @@ class ReferencePerceptionGraphV1:
             self._state = state
 
 
+class ReferencePerceptionGraphV2(ReferencePerceptionGraphV1):
+    """Final M4 graph with explicit bounded rolling occupancy ownership."""
+
+    def __init__(
+        self,
+        *,
+        config: ReferencePerceptionGraphConfigV2,
+        source: SourceProvider,
+        detector: DetectorProvider,
+        geometry: GeometryAssociationProvider,
+        temporal: TemporalStateProvider,
+        motion: MotionProvider,
+        rolling: RollingMapProvider,
+        threat: ThreatProvider,
+        run_mode: GraphRunMode,
+        telemetry_identity: PipelineTelemetryIdentity | None = None,
+        telemetry_sink: PipelineTelemetrySink | None = None,
+        clock_ns: Callable[[], int] = time.monotonic_ns,
+    ) -> None:
+        if config.graph_id != REFERENCE_GRAPH_ID_V2:
+            raise GraphExecutionError("reference graph v2 id is incompatible")
+        if config.source_profile_id != BASELINE_PROFILE_ID:
+            raise GraphExecutionError("reference graph v2 source profile is incompatible")
+        self._has_run = False
+        self._run_once_lock = Lock()
+        self._initialize(
+            config=config,
+            source=source,
+            detector=detector,
+            geometry=geometry,
+            temporal=temporal,
+            motion=motion,
+            rolling=rolling,
+            threat=threat,
+            run_mode=run_mode,
+            telemetry_identity=telemetry_identity,
+            telemetry_sink=telemetry_sink,
+            clock_ns=clock_ns,
+        )
+
+    def _begin_run(self) -> None:
+        with self._run_once_lock:
+            if self._has_run:
+                raise GraphExecutionError(
+                    "reference graph v2 restart requires freshly instantiated providers"
+                )
+            self._has_run = True
+        super()._begin_run()
+
+
 class _NullStage:
     output_count: int | None = None
 
@@ -640,20 +886,25 @@ class _NullStage:
 def _packet_from_item(item: object) -> SourcePacket:
     if isinstance(item, SourcePacket):
         return item
-    if isinstance(item, (_Detected, _Associated, _Temporal)):
+    if isinstance(item, (_Detected, _Associated, _Temporal, _Rolled)):
         return item.packet
     raise GraphExecutionError("queue contained an incompatible item")
 
 
 __all__ = [
     "GRAPH_RESULT_SCHEMA",
+    "GRAPH_RESULT_SCHEMA_V2",
     "REFERENCE_GRAPH_ID",
+    "REFERENCE_GRAPH_ID_V2",
     "TERMINAL_OUTCOME_SCHEMA",
     "DeliveredFrame",
     "GraphExecutionError",
     "GraphRunResult",
+    "GraphRunResultV2",
+    "GraphRunMode",
     "GraphState",
     "ReferencePerceptionGraphV1",
+    "ReferencePerceptionGraphV2",
     "TerminalOutcome",
     "TerminalOutcomeType",
 ]