DCCONSTRUCTIONS
e515ab1b8c
Preserve the completed teach-and-repeat laboratory stage: reference preparation, cascaded acquisition, local tracking and recovery, recording lifecycle, replay qualification, and persistent Rerun scene controls. Document the open grid-picking regression and Rerun upgrade contract. No autonomous driving or loop-closure optimization is claimed.
77 lines
2.8 KiB
Python
77 lines
2.8 KiB
Python
"""Experimental temporal qualification; never grants vehicle authority."""
|
|
|
|
import numpy as np
|
|
|
|
from .registration import angle_deg, rigid, transform
|
|
|
|
TRACKING_POLICY = dict(
|
|
version="causal-consistency/v1",
|
|
consecutive=3,
|
|
maximum_position_change_m=0.5,
|
|
maximum_rotation_change_deg=5.0,
|
|
maximum_age_s=8.0,
|
|
)
|
|
|
|
|
|
class CausalTracking:
|
|
def __init__(self):
|
|
self.matrix = None
|
|
self.sample_ns = 0
|
|
self.segment = 0
|
|
self.streak = 0
|
|
self.state = "acquiring"
|
|
self.reason = "initial"
|
|
|
|
def clear(self, reason):
|
|
self.matrix = None
|
|
self.sample_ns = 0
|
|
self.streak = 0
|
|
self.state = "lost"
|
|
self.reason = reason
|
|
|
|
def tick(self, now_ns, segment):
|
|
if segment != self.segment:
|
|
self.clear("receipt-gap")
|
|
self.segment = segment
|
|
elif (
|
|
self.matrix is not None
|
|
and (now_ns - self.sample_ns) / 1e9 > TRACKING_POLICY["maximum_age_s"]
|
|
):
|
|
self.clear("stale")
|
|
|
|
def accept(self, result, sample, now_ns, segment):
|
|
self.tick(now_ns, segment)
|
|
age = (now_ns - sample["monotonic_ns"]) / 1e9
|
|
evidence = dict(age_s=age, position_change_m=None, rotation_change_deg=None)
|
|
if sample["segment"] != segment:
|
|
# An old job must never overwrite new-segment state.
|
|
return {**evidence, "accepted": False, "reason": "old-segment"}
|
|
if not 0 <= age <= TRACKING_POLICY["maximum_age_s"]:
|
|
self.clear("stale-result")
|
|
elif result["status"] != "candidate":
|
|
self.clear("registration-rejected")
|
|
else:
|
|
matrix = rigid(result["T_reference_query"])
|
|
if self.matrix is not None:
|
|
position = sample["path"][-1:]
|
|
delta = float(
|
|
np.linalg.norm(transform(position, matrix) - transform(position, self.matrix))
|
|
)
|
|
rotation = angle_deg(matrix[:3, :3] @ self.matrix[:3, :3].T)
|
|
evidence.update(position_change_m=delta, rotation_change_deg=rotation)
|
|
if (
|
|
delta > TRACKING_POLICY["maximum_position_change_m"]
|
|
or rotation > TRACKING_POLICY["maximum_rotation_change_deg"]
|
|
):
|
|
self.clear("inconsistent-candidate")
|
|
return {**evidence, "accepted": False, "reason": self.reason}
|
|
self.matrix = matrix
|
|
self.sample_ns = sample["monotonic_ns"]
|
|
self.streak += 1
|
|
self.state = (
|
|
"tracking" if self.streak >= TRACKING_POLICY["consecutive"] else "acquiring"
|
|
)
|
|
self.reason = "consistent-candidate"
|
|
return {**evidence, "accepted": True, "reason": self.reason}
|
|
return {**evidence, "accepted": False, "reason": self.reason}
|