IQ.Pilot Release Commit @ b6534c0

iqpilot/selfdrive/constructiond.py

@@ -0,0 +1,153 @@
+#!/usr/bin/env python3
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+
+constructiond: work-zone detector for Speed Limit Assist.
+
+Samples the road camera at ~2 Hz and looks for work-zone orange (barrels, drums,
+diamond signs) in the NV12 chroma plane. Sunlit yellow lane paint renders with
+nearly identical hue to barrel orange on this camera, but its red chroma (V)
+saturates below ~168 while retroreflective barrel orange reaches 170-190, so the
+V floor is the load-bearing threshold — do not lower it without re-running the
+paint/barrel separation sweep on real footage.
+"""
+from collections import deque
+
+import time
+
+import numpy as np
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import custom
+from iqpilot.cereal.visionipc import VisionStreamType
+from msgq.visionipc import VisionIpcClient
+from iqpilot.common.swaglog import cloudlog
+
+State = custom.IQConstructionZone.State
+
+ANALYSIS_PERIOD = 0.5
+
+# fractions of the chroma plane; excludes sky and hood
+ROI_TOP, ROI_BOTTOM = 0.40, 0.94
+ROI_LEFT, ROI_RIGHT = 0.04, 0.96
+
+V_MIN = 170     # paint tops out ~168 in every DAYLIGHT condition sampled
+CB_MIN = 16
+HUE_LO, HUE_HI = 0.65, 2.0  # (V-128)/(128-U): yellow paint ~0.4, barrel orange ~0.7-1.5, red ~3.0
+
+# Daylight-only gate: retroreflective amber markers (guardrail chevrons, object
+# markers) blaze past V_MIN under headlights at night — 137px on one chevron on
+# real footage — and there is no night work-zone ground truth to tune against.
+# Night ROI mean luma measured ~36-38, validated daytime footage 84-98.
+LUMA_MIN = 65.0
+
+# hits only accumulate at highway-approach speeds: brightly lit lots (truck
+# stops) can pass the luma gate at night with orange signage, and the clamp is
+# meaningless below it anyway. An already-active zone still holds while slowed.
+MIN_ENTER_SPEED = 13.4  # m/s (~30 mph)
+
+HIT_FRAC = 3.0e-4
+WASH_FRAC = 0.10  # more orange than this is scene lighting (sunset), not objects
+ENTER_HITS = 3
+ENTER_WINDOW = 10  # analyses (~5 s)
+HOLD_SEC = 120.0   # barrel-free stretches inside a zone last minutes; hold through them
+
+
+def orange_fraction(buf) -> tuple[float, float]:
+  """Returns (hot-orange fraction, mean luma) of the road ROI."""
+  h, w, stride, uv_off = buf.height, buf.width, buf.stride, buf.uv_offset
+  ch, cw = h // 2, w // 2
+  uv = buf.data[uv_off:uv_off + ch * stride].reshape(ch, stride)
+
+  r0, r1 = int(ROI_TOP * ch), int(ROI_BOTTOM * ch)
+  c0, c1 = int(ROI_LEFT * cw), int(ROI_RIGHT * cw)
+  step = 2 if cw > 700 else 1
+
+  u = uv[r0:r1:step, 2 * c0:2 * c1:2 * step].astype(np.int16)
+  v = uv[r0:r1:step, 2 * c0 + 1:2 * c1:2 * step].astype(np.int16)
+
+  cb = 128 - u
+  cr = v - 128
+  mask = (v >= V_MIN) & (cb >= CB_MIN) & (cr >= HUE_LO * cb) & (cr <= HUE_HI * cb)
+
+  y_plane = buf.data[:h * stride].reshape(h, stride)
+  luma = float(y_plane[2 * r0:2 * r1:4, 2 * c0:2 * c1:8].mean())
+
+  return float(mask.mean()), luma
+
+
+class ConstructionZoneDetector:
+  def __init__(self):
+    self.recent = deque(maxlen=ENTER_WINDOW)
+    self.last_hit_t: float | None = None
+    self.active = False
+    self.frac = 0.0
+
+  @property
+  def state(self):
+    if self.active:
+      return State.active
+    if any(self.recent):
+      return State.pending
+    return State.inactive
+
+  def seconds_since_hit(self, now: float) -> float:
+    if self.last_hit_t is None:
+      return -1.0
+    return now - self.last_hit_t
+
+  def update(self, frac: float, now: float, luma: float = 255.0, v_ego: float = 255.0) -> bool:
+    self.frac = frac
+    hit = (HIT_FRAC <= frac <= WASH_FRAC) and luma >= LUMA_MIN and v_ego >= MIN_ENTER_SPEED
+    self.recent.append(hit)
+    if hit:
+      self.last_hit_t = now
+
+    if self.active:
+      if self.last_hit_t is None or (now - self.last_hit_t) > HOLD_SEC:
+        self.active = False
+        self.recent.clear()
+    elif sum(self.recent) >= ENTER_HITS:
+      self.active = True
+    return self.active
+
+
+def main():
+  pm = messaging.PubMaster(["iqConstructionZone"])
+  car_state_sock = messaging.sub_sock("carState", conflate=True)
+  detector = ConstructionZoneDetector()
+  v_ego = 0.0
+
+  vipc = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
+  while not vipc.connect(False):
+    time.sleep(0.5)
+  cloudlog.info("constructiond: connected to road camera stream")
+
+  while True:
+    t0 = time.monotonic()
+    buf = vipc.recv(200)
+    if buf is None:
+      # no publish on camera stall: SLC sees us stale and releases the clamp
+      continue
+
+    cs = messaging.recv_one_or_none(car_state_sock)
+    if cs is not None:
+      v_ego = cs.carState.vEgo
+
+    frac, luma = orange_fraction(buf)
+    detector.update(frac, t0, luma, v_ego)
+
+    msg = messaging.new_message("iqConstructionZone")
+    msg.valid = True
+    cz = msg.iqConstructionZone
+    cz.state = detector.state
+    cz.active = detector.active
+    cz.orangeFraction = frac
+    cz.secondsSinceHit = detector.seconds_since_hit(t0)
+    pm.send("iqConstructionZone", msg)
+
+    time.sleep(max(0.0, ANALYSIS_PERIOD - (time.monotonic() - t0)))
+
+
+if __name__ == "__main__":
+  main()