"""Next Drive: top-down urban intersection simulator, expert planner, rule-based driver, learned driver and safety check. Simulation and training demo only. No physical vehicle, no claim about real-road safety. The ego car follows a fixed route (straight, left or right turn) and only chooses its longitudinal action every 0.1 s from ACTIONS. Everything is deterministic given (seed, kind, density) and is ported line by line to web/core.js so the Space can generate new drives. """ import math import numpy as np from nlrt import Mulberry32, forward DT, VMAX, V_LIMIT, T_MAX = 0.1, 14.0, 13.9, 45.0 ACTIONS = [-6.0, -3.0, -1.5, 0.0, 1.0, 2.0] ACTION_NAMES = ["brake hard", "brake", "ease off", "hold speed", "speed up", "accelerate"] LANE, S_STOP, HALF = 1.75, 60.0, 2.2 ROUTES = ["straight", "left", "right"] KINDS = ["light", "crosswalk", "lead_brake", "occlusion", "mixed"] TURN = {"right": (-5.75, 4.0), "left": (-5.25, 7.0)} N_FEAT = 21 # ------------------------------------------------------------------ geometry def path_len(route): if route == "straight": return 130.0 y0, r = TURN[route] return (y0 + 70.0) + r * math.pi / 2 + 55.0 def pose(route, s): """Position and heading on the route at arc length s (clamped to the route).""" s = max(0.0, min(s, path_len(route))) if route == "straight": return LANE, -70.0 + s, math.pi / 2 y0, r = TURN[route] L1 = y0 + 70.0 if s < L1: return LANE, -70.0 + s, math.pi / 2 arc = r * math.pi / 2 if s < L1 + arc: th = (s - L1) / r if route == "right": return LANE + r - r * math.cos(th), y0 + r * math.sin(th), math.pi / 2 - th return LANE - r + r * math.cos(th), y0 + r * math.sin(th), math.pi / 2 + th d = s - L1 - arc if route == "right": return LANE + r + d, y0 + r, 0.0 return LANE - r - d, y0 + r, math.pi def circles(x, y, h): c, s = math.cos(h), math.sin(h) return [(x + d * c, y + d * s) for d in (-1.4, 0.0, 1.4)] def hits_car(ego, other, margin=0.0): for ax, ay in ego: for bx, by in other: dx, dy, rr = ax - bx, ay - by, 2.0 + margin if dx * dx + dy * dy < rr * rr: return True return False def hits_ped(ego, px, py, margin=0.0): for ax, ay in ego: dx, dy, rr = ax - px, ay - py, 1.35 + margin if dx * dx + dy * dy < rr * rr: return True return False # ------------------------------------------------------------------ traffic lights def light_ns(sc, t): G, Y, R = sc["G"], 3.0, sc["R"] u = (t + sc["phase"]) % (G + Y + R) if u < G: return 0, G - u if u < G + Y: return 1, G + Y - u return 2, G + Y + R - u def light_ew(sc, t): G, Y, R = sc["G"], 3.0, sc["R"] u = (t + sc["phase"]) % (G + Y + R) if u < G + Y: return 2 r = u - G - Y if r < 1.0: return 2 if r < R - 4.0: return 0 if r < R - 1.0: return 1 return 2 # ------------------------------------------------------------------ scenarios def make_scenario(seed, kind="mixed", density=1): rng = Mulberry32((seed * 7919 + KINDS.index(kind) * 101 + density * 13) % 4294967296) u = rng.random() route = "straight" if u < 0.5 else ("left" if u < 0.75 else "right") if kind in ("occlusion", "crosswalk") and rng.random() < 0.5: route = "straight" G, R = 7.0 + 6.0 * rng.random(), 8.0 + 5.0 * rng.random() sc = {"seed": seed, "kind": kind, "density": density, "route": route, "G": G, "R": R, "phase": rng.random() * (G + 3.0 + R), "cars": [], "peds": [], "parked": [], "lead": None} for k in range(density + 1): # cross traffic, both directions for d in (1, -1): sc["cars"].append({"axis": "x", "dir": d, "lane": -LANE * d, "p0": -70.0 * d, "v0": 8.0 + 4.0 * rng.random(), "t0": k * 7.0 + 5.0 * rng.random()}) if route == "left" or kind == "mixed": # oncoming traffic (southbound) for k in range(density): sc["cars"].append({"axis": "y", "dir": -1, "lane": -LANE, "p0": 70.0, "v0": 8.0 + 4.0 * rng.random(), "t0": k * 6.0 + 4.0 * rng.random()}) if kind in ("crosswalk", "mixed"): for k in range(density + 1): d = 1 if rng.random() < 0.5 else -1 sc["peds"].append({"x0": -9.0 * d, "y0": -7.5 + 1.6 * (rng.random() - 0.5), "vx": d * (1.0 + 0.6 * rng.random()), "vy": 0.0, "t0": 1.0 + 11.0 * rng.random(), "x_end": 9.0 * d}) if kind in ("lead_brake", "mixed"): sc["lead"] = {"s0": 14.0 + 10.0 * rng.random(), "v0": 9.0 + 3.0 * rng.random(), "tb": 2.5 + 5.0 * rng.random(), "db": 1.5 + 1.5 * rng.random()} if kind in ("occlusion", "mixed"): y = -46.0 gaps = [] while y < -16.0: L = 4.6 sc["parked"].append([4.6, y + L / 2, 2.0, L]) y += L + 0.4 + (2.6 if rng.random() < 0.45 else 0.0) gaps.append(y - 0.2) n_emerge = density for k in range(min(n_emerge, len(gaps))): gy = gaps[rng.randint(len(gaps))] - 1.0 D = 12.0 + 10.0 * rng.random() sc["peds"].append({"x0": 5.8, "y0": gy, "vx": -(1.2 + 0.4 * rng.random()), "vy": 0.0, "t0": max(0.0, (gy + 70.0 - HALF - D) / 10.0), "x_end": -6.0}) return sc # ------------------------------------------------------------------ world state def init_world(sc): w = {"t": 0.0, "s": 0.0, "v": 10.0, "a": 0.0, "cars": [], "peds": [], "lead": None} for c in sc["cars"]: w["cars"].append({"p": c["p0"], "v": c["v0"], "active": False, "done": False, **{k: c[k] for k in ("axis", "dir", "lane", "v0", "t0")}}) for p in sc["peds"]: w["peds"].append({"x": p["x0"], "y": p["y0"], "vx": 0.0, "vy": 0.0, "done": False, **{k: p[k] for k in ("t0", "x_end")}, "svx": p["vx"]}) if sc["lead"]: L = sc["lead"] w["lead"] = {"s": L["s0"], "v": L["v0"], **L} return w def car_xyh(c): if c["axis"] == "x": return c["p"], c["lane"], 0.0 if c["dir"] > 0 else math.pi return c["lane"], c["p"], math.pi / 2 if c["dir"] > 0 else -math.pi / 2 def step_agents(sc, w): t = w["t"] ew, (ns, _) = light_ew(sc, t), light_ns(sc, t) lanes = {} for c in w["cars"]: if not c["active"] and not c["done"] and t >= c["t0"]: c["active"] = True if c["active"]: lanes.setdefault((c["axis"], c["dir"]), []).append(c) for key, cs in lanes.items(): cs.sort(key=lambda c: -c["p"] * c["dir"]) # front-most first for i, c in enumerate(cs): light = ew if c["axis"] == "x" else ns front = c["p"] * c["dir"] + HALF d_stop = -10.0 - front a = min(2.0, 1.5 * (c["v0"] - c["v"])) if light != 0 and d_stop > -1.0 and c["v"] * c["v"] / (2 * 3.0) < d_stop + 1.0: a = min(a, -c["v"] * c["v"] / (2 * max(d_stop - 0.5, 0.3))) if i > 0: gap = (cs[i - 1]["p"] - c["p"]) * c["dir"] - 4.4 if gap < 6.0 + c["v"] * 1.2: a = min(a, -3.0 if gap > 3.0 else -6.0) c["v"] = max(0.0, c["v"] + max(-6.0, a) * DT) c["p"] += c["dir"] * c["v"] * DT if c["p"] * c["dir"] > 75.0: c["active"], c["done"] = False, True for p in w["peds"]: if p["done"]: continue if t >= p["t0"]: p["vx"] = p["svx"] p["x"] += p["vx"] * DT if (p["x"] - p["x_end"]) * p["svx"] > 0: p["done"] = True L = w["lead"] if L is not None: if L["tb"] <= t < L["tb"] + L["db"]: a = -6.0 else: a = min(2.0, 1.5 * (L["v0"] - L["v"])) d_stop = S_STOP - (L["s"] + HALF) if ns != 0 and d_stop > -1.0 and L["v"] * L["v"] / (2 * 3.0) < d_stop + 1.0: a = min(a, -L["v"] * L["v"] / (2 * max(d_stop - 0.5, 0.3))) L["v"] = max(0.0, L["v"] + max(-6.0, a) * DT) L["s"] += L["v"] * DT # ------------------------------------------------------------------ perception def seg_hits_rect(x0, y0, x1, y1, rect): cx, cy, wd, ht = rect xmin, xmax, ymin, ymax = cx - wd / 2, cx + wd / 2, cy - ht / 2, cy + ht / 2 t0, t1 = 0.0, 1.0 for p, q in ((-(x1 - x0), x0 - xmin), (x1 - x0, xmax - x0), (-(y1 - y0), y0 - ymin), (y1 - y0, ymax - y0)): if p == 0: if q < 0: return False else: r = q / p if p < 0: if r > t1: return False if r > t0: t0 = r else: if r < t0: return False if r < t1: t1 = r return True def observe(sc, w): ex, ey, eh = pose(sc["route"], w["s"]) eye = (ex + 1.0 * math.cos(eh), ey + 1.0 * math.sin(eh)) agents = [] for c in w["cars"]: if c["active"]: x, y, h = car_xyh(c) agents.append({"kind": "car", "x": x, "y": y, "h": h, "vx": c["v"] * math.cos(h), "vy": c["v"] * math.sin(h)}) L = w["lead"] if L is not None and L["s"] < path_len(sc["route"]): x, y, h = pose(sc["route"], L["s"]) agents.append({"kind": "lead", "x": x, "y": y, "h": h, "vx": L["v"] * math.cos(h), "vy": L["v"] * math.sin(h), "s": L["s"], "v": L["v"]}) visible = [] for p in w["peds"]: if p["done"]: continue dx, dy = p["x"] - eye[0], p["y"] - eye[1] vis = dx * dx + dy * dy < 3600.0 and not any(seg_hits_rect(eye[0], eye[1], p["x"], p["y"], r) for r in sc["parked"]) visible.append(vis) if vis: agents.append({"kind": "ped", "x": p["x"], "y": p["y"], "h": 0.0, "vx": p["vx"], "vy": p["vy"]}) phantoms = [] s_front = w["s"] + HALF for r in sc["parked"]: gy = r[1] + r[3] / 2 + 0.3 ahead = (gy + 70.0) - s_front if 0.0 < ahead < 35.0 and seg_hits_rect(eye[0], eye[1], 4.2, gy, r): phantoms.append({"x": 4.2, "y": gy, "vx": -1.4, "vy": 0.0, "ahead": ahead}) ls, rem = light_ns(sc, w["t"]) return {"t": w["t"], "s": w["s"], "v": w["v"], "a": w["a"], "route": sc["route"], "agents": agents, "phantoms": phantoms, "light": ls, "remaining": rem, "sc": sc, "visible": visible} # ------------------------------------------------------------------ prediction helpers def rollout_hits(obs, plan, horizon=3.0, dt=0.25, margin_car=0.4, margin_ped=0.6): """plan(k, tau) -> accel. Returns (first collision time or None, red-light crossing time or None, s_end, soft penalty). Soft penalty: speed above the limit, speed above what allows stopping before an occluded gap (where a hidden pedestrian could step out), and following closer than 2 m + 1.2 s to the lead car.""" s, v = obs["s"], obs["v"] route = obs["route"] n = int(round(horizon / dt)) excess = 0.0 red_t = None lead = [ag for ag in obs["agents"] if ag["kind"] == "lead"] for k in range(1, n + 1): tau = k * dt a = plan(k, tau) v = min(VMAX, max(0.0, v + a * dt)) s_prev = s s = s + v * dt if v > V_LIMIT: excess += v - V_LIMIT for ph in obs["phantoms"]: d = ph["y"] + 70.0 - (s + HALF) if d > -1.0: excess += max(0.0, v - (math.sqrt(2 * 4.0 * max(d - 2.0, 0.0)) + 3.0)) for ag in lead: gap = ag["s"] + ag["v"] * tau - s - 4.4 excess += 0.5 * max(0.0, 2.0 + 1.2 * v - gap) if red_t is None and s > s_prev: lt, _ = light_ns(obs["sc"], obs["t"] + tau) crossing = s_prev + HALF < S_STOP - 0.5 <= s + HALF creeping = S_STOP - 0.5 <= obs["s"] + HALF < S_STOP + 6.0 and obs["v"] < 3.0 if lt == 2 and (crossing or creeping): red_t = tau ex, ey, eh = pose(route, s) ec = circles(ex, ey, eh) for ag in obs["agents"]: px, py = ag["x"] + ag["vx"] * tau, ag["y"] + ag["vy"] * tau if (px - ex) * (px - ex) + (py - ey) * (py - ey) > 64.0: continue if ag["kind"] == "ped": if hits_ped(ec, px, py, margin_ped): return tau, red_t, s, excess elif hits_car(ec, circles(px, py, ag["h"]), margin_car): return tau, red_t, s, excess return None, red_t, s, excess # ------------------------------------------------------------------ drivers def expert(obs): """Search over two-phase action sequences (1 s then 2 s), constant-velocity predictions, phantom pedestrians.""" best, best_cost = 0, float("inf") for i, a1 in enumerate(ACTIONS): for a2 in ACTIONS: plan = lambda k, tau, a1=a1, a2=a2: a1 if tau <= 1.0 + 1e-9 else a2 hit, red, s_end, excess = rollout_hits(obs, plan) cost = 0.0 if hit is not None: cost += 1e5 - 1e4 * hit if red is not None: cost += 2e4 cost += -10.0 * (s_end - obs["s"]) cost += 40.0 if a1 == -6.0 else (8.0 if a1 == -3.0 else 0.0) cost += 3.0 * abs(a1 - obs["a"]) cost += 50.0 * excess if cost < best_cost: best, best_cost = i, cost return best def rule_driver(obs): """IDM-style rules: lights, lead car, visible pedestrians near the route, yield before a left turn.""" v, s = obs["v"], obs["s"] route = obs["route"] s_front = s + HALF obst = [] for ag in obs["agents"]: if ag["kind"] == "lead": obst.append((ag["s"] - s - 4.4, ag["v"])) d_stop = S_STOP - s_front if d_stop > 0 and (obs["light"] == 2 or (obs["light"] == 1 and v * v / (2 * 3.0) < d_stop)): obst.append((d_stop - 0.5, 0.0)) for ag in obs["agents"]: if ag["kind"] != "ped": continue for j in range(0, 51, 2): px, py, _ = pose(route, s + j) fx, fy = ag["x"] + ag["vx"] * 1.5, ag["y"] + ag["vy"] * 1.5 d1 = (ag["x"] - px) * (ag["x"] - px) + (ag["y"] - py) * (ag["y"] - py) d2 = (fx - px) * (fx - px) + (fy - py) * (fy - py) if min(d1, d2) < 6.76: obst.append((j - HALF - 1.5, 0.0)) break if route == "left": y0, _ = TURN["left"] d_turn = (y0 + 70.0) - s_front if d_turn > -1.0: for ag in obs["agents"]: if ag["kind"] == "car" and ag["vy"] < -0.5 and -4.0 < ag["y"] < 45.0 and ag["x"] < 0: obst.append((d_turn - 0.5, 0.0)) break q = v / 12.0 q4 = (q * q) * (q * q) a = 1.5 * (1.0 - q4) for gap, vo in obst: gap = max(gap, 0.1) sstar = 2.0 + v * 1.2 + v * (v - vo) / (2 * math.sqrt(1.5 * 3.0)) a = min(a, 1.5 * (1.0 - q4 - (sstar / gap) * (sstar / gap))) a = max(-6.0, min(2.0, a)) return min(range(len(ACTIONS)), key=lambda i: (abs(ACTIONS[i] - a), i)) def features(obs): v, s = obs["v"], obs["s"] d_stop = S_STOP - (s + HALF) f = [v / 14.0, obs["a"] / 6.0, min(max(d_stop, 0.0), 60.0) / 60.0, 1.0 if d_stop < 0 else 0.0, 1.0 if obs["light"] == 0 else 0.0, 1.0 if obs["light"] == 1 else 0.0, 1.0 if obs["light"] == 2 else 0.0, min(obs["remaining"], 15.0) / 15.0] lead = [ag for ag in obs["agents"] if ag["kind"] == "lead"] if lead: f += [min(max(lead[0]["s"] - s - 4.4, 0.0), 50.0) / 50.0, max(-1.0, min(1.0, (lead[0]["v"] - v) / 14.0))] else: f += [1.0, 0.0] for acc in (0.0, -3.0, 1.0): hit, _, _, _ = rollout_hits(obs, lambda k, tau, acc=acc: acc, horizon=5.0) f.append((hit if hit is not None else 5.0) / 5.0) ex, ey, _ = pose(obs["route"], s) peds = [math.sqrt((ag["x"] - ex) * (ag["x"] - ex) + (ag["y"] - ey) * (ag["y"] - ey)) for ag in obs["agents"] if ag["kind"] == "ped"] f.append(min(min(peds), 40.0) / 40.0 if peds else 1.0) f.append(min(min(p["ahead"] for p in obs["phantoms"]), 40.0) / 40.0 if obs["phantoms"] else 1.0) busy = any(ag["kind"] == "car" and abs(ag["x"]) < 12.0 and abs(ag["y"]) < 12.0 for ag in obs["agents"]) f.append(1.0 if busy else 0.0) f += [1.0 if obs["route"] == r else 0.0 for r in ROUTES] f.append(min(v * v / 12.0, 60.0) / 60.0) f.append(1.0 if obs["light"] != 0 and d_stop > 0 and v * v / 6.0 < d_stop + 1.0 else 0.0) return f def learned(obs, layers): z = forward(layers, np.asarray([features(obs)]))[0] return int(np.argmax(z)) def shield(obs, i): """Safety check: keep action i only if braking hard after 0.5 s still avoids predicted collisions / red-light entry.""" def unsafe(a): hit, red, _, _ = rollout_hits(obs, lambda k, tau: a if tau <= 0.5 + 1e-9 else -6.0, margin_car=0.2, margin_ped=0.4) if hit is not None: return True if red is not None: _, red_b, _, _ = rollout_hits(obs, lambda k, tau: -6.0, margin_car=0.2, margin_ped=0.4) return red_b is None return False if not unsafe(ACTIONS[i]): return i, False for j in range(i - 1, -1, -1): if not unsafe(ACTIONS[j]): return j, True return 0, True # ------------------------------------------------------------------ episode def run_episode(sc, driver, layers=None, record=True, timer=None, on_obs=None): w = init_world(sc) route = sc["route"] goal = path_len(route) - 5.0 out = {"collision": False, "collision_with": None, "red_violations": 0, "reached": False, "hard_brakes": 0, "overrides": 0, "steps": 0, "time": 0.0, "decision_ms": [], "trace": [], "events": [], "actions": []} prev_a = 0.0 for step in range(int(round(T_MAX / DT))): obs = observe(sc, w) if on_obs is not None: on_obs(obs) t0 = timer() if timer else 0.0 override = False if driver == "expert": i = expert(obs) elif driver == "rule": i = rule_driver(obs) elif driver == "learned": i = learned(obs, layers) else: i, override = shield(obs, learned(obs, layers)) if timer: out["decision_ms"].append(1000.0 * (timer() - t0)) a = ACTIONS[i] out["actions"].append(i) if override: out["overrides"] += 1 if a <= -6.0 and prev_a > -6.0: out["hard_brakes"] += 1 out["events"].append({"t": round(w["t"], 2), "type": "hard_brake"}) prev_a = a s_front_prev = w["s"] + HALF w["v"] = min(VMAX, max(0.0, w["v"] + a * DT)) w["s"] += w["v"] * DT w["a"] = a step_agents(sc, w) w["t"] = (step + 1) * DT if s_front_prev < S_STOP <= w["s"] + HALF and light_ns(sc, w["t"])[0] == 2: out["red_violations"] += 1 out["events"].append({"t": round(w["t"], 2), "type": "red_light"}) ex, ey, eh = pose(route, w["s"]) ec = circles(ex, ey, eh) hit = None for c in w["cars"]: if c["active"] and hits_car(ec, circles(*car_xyh(c))): hit = "car" if w["lead"] is not None and w["lead"]["s"] < path_len(route) and hits_car(ec, circles(*pose(route, w["lead"]["s"]))): hit = "lead car" for p in w["peds"]: if not p["done"] and hits_ped(ec, p["x"], p["y"]): hit = "pedestrian" if record: out["trace"].append([round(w["t"], 2), round(ex, 3), round(ey, 3), round(eh, 4), round(w["v"], 3), i, int(override), [[round(v_, 2) for v_ in car_xyh(c)] for c in w["cars"] if c["active"]] + ([[round(v_, 2) for v_ in pose(route, w["lead"]["s"])]] if w["lead"] is not None and w["lead"]["s"] < path_len(route) else []), [[round(p["x"], 2), round(p["y"], 2)] for p in w["peds"] if not p["done"]], obs["visible"], light_ns(sc, w["t"])[0], light_ew(sc, w["t"])]) out["steps"] = step + 1 out["time"] = w["t"] if hit: out["collision"], out["collision_with"] = True, hit out["events"].append({"t": round(w["t"], 2), "type": "collision", "with": hit}) break if w["s"] >= goal: out["reached"] = True break return out # ------------------------------------------------------------------ batch jobs (used with multiprocessing) def summarize(r): return {k: r[k] for k in ("collision", "collision_with", "red_violations", "reached", "hard_brakes", "overrides", "steps", "time")} def collect_job(job): """job = (seed, kind, density, driver, layers). Runs the driver and labels every visited state with the expert.""" seed, kind, density, driver, layers = job X, y = [], [] def label(obs): X.append(features(obs)) y.append(expert(obs)) r = run_episode(make_scenario(seed, kind, density), driver, layers, record=False, on_obs=label) return X, y, summarize(r) def eval_job(job): seed, kind, density, driver, layers = job sc = make_scenario(seed, kind, density) r = run_episode(sc, driver, layers, record=False) return {"seed": seed, "kind": kind, "density": density, "driver": driver, "route": sc["route"], **summarize(r), "events": r["events"][-4:]}