Download drive_core.py from thelamapi/Next-Drive: direct link, hf CLI and curl.
- Browser
- Download file 21.7 kB
-
https://huggingface.co/thelamapi/Next-Drive/resolve/main/drive_core.py
- Command line
-
hf download hf://thelamapi/Next-Drive/drive_core.py
-
curl -L -o drive_core.py https://huggingface.co/thelamapi/Next-Drive/resolve/main/drive_core.py
21.7 kB
| """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:]} | |