From 915b439293e712b81b9fc6d8c8770eb378262af9 Mon Sep 17 00:00:00 2001 From: Catarina Ferreira Date: Wed, 1 Oct 2025 14:50:38 +0100 Subject: [PATCH 1/3] adding adas algo and webUI --- adas/adas_pothole_detector.py | 200 ++++++++++++++++++++++++++++++++++ adas/fake_publisher_Carla.py | 46 ++++++++ infotainment/heatmap.py | 113 +++++++++++++++++++ 3 files changed, 359 insertions(+) create mode 100644 adas/adas_pothole_detector.py create mode 100644 adas/fake_publisher_Carla.py create mode 100644 infotainment/heatmap.py diff --git a/adas/adas_pothole_detector.py b/adas/adas_pothole_detector.py new file mode 100644 index 0000000..4897b68 --- /dev/null +++ b/adas/adas_pothole_detector.py @@ -0,0 +1,200 @@ +#!/usr/bin/env python3 +import json +import math +import time +import threading +from collections import deque +from typing import Optional, Dict + +import zenoh +from zenoh import Encoding + +# --------- Topics ---------- +POSE_TOPIC = "vehicle/pose" +IMU_TOPIC = "vehicle/imu/raw" +POTHOLE_TOPIC = "detect/pothole/event" +ALERT_TOPIC = "hmi/alert" + +# --------- Tunables ---------- +ACC_Z_HP_ALPHA = 0.9 +ACC_SPIKE_THRESHOLD = 2.0 +MIN_SPEED_MPS = 4.0 +WINDOW_SEC = 0.5 +REFRACTORY_SEC = 1.5 +MERGE_DIST_M = 5.0 +SEVERITY_HIGH = 3.5 + +# --------- State ---------- +latest_pose: Dict = {} +imu_buf = deque() +last_event_ts = 0.0 +last_event_xy = None +lock = threading.Lock() + + +def zbytes_to_json(sample_payload): + text = sample_payload.to_string() + obj = json.loads(text) + if isinstance(obj, str): + obj = json.loads(obj) + return obj + + +class HighPass: + def __init__(self, alpha: float): + self.alpha = alpha + self.last_in = None + self.last_hp = 0.0 + + def step(self, x: float) -> float: + if self.last_in is None: + self.last_in = x + self.last_hp = 0.0 + return 0.0 + hp = self.alpha * (self.last_hp + x - self.last_in) + self.last_in = x + self.last_hp = hp + return hp + + +hp_z = HighPass(ACC_Z_HP_ALPHA) + + +def pose_cb(sample): + global latest_pose + try: + msg = zbytes_to_json(sample.payload) + with lock: + latest_pose = msg + except Exception as e: + print("POSE parse error:", e) + + +def imu_cb(sample): + try: + msg = zbytes_to_json(sample.payload) + ts = msg.get("ts", time.time()) + acc = msg.get("acc", {}) + az = float(acc.get("z", 0.0)) + ax = float(acc.get("x", 0.0)) + ay = float(acc.get("y", 0.0)) + + az_hp = hp_z.step(az) + + with lock: + imu_buf.append((ts, az_hp, ax, ay, az)) + cut = ts - WINDOW_SEC + while imu_buf and imu_buf[0][0] < cut: + imu_buf.popleft() + except Exception as e: + print("IMU parse error:", e) + + +def current_speed_and_pos() -> Optional[Dict]: + with lock: + if not latest_pose: + return None + return { + "speed_mps": float(latest_pose.get("speed_mps", 0.0)), + "x": latest_pose.get("x"), + "y": latest_pose.get("y"), + } + + +def severity_from_spike(spike: float) -> str: + return "HIGH" if abs(spike) >= SEVERITY_HIGH else "LOW" + + +def should_merge(prev_xy, new_xy) -> bool: + if prev_xy is None: + return False + (px, py) = prev_xy + (nx, ny) = new_xy + dist = math.hypot(nx - px, ny - py) + return dist <= MERGE_DIST_M + + +def detection_loop(session): + global last_event_ts, last_event_xy + + pothole_pub = session.declare_publisher(POTHOLE_TOPIC, encoding=Encoding.APPLICATION_JSON) + alert_pub = session.declare_publisher(ALERT_TOPIC, encoding=Encoding.APPLICATION_JSON) + + print("ADAS detection loop started…") + while True: + time.sleep(0.05) + + pose = current_speed_and_pos() + if not pose: + continue + + speed = pose["speed_mps"] + if speed < MIN_SPEED_MPS: + continue + + with lock: + if not imu_buf: + continue + spikes = [abs(s[1]) for s in imu_buf] + max_spike = max(spikes) if spikes else 0.0 + ts_latest = imu_buf[-1][0] + + if max_spike < ACC_SPIKE_THRESHOLD: + continue + + if (ts_latest - last_event_ts) < REFRACTORY_SEC: + new_xy = (float(pose["x"]), float(pose["y"])) if pose["x"] is not None else None + if new_xy and not should_merge(last_event_xy, new_xy): + pass + else: + continue + + # --- Projection from CARLA meters to Lisbon lat/lon --- + base_lat, base_lon = 38.7169, -9.1390 + lat = base_lat + float(pose["y"]) * 0.00001 + lon = base_lon + float(pose["x"]) * 0.00001 + + sev = severity_from_spike(max_spike) + event = { + "ts": ts_latest, + "lat": lat, + "lon": lon, + "severity": sev, + "score": round(min(0.99, max_spike / (SEVERITY_HIGH * 1.5)), 2), + } + + pothole_pub.put(json.dumps(event)) + + alert = { + "ts": ts_latest, + "level": "warning" if sev == "LOW" else "danger", + "title": "Road anomaly", + "msg": f"Pothole ({sev}) ahead", + } + alert_pub.put(json.dumps(alert)) + + print("🚧 Emitted:", event) + + last_event_ts = ts_latest + last_event_xy = (float(pose["x"]), float(pose["y"])) if pose["x"] is not None else None + + +def main(): + zconf = zenoh.Config() + with zenoh.open(zconf) as session: + session.declare_subscriber(POSE_TOPIC, pose_cb) + session.declare_subscriber(IMU_TOPIC, imu_cb) + + t = threading.Thread(target=detection_loop, args=(session,), daemon=True) + t.start() + + print("ADAS node running. Press Ctrl+C to exit.") + try: + while True: + time.sleep(1.0) + except KeyboardInterrupt: + print("Shutting down ADAS node…") + + +if __name__ == "__main__": + main() diff --git a/adas/fake_publisher_Carla.py b/adas/fake_publisher_Carla.py new file mode 100644 index 0000000..f50873a --- /dev/null +++ b/adas/fake_publisher_Carla.py @@ -0,0 +1,46 @@ +# fake_publisher.py +import time +import json +import math +import zenoh +from zenoh import Encoding + +POSE_TOPIC = "vehicle/pose" +IMU_TOPIC = "vehicle/imu/raw" + +def main(): + with zenoh.open(zenoh.Config()) as session: + pose_pub = session.declare_publisher(POSE_TOPIC, encoding=Encoding.APPLICATION_JSON) + imu_pub = session.declare_publisher(IMU_TOPIC, encoding=Encoding.APPLICATION_JSON) + + t = 0 + while True: + ts = time.time() + + # Fake pose: car moves along X axis, speed 12.3 m/s + pose = { + "ts": ts, + "x": 123.45 + t, + "y": 67.89, + "yaw": 0.0, + "speed_mps": 12.3 + } + pose_pub.put(json.dumps(pose)) + print("πŸ“‘ Published pose", pose) + + # Fake IMU: z acceleration alternates between normal (9.8) and a spike (12) + acc_z = 9.8 if t % 5 else 13.0 + imu = { + "ts": ts + 0.01, + "src": "carla", + "hz": 100, + "acc": { "x": 0.2, "y": 0.0, "z": acc_z } + } + imu_pub.put(json.dumps(imu)) + print("πŸ“‘ Published imu", imu) + + time.sleep(0.1) + t += 1 + +if __name__ == "__main__": + main() diff --git a/infotainment/heatmap.py b/infotainment/heatmap.py new file mode 100644 index 0000000..bf3ea6b --- /dev/null +++ b/infotainment/heatmap.py @@ -0,0 +1,113 @@ +import json +import streamlit as st +import folium +from folium.plugins import HeatMap +from streamlit_folium import st_folium +import zenoh +from streamlit_autorefresh import st_autorefresh + +# Topics +POSE_TOPIC = "vehicle/pose" +IMU_TOPIC = "vehicle/imu/raw" +POTHOLE_TOPIC = "detect/pothole/event" +ALERT_TOPIC = "hmi/alert" + +# ----------------- +# Helpers +# ----------------- +def safe_json_decode(payload): + try: + obj = json.loads(payload.to_string()) + if isinstance(obj, str): + obj = json.loads(obj) + return obj + except Exception as e: + return {"ERROR": {"message": str(e)}} + + +def init_zenoh(): + """Start zenoh subscribers once, store state in st.session_state""" + if "zenoh_state" not in st.session_state: + st.session_state["zenoh_state"] = { + "vehicle_pose": {}, + "imu": {}, + "potholes": [], + "alerts": [] + } + + state = st.session_state["zenoh_state"] + + def pose_cb(sample): state["vehicle_pose"] = safe_json_decode(sample.payload) + def imu_cb(sample): state["imu"] = safe_json_decode(sample.payload) + def pothole_cb(sample): + event = safe_json_decode(sample.payload) + if "ERROR" not in event: + if event.get("lat") is not None and event.get("lon") is not None: + state["potholes"].append(event) + def alert_cb(sample): + alert = safe_json_decode(sample.payload) + if "ERROR" not in alert: + state["alerts"].append(alert) + + if "zenoh_session" not in st.session_state: + session = zenoh.open(zenoh.Config()) + session.declare_subscriber(POSE_TOPIC, pose_cb) + session.declare_subscriber(IMU_TOPIC, imu_cb) + session.declare_subscriber(POTHOLE_TOPIC, pothole_cb) + session.declare_subscriber(ALERT_TOPIC, alert_cb) + st.session_state["zenoh_session"] = session + + +# ----------------- +# UI +# ----------------- +st.set_page_config(page_title="ADAS Heatmap", layout="wide") +st.title("πŸš— ADAS Infotainment Dashboard") + +# Initialize Zenoh +init_zenoh() + +col1, col2 = st.columns([1, 2]) + +with col1: + st.subheader("πŸ“‘ Vehicle Pose") + st.json(st.session_state["zenoh_state"]["vehicle_pose"]) + + st.subheader("πŸ“‘ IMU Data") + st.json(st.session_state["zenoh_state"]["imu"]) + + st.subheader("⚠️ Alerts") + alerts = st.session_state["zenoh_state"]["alerts"] + if alerts: + for a in alerts[-5:]: + st.warning(f"{a['title']}: {a['msg']} ({a['level']})") + else: + st.info("No alerts yet") + +with col2: + st.subheader("πŸ”₯ Pothole Heatmap (Lisbon)") + + # build base map only once + if "map" not in st.session_state: + st.session_state["map"] = folium.Map(location=[38.7169, -9.1390], zoom_start=15) + + # fresh heatmap layer each rerun + m = folium.Map(location=[38.7169, -9.1390], zoom_start=15) + + potholes = st.session_state["zenoh_state"]["potholes"] + heat_data = [ + [p["lat"], p["lon"], 1 if p["severity"] == "LOW" else 3] + for p in potholes + if p.get("lat") is not None and p.get("lon") is not None + ] + + if heat_data: + HeatMap(heat_data).add_to(m) + + st_folium(m, width=800, height=600, key="heatmap") + +# ----------------- +# Auto-refresh every 2s (keeps state in memory) +# ----------------- +st_autorefresh(interval=2000, key="refresh") + From c8276ddb32799f1e71d3b6b91a9954a40a083073 Mon Sep 17 00:00:00 2001 From: Catarina Ferreira Date: Wed, 1 Oct 2025 17:52:46 +0100 Subject: [PATCH 2/3] altered heatmap --- adas/adas_pothole_detector.py | 34 ++-- adas/fake_publisher_Carla.py | 1 - infotainment/heatmap.py | 301 +++++++++++++++++++++++++++------- 3 files changed, 261 insertions(+), 75 deletions(-) diff --git a/adas/adas_pothole_detector.py b/adas/adas_pothole_detector.py index 4897b68..4e1c338 100644 --- a/adas/adas_pothole_detector.py +++ b/adas/adas_pothole_detector.py @@ -64,12 +64,27 @@ def pose_cb(sample): global latest_pose try: msg = zbytes_to_json(sample.payload) + + # Convert CARLA x,y (meters) β†’ lat/lon once + base_lat, base_lon = 38.711046, -9.138637 + lat = base_lat + float(msg.get("y", 0.0)) * 0.00001 + lon = base_lon + float(msg.get("x", 0.0)) * 0.00001 + + msg_converted = { + "ts": msg.get("ts", time.time()), + "lat": lat, + "lon": lon, + "yaw": msg.get("yaw", 0.0), + "speed_mps": msg.get("speed_mps", 0.0) + } + with lock: - latest_pose = msg + latest_pose = msg_converted except Exception as e: print("POSE parse error:", e) + def imu_cb(sample): try: msg = zbytes_to_json(sample.payload) @@ -96,8 +111,8 @@ def current_speed_and_pos() -> Optional[Dict]: return None return { "speed_mps": float(latest_pose.get("speed_mps", 0.0)), - "x": latest_pose.get("x"), - "y": latest_pose.get("y"), + "lat": latest_pose.get("lat"), + "lon": latest_pose.get("lon"), } @@ -128,6 +143,7 @@ def detection_loop(session): if not pose: continue + lat, lon = pose["lat"], pose["lon"] speed = pose["speed_mps"] if speed < MIN_SPEED_MPS: continue @@ -143,17 +159,13 @@ def detection_loop(session): continue if (ts_latest - last_event_ts) < REFRACTORY_SEC: - new_xy = (float(pose["x"]), float(pose["y"])) if pose["x"] is not None else None + # use last lat/lon for merging instead of x,y + new_xy = (lat, lon) if lat is not None and lon is not None else None if new_xy and not should_merge(last_event_xy, new_xy): pass else: continue - # --- Projection from CARLA meters to Lisbon lat/lon --- - base_lat, base_lon = 38.7169, -9.1390 - lat = base_lat + float(pose["y"]) * 0.00001 - lon = base_lon + float(pose["x"]) * 0.00001 - sev = severity_from_spike(max_spike) event = { "ts": ts_latest, @@ -176,7 +188,7 @@ def detection_loop(session): print("🚧 Emitted:", event) last_event_ts = ts_latest - last_event_xy = (float(pose["x"]), float(pose["y"])) if pose["x"] is not None else None + last_event_xy = (lat, lon) def main(): @@ -197,4 +209,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/adas/fake_publisher_Carla.py b/adas/fake_publisher_Carla.py index f50873a..c1db9e8 100644 --- a/adas/fake_publisher_Carla.py +++ b/adas/fake_publisher_Carla.py @@ -1,4 +1,3 @@ -# fake_publisher.py import time import json import math diff --git a/infotainment/heatmap.py b/infotainment/heatmap.py index bf3ea6b..33c550d 100644 --- a/infotainment/heatmap.py +++ b/infotainment/heatmap.py @@ -1,16 +1,52 @@ +#!/usr/bin/env python3 import json +import time +import sqlite3 +import math import streamlit as st -import folium -from folium.plugins import HeatMap -from streamlit_folium import st_folium +import pydeck as pdk import zenoh from streamlit_autorefresh import st_autorefresh -# Topics -POSE_TOPIC = "vehicle/pose" -IMU_TOPIC = "vehicle/imu/raw" -POTHOLE_TOPIC = "detect/pothole/event" -ALERT_TOPIC = "hmi/alert" +DB_PATH = "adas.db" + +# Use the SAME anchor & scale as your ADAS node +ANCHOR_LAT = 38.711046 +ANCHOR_LON = -9.138637 +METERS_TO_DEGREES = 1e-5 # same 0.00001 you used in ADAS + +# ----------------- +# DB Setup +# ----------------- +def init_db(): + conn = sqlite3.connect(DB_PATH, check_same_thread=False) + cur = conn.cursor() + cur.execute("PRAGMA journal_mode=WAL;") + + # Pose stores lat/lon now + cur.execute("""CREATE TABLE IF NOT EXISTS pose ( + ts REAL, lat REAL, lon REAL, yaw REAL, speed_mps REAL + )""") + cur.execute("""CREATE TABLE IF NOT EXISTS imu ( + ts REAL, ax REAL, ay REAL, az REAL + )""") + cur.execute("""CREATE TABLE IF NOT EXISTS potholes ( + ts REAL, lat REAL, lon REAL, severity TEXT + )""") + cur.execute("""CREATE TABLE IF NOT EXISTS alerts ( + ts REAL, level TEXT, title TEXT, msg TEXT + )""") + + # Clear tables once per Streamlit session (nice for dev) + if "db_cleared" not in st.session_state: + for table in ["pose", "imu", "potholes", "alerts"]: + cur.execute(f"DELETE FROM {table}") + conn.commit() + st.session_state["db_cleared"] = True + + return conn + +conn = init_db() # ----------------- # Helpers @@ -24,90 +60,229 @@ def safe_json_decode(payload): except Exception as e: return {"ERROR": {"message": str(e)}} - def init_zenoh(): - """Start zenoh subscribers once, store state in st.session_state""" - if "zenoh_state" not in st.session_state: - st.session_state["zenoh_state"] = { - "vehicle_pose": {}, - "imu": {}, - "potholes": [], - "alerts": [] - } - - state = st.session_state["zenoh_state"] - - def pose_cb(sample): state["vehicle_pose"] = safe_json_decode(sample.payload) - def imu_cb(sample): state["imu"] = safe_json_decode(sample.payload) + if "zenoh_inited" in st.session_state: + return + st.session_state["zenoh_inited"] = True + + def pose_cb(sample): + msg = safe_json_decode(sample.payload) + if "ERROR" in msg: + return + + # Robust: if publisher ever sends lat/lon, use them; otherwise convert x/y (meters) + if "lat" in msg and "lon" in msg: + lat = float(msg.get("lat", 0.0)) + lon = float(msg.get("lon", 0.0)) + else: + x_m = float(msg.get("x", 0.0)) + y_m = float(msg.get("y", 0.0)) + lat = ANCHOR_LAT + y_m * METERS_TO_DEGREES + lon = ANCHOR_LON + x_m * METERS_TO_DEGREES + + c = sqlite3.connect(DB_PATH) + c.execute( + "INSERT INTO pose (ts, lat, lon, yaw, speed_mps) VALUES (?, ?, ?, ?, ?)", + ( + float(msg.get("ts", time.time())), + lat, + lon, + float(msg.get("yaw", 0.0)), + float(msg.get("speed_mps", 0.0)), + ), + ) + c.commit() + c.close() + + def imu_cb(sample): + msg = safe_json_decode(sample.payload) + if "ERROR" in msg: + return + acc = msg.get("acc", {}) + c = sqlite3.connect(DB_PATH) + c.execute( + "INSERT INTO imu (ts, ax, ay, az) VALUES (?, ?, ?, ?)", + ( + float(msg.get("ts", time.time())), + float(acc.get("x", 0.0)), + float(acc.get("y", 0.0)), + float(acc.get("z", 0.0)), + ), + ) + c.commit() + c.close() + def pothole_cb(sample): - event = safe_json_decode(sample.payload) - if "ERROR" not in event: - if event.get("lat") is not None and event.get("lon") is not None: - state["potholes"].append(event) - def alert_cb(sample): - alert = safe_json_decode(sample.payload) - if "ERROR" not in alert: - state["alerts"].append(alert) + msg = safe_json_decode(sample.payload) + if "ERROR" in msg: + return + if msg.get("lat") is None or msg.get("lon") is None: + return + c = sqlite3.connect(DB_PATH) + c.execute( + "INSERT INTO potholes (ts, lat, lon, severity) VALUES (?, ?, ?, ?)", + ( + float(msg.get("ts", time.time())), + float(msg["lat"]), + float(msg["lon"]), + str(msg.get("severity", "LOW")), + ), + ) + c.commit() + c.close() - if "zenoh_session" not in st.session_state: - session = zenoh.open(zenoh.Config()) - session.declare_subscriber(POSE_TOPIC, pose_cb) - session.declare_subscriber(IMU_TOPIC, imu_cb) - session.declare_subscriber(POTHOLE_TOPIC, pothole_cb) - session.declare_subscriber(ALERT_TOPIC, alert_cb) - st.session_state["zenoh_session"] = session + def alert_cb(sample): + msg = safe_json_decode(sample.payload) + if "ERROR" in msg: + return + c = sqlite3.connect(DB_PATH) + c.execute( + "INSERT INTO alerts (ts, level, title, msg) VALUES (?, ?, ?, ?)", + ( + float(msg.get("ts", time.time())), + str(msg.get("level", "")), + str(msg.get("title", "")), + str(msg.get("msg", "")), + ), + ) + c.commit() + c.close() + session = zenoh.open(zenoh.Config()) + session.declare_subscriber("vehicle/pose", pose_cb) + session.declare_subscriber("vehicle/imu/raw", imu_cb) + session.declare_subscriber("detect/pothole/event", pothole_cb) + session.declare_subscriber("hmi/alert", alert_cb) + st.session_state["zenoh_session"] = session # ----------------- # UI # ----------------- -st.set_page_config(page_title="ADAS Heatmap", layout="wide") -st.title("πŸš— ADAS Infotainment Dashboard") +st.set_page_config(page_title="ADAS Heatmap (DB)", layout="wide") +st.title("πŸš— ADAS Infotainment Dashboard with DB") -# Initialize Zenoh +# start zenoh subscribers init_zenoh() +# auto-refresh every 2s +st_autorefresh(interval=2000, key="refresh_db_view") + col1, col2 = st.columns([1, 2]) with col1: st.subheader("πŸ“‘ Vehicle Pose") - st.json(st.session_state["zenoh_state"]["vehicle_pose"]) + pose_row = conn.execute( + "SELECT ts, lat, lon, yaw, speed_mps FROM pose ORDER BY ts DESC LIMIT 1" + ).fetchone() + if pose_row: + st.json({ + "ts": pose_row[0], + "lat": pose_row[1], + "lon": pose_row[2], + "yaw": pose_row[3], + "speed_mps": pose_row[4] + }) + else: + st.info("No pose yet") st.subheader("πŸ“‘ IMU Data") - st.json(st.session_state["zenoh_state"]["imu"]) + imu_row = conn.execute( + "SELECT ts, ax, ay, az FROM imu ORDER BY ts DESC LIMIT 1" + ).fetchone() + if imu_row: + st.json({"ts": imu_row[0], "ax": imu_row[1], "ay": imu_row[2], "az": imu_row[3]}) + else: + st.info("No IMU yet") st.subheader("⚠️ Alerts") - alerts = st.session_state["zenoh_state"]["alerts"] - if alerts: - for a in alerts[-5:]: - st.warning(f"{a['title']}: {a['msg']} ({a['level']})") + alert_rows = conn.execute( + "SELECT ts, level, title, msg FROM alerts ORDER BY ts DESC LIMIT 5" + ).fetchall() + if alert_rows: + for a in alert_rows: + st.warning(f"{a[2]}: {a[3]} ({a[1]})") else: st.info("No alerts yet") with col2: - st.subheader("πŸ”₯ Pothole Heatmap (Lisbon)") + st.subheader("πŸ”₯ Vehicle Path + Potholes") + + # Pose path (already lat/lon in DB) + pose_rows = conn.execute( + "SELECT lat, lon, speed_mps FROM pose ORDER BY ts DESC LIMIT 200" + ).fetchall() + path_points = [{"lon": lon, "lat": lat, "speed": float(v)} for (lat, lon, v) in reversed(pose_rows)] - # build base map only once - if "map" not in st.session_state: - st.session_state["map"] = folium.Map(location=[38.7169, -9.1390], zoom_start=15) + # Vehicle path segments + segments = [] + for i in range(len(path_points) - 1): + a, b = path_points[i], path_points[i + 1] + red = int(min(255, a["speed"] * 10)) + green = 255 - red + segments.append({ + "sourcePosition": [a["lon"], a["lat"]], + "targetPosition": [b["lon"], b["lat"]], + "color": [red, green, 0] + }) + + # Car icon at latest pose + if path_points: + car_lat, car_lon = path_points[-1]["lat"], path_points[-1]["lon"] + else: + car_lat, car_lon = ANCHOR_LAT, ANCHOR_LON - # fresh heatmap layer each rerun - m = folium.Map(location=[38.7169, -9.1390], zoom_start=15) + # Potholes (already lat/lon from ADAS) + pothole_rows = conn.execute( + "SELECT ts, lat, lon, severity FROM potholes ORDER BY ts DESC LIMIT 200" + ).fetchall() + pothole_data = [] + for ts, lat, lon, severity in pothole_rows: + weight = 1 if severity == "LOW" else 3 + pothole_data.append({"lon": float(lon), "lat": float(lat), "weight": weight}) - potholes = st.session_state["zenoh_state"]["potholes"] - heat_data = [ - [p["lat"], p["lon"], 1 if p["severity"] == "LOW" else 3] - for p in potholes - if p.get("lat") is not None and p.get("lon") is not None - ] + view_state = pdk.ViewState(latitude=car_lat, longitude=car_lon, zoom=15) - if heat_data: - HeatMap(heat_data).add_to(m) + layers = [] + if segments: + layers.append(pdk.Layer("LineLayer", data=segments, + get_source_position="sourcePosition", + get_target_position="targetPosition", + get_color="color", get_width=3)) + car_icon = { + "url": "https://cdn-icons-png.flaticon.com/512/61/61168.png", + "width": 512, + "height": 512, + "anchorY": 512 + } + layers.append(pdk.Layer("IconLayer", + data=[{"lat": car_lat, "lon": car_lon, "icon": car_icon}], + get_icon="icon", get_size=4, size_scale=6, + get_position="[lon, lat]")) + if pothole_data: + layers.append(pdk.Layer("HeatmapLayer", + data=pothole_data, + get_position="[lon, lat]", + get_weight="weight", + radiusPixels=40)) - st_folium(m, width=800, height=600, key="heatmap") + deck = pdk.Deck(layers=layers, initial_view_state=view_state, + map_style="light", + tooltip={"text": "Pothole\nLat: {lat}\nLon: {lon}"}) + st.pydeck_chart(deck, use_container_width=True) # ----------------- -# Auto-refresh every 2s (keeps state in memory) +# Debug section # ----------------- -st_autorefresh(interval=2000, key="refresh") +st.subheader("πŸ—„οΈ Debug DB contents") +with st.expander("Show counts"): + counts = {} + for table in ["pose", "imu", "potholes", "alerts"]: + c = conn.execute(f"SELECT COUNT(*) FROM {table}").fetchone()[0] + counts[table] = c + st.write(counts) +with st.expander("Show recent rows"): + st.write("Pose", conn.execute("SELECT * FROM pose ORDER BY ts DESC LIMIT 5").fetchall()) + st.write("IMU", conn.execute("SELECT * FROM imu ORDER BY ts DESC LIMIT 5").fetchall()) + st.write("Potholes", conn.execute("SELECT * FROM potholes ORDER BY ts DESC LIMIT 5").fetchall()) + st.write("Alerts", conn.execute("SELECT * FROM alerts ORDER BY ts DESC LIMIT 5").fetchall()) From ac533ed48cd6c8f229c662b2cc8b48dc1155e258 Mon Sep 17 00:00:00 2001 From: Catarina Ferreira Date: Wed, 1 Oct 2025 18:48:37 +0100 Subject: [PATCH 3/3] refactored code --- adas/adas_pothole_detector.py | 106 +++++++++++++------------------- adas/fake_publisher_Carla.py | 52 +++++++++++----- infotainment/heatmap.py | 110 +++++++++++++++++++++------------- requirements.txt | 7 +++ 4 files changed, 157 insertions(+), 118 deletions(-) create mode 100644 requirements.txt diff --git a/adas/adas_pothole_detector.py b/adas/adas_pothole_detector.py index 4e1c338..84a98cc 100644 --- a/adas/adas_pothole_detector.py +++ b/adas/adas_pothole_detector.py @@ -17,11 +17,11 @@ # --------- Tunables ---------- ACC_Z_HP_ALPHA = 0.9 -ACC_SPIKE_THRESHOLD = 2.0 +ACC_SPIKE_THRESHOLD = 2.0 # g spike threshold MIN_SPEED_MPS = 4.0 WINDOW_SEC = 0.5 -REFRACTORY_SEC = 1.5 -MERGE_DIST_M = 5.0 +REFRACTORY_SEC = 1.5 # min time between events +MERGE_DIST_M = 5.0 # potholes within 5m = same SEVERITY_HIGH = 3.5 # --------- State ---------- @@ -31,7 +31,7 @@ last_event_xy = None lock = threading.Lock() - +# --------- Utils ---------- def zbytes_to_json(sample_payload): text = sample_payload.to_string() obj = json.loads(text) @@ -39,7 +39,6 @@ def zbytes_to_json(sample_payload): obj = json.loads(obj) return obj - class HighPass: def __init__(self, alpha: float): self.alpha = alpha @@ -56,19 +55,45 @@ def step(self, x: float) -> float: self.last_hp = hp return hp - hp_z = HighPass(ACC_Z_HP_ALPHA) +def haversine(lat1, lon1, lat2, lon2): + """Great-circle distance in meters.""" + R = 6371000 + phi1, phi2 = math.radians(lat1), math.radians(lat2) + dphi = math.radians(lat2 - lat1) + dlambda = math.radians(lon2 - lon1) + a = math.sin(dphi/2)**2 + math.cos(phi1) * math.cos(phi2) * math.sin(dlambda/2)**2 + c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) + return R * c + +def should_merge(prev_xy, new_xy) -> bool: + if prev_xy is None or new_xy is None: + return False + plat, plon = prev_xy + nlat, nlon = new_xy + return haversine(plat, plon, nlat, nlon) <= MERGE_DIST_M +# --------- Callbacks ---------- def pose_cb(sample): + """Convert Carla x/y (meters) to lat/lon once.""" global latest_pose try: msg = zbytes_to_json(sample.payload) - # Convert CARLA x,y (meters) β†’ lat/lon once + # Anchor: GraΓ§a / Alfama in Lisbon base_lat, base_lon = 38.711046, -9.138637 - lat = base_lat + float(msg.get("y", 0.0)) * 0.00001 - lon = base_lon + float(msg.get("x", 0.0)) * 0.00001 + + # Conversion factors: meters β†’ degrees at Lisbon latitude + M_PER_DEG_LAT = 1.0 / 111111.0 # ~0.000009Β° + M_PER_DEG_LON = 1.0 / 88000.0 # ~0.000011Β° at 38.7Β°N + + x_m = float(msg.get("x", 0.0)) # east-west (meters) + y_m = float(msg.get("y", 0.0)) # north-south (meters) + + # Apply proper scaling + lat = base_lat + y_m * M_PER_DEG_LAT + lon = base_lon + x_m * M_PER_DEG_LON msg_converted = { "ts": msg.get("ts", time.time()), @@ -83,52 +108,33 @@ def pose_cb(sample): except Exception as e: print("POSE parse error:", e) - - def imu_cb(sample): try: msg = zbytes_to_json(sample.payload) ts = msg.get("ts", time.time()) acc = msg.get("acc", {}) az = float(acc.get("z", 0.0)) - ax = float(acc.get("x", 0.0)) - ay = float(acc.get("y", 0.0)) az_hp = hp_z.step(az) with lock: - imu_buf.append((ts, az_hp, ax, ay, az)) + imu_buf.append((ts, az_hp)) cut = ts - WINDOW_SEC while imu_buf and imu_buf[0][0] < cut: imu_buf.popleft() except Exception as e: print("IMU parse error:", e) - def current_speed_and_pos() -> Optional[Dict]: with lock: if not latest_pose: return None - return { - "speed_mps": float(latest_pose.get("speed_mps", 0.0)), - "lat": latest_pose.get("lat"), - "lon": latest_pose.get("lon"), - } - + return latest_pose def severity_from_spike(spike: float) -> str: return "HIGH" if abs(spike) >= SEVERITY_HIGH else "LOW" - -def should_merge(prev_xy, new_xy) -> bool: - if prev_xy is None: - return False - (px, py) = prev_xy - (nx, ny) = new_xy - dist = math.hypot(nx - px, ny - py) - return dist <= MERGE_DIST_M - - +# --------- Detection Loop ----------# --------- Detection Loop ---------- def detection_loop(session): global last_event_ts, last_event_xy @@ -155,42 +161,15 @@ def detection_loop(session): max_spike = max(spikes) if spikes else 0.0 ts_latest = imu_buf[-1][0] + # only trigger if above threshold if max_spike < ACC_SPIKE_THRESHOLD: continue - if (ts_latest - last_event_ts) < REFRACTORY_SEC: - # use last lat/lon for merging instead of x,y - new_xy = (lat, lon) if lat is not None and lon is not None else None - if new_xy and not should_merge(last_event_xy, new_xy): - pass - else: - continue - - sev = severity_from_spike(max_spike) - event = { - "ts": ts_latest, - "lat": lat, - "lon": lon, - "severity": sev, - "score": round(min(0.99, max_spike / (SEVERITY_HIGH * 1.5)), 2), - } - - pothole_pub.put(json.dumps(event)) - - alert = { - "ts": ts_latest, - "level": "warning" if sev == "LOW" else "danger", - "title": "Road anomaly", - "msg": f"Pothole ({sev}) ahead", - } - alert_pub.put(json.dumps(alert)) - - print("🚧 Emitted:", event) - - last_event_ts = ts_latest - last_event_xy = (lat, lon) + # refractory & merge check + if (ts_latest - last_event_ts) < REFRACTORY_SEC and sh +# --------- Main ---------- def main(): zconf = zenoh.Config() with zenoh.open(zconf) as session: @@ -207,6 +186,5 @@ def main(): except KeyboardInterrupt: print("Shutting down ADAS node…") - if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/adas/fake_publisher_Carla.py b/adas/fake_publisher_Carla.py index c1db9e8..1e2e6c7 100644 --- a/adas/fake_publisher_Carla.py +++ b/adas/fake_publisher_Carla.py @@ -1,45 +1,69 @@ -import time +#!/usr/bin/env python3 import json -import math +import time import zenoh from zenoh import Encoding POSE_TOPIC = "vehicle/pose" IMU_TOPIC = "vehicle/imu/raw" +# Road length = 2000 m +ROAD_LENGTH_M = 2000 +CAR_SPEED = 12.3 # m/s (~44 km/h) +STEP = 1.0 # seconds per tick + +# Pothole distances along the route (in meters) +POTHOLE_POSITIONS = [500, 1000, 1500] + def main(): with zenoh.open(zenoh.Config()) as session: pose_pub = session.declare_publisher(POSE_TOPIC, encoding=Encoding.APPLICATION_JSON) imu_pub = session.declare_publisher(IMU_TOPIC, encoding=Encoding.APPLICATION_JSON) - t = 0 - while True: + pos_x = 0.0 + ts = time.time() + + while pos_x <= ROAD_LENGTH_M: ts = time.time() - # Fake pose: car moves along X axis, speed 12.3 m/s + # Car pose (drives east along lon, lat fixed) + base_lat, base_lon = 38.71165701061101, -9.138637 + M_PER_DEG_LON = 1.0 / 88000.0 + + lat = base_lat + lon = base_lon + pos_x * M_PER_DEG_LON + pose = { "ts": ts, - "x": 123.45 + t, - "y": 67.89, + "x": pos_x, + "y": 0.0, + "lat": lat, + "lon": lon, "yaw": 0.0, - "speed_mps": 12.3 + "speed_mps": CAR_SPEED, } pose_pub.put(json.dumps(pose)) print("πŸ“‘ Published pose", pose) - # Fake IMU: z acceleration alternates between normal (9.8) and a spike (12) - acc_z = 9.8 if t % 5 else 13.0 + # IMU with spike if near pothole + if any(abs(pos_x - p) < CAR_SPEED for p in POTHOLE_POSITIONS): + acc_z = 13.0 # simulate spike at pothole + else: + acc_z = 9.8 + imu = { "ts": ts + 0.01, - "src": "carla", + "src": "carla-sim", "hz": 100, - "acc": { "x": 0.2, "y": 0.0, "z": acc_z } + "acc": {"x": 0.2, "y": 0.0, "z": acc_z}, } imu_pub.put(json.dumps(imu)) print("πŸ“‘ Published imu", imu) - time.sleep(0.1) - t += 1 + time.sleep(STEP) + pos_x += CAR_SPEED * STEP + + print("βœ… Simulation finished: car reached end of 2 km road") if __name__ == "__main__": main() diff --git a/infotainment/heatmap.py b/infotainment/heatmap.py index 33c550d..7b9612d 100644 --- a/infotainment/heatmap.py +++ b/infotainment/heatmap.py @@ -2,7 +2,6 @@ import json import time import sqlite3 -import math import streamlit as st import pydeck as pdk import zenoh @@ -10,10 +9,10 @@ DB_PATH = "adas.db" -# Use the SAME anchor & scale as your ADAS node +# Same anchor & scale as the ADAS node ANCHOR_LAT = 38.711046 ANCHOR_LON = -9.138637 -METERS_TO_DEGREES = 1e-5 # same 0.00001 you used in ADAS +METERS_TO_DEGREES = 1e-5 # ----------------- # DB Setup @@ -23,7 +22,6 @@ def init_db(): cur = conn.cursor() cur.execute("PRAGMA journal_mode=WAL;") - # Pose stores lat/lon now cur.execute("""CREATE TABLE IF NOT EXISTS pose ( ts REAL, lat REAL, lon REAL, yaw REAL, speed_mps REAL )""") @@ -37,7 +35,7 @@ def init_db(): ts REAL, level TEXT, title TEXT, msg TEXT )""") - # Clear tables once per Streamlit session (nice for dev) + # Clear once per Streamlit session (handy in dev) if "db_cleared" not in st.session_state: for table in ["pose", "imu", "potholes", "alerts"]: cur.execute(f"DELETE FROM {table}") @@ -70,15 +68,24 @@ def pose_cb(sample): if "ERROR" in msg: return - # Robust: if publisher ever sends lat/lon, use them; otherwise convert x/y (meters) - if "lat" in msg and "lon" in msg: - lat = float(msg.get("lat", 0.0)) - lon = float(msg.get("lon", 0.0)) - else: + lat = msg.get("lat") + lon = msg.get("lon") + + if lat is None or lon is None: + # fallback to convert x/y from CARLA world coords x_m = float(msg.get("x", 0.0)) y_m = float(msg.get("y", 0.0)) - lat = ANCHOR_LAT + y_m * METERS_TO_DEGREES - lon = ANCHOR_LON + x_m * METERS_TO_DEGREES + + base_lat, base_lon = 38.711046, -9.138637 + M_PER_DEG_LAT = 1.0 / 111111.0 + M_PER_DEG_LON = 1.0 / 88000.0 + + lat = base_lat + y_m * M_PER_DEG_LAT + lon = base_lon + x_m * M_PER_DEG_LON + + # ensure numeric + lat = float(lat) + lon = float(lon) c = sqlite3.connect(DB_PATH) c.execute( @@ -94,6 +101,7 @@ def pose_cb(sample): c.commit() c.close() + def imu_cb(sample): msg = safe_json_decode(sample.payload) if "ERROR" in msg: @@ -116,21 +124,25 @@ def pothole_cb(sample): msg = safe_json_decode(sample.payload) if "ERROR" in msg: return - if msg.get("lat") is None or msg.get("lon") is None: - return + + # potholes from ADAS are already in lat/lon β†’ trust directly + if "lat" not in msg or "lon" not in msg: + return # skip invalid pothole messages + c = sqlite3.connect(DB_PATH) c.execute( "INSERT INTO potholes (ts, lat, lon, severity) VALUES (?, ?, ?, ?)", ( float(msg.get("ts", time.time())), - float(msg["lat"]), - float(msg["lon"]), + float(msg["lat"]), # no conversion + float(msg["lon"]), # no conversion str(msg.get("severity", "LOW")), ), ) c.commit() c.close() + def alert_cb(sample): msg = safe_json_decode(sample.payload) if "ERROR" in msg: @@ -161,10 +173,7 @@ def alert_cb(sample): st.set_page_config(page_title="ADAS Heatmap (DB)", layout="wide") st.title("πŸš— ADAS Infotainment Dashboard with DB") -# start zenoh subscribers init_zenoh() - -# auto-refresh every 2s st_autorefresh(interval=2000, key="refresh_db_view") col1, col2 = st.columns([1, 2]) @@ -207,7 +216,7 @@ def alert_cb(sample): with col2: st.subheader("πŸ”₯ Vehicle Path + Potholes") - # Pose path (already lat/lon in DB) + # Path points (already lat/lon) pose_rows = conn.execute( "SELECT lat, lon, speed_mps FROM pose ORDER BY ts DESC LIMIT 200" ).fetchall() @@ -217,6 +226,7 @@ def alert_cb(sample): segments = [] for i in range(len(path_points) - 1): a, b = path_points[i], path_points[i + 1] + # simple speed-based color red = int(min(255, a["speed"] * 10)) green = 255 - red segments.append({ @@ -225,7 +235,7 @@ def alert_cb(sample): "color": [red, green, 0] }) - # Car icon at latest pose + # Latest car position (safe fallback to anchor) if path_points: car_lat, car_lon = path_points[-1]["lat"], path_points[-1]["lon"] else: @@ -240,38 +250,58 @@ def alert_cb(sample): weight = 1 if severity == "LOW" else 3 pothole_data.append({"lon": float(lon), "lat": float(lat), "weight": weight}) - view_state = pdk.ViewState(latitude=car_lat, longitude=car_lon, zoom=15) + # βœ… Fixed center on Lisbon anchor (static map) + view_state = pdk.ViewState( + latitude=ANCHOR_LAT, + longitude=ANCHOR_LON, + zoom=14 # zoom out a bit so you always see car + potholes + ) layers = [] if segments: - layers.append(pdk.Layer("LineLayer", data=segments, - get_source_position="sourcePosition", - get_target_position="targetPosition", - get_color="color", get_width=3)) + layers.append(pdk.Layer( + "LineLayer", + data=segments, + get_source_position="sourcePosition", + get_target_position="targetPosition", + get_color="color", + get_width=3 + )) + car_icon = { "url": "https://cdn-icons-png.flaticon.com/512/61/61168.png", "width": 512, "height": 512, "anchorY": 512 } - layers.append(pdk.Layer("IconLayer", - data=[{"lat": car_lat, "lon": car_lon, "icon": car_icon}], - get_icon="icon", get_size=4, size_scale=6, - get_position="[lon, lat]")) + layers.append(pdk.Layer( + "IconLayer", + data=[{"lat": car_lat, "lon": car_lon, "icon": car_icon}], + get_icon="icon", + get_size=4, + size_scale=6, + get_position="[lon, lat]" + )) + if pothole_data: - layers.append(pdk.Layer("HeatmapLayer", - data=pothole_data, - get_position="[lon, lat]", - get_weight="weight", - radiusPixels=40)) - - deck = pdk.Deck(layers=layers, initial_view_state=view_state, - map_style="light", - tooltip={"text": "Pothole\nLat: {lat}\nLon: {lon}"}) + layers.append(pdk.Layer( + "HeatmapLayer", + data=pothole_data, + get_position="[lon, lat]", + get_weight="weight", + radiusPixels=40 + )) + + deck = pdk.Deck( + layers=layers, + initial_view_state=view_state, + map_style="light", + tooltip={"text": "Pothole\nLat: {lat}\nLon: {lon}"} + ) st.pydeck_chart(deck, use_container_width=True) # ----------------- -# Debug section +# Debug # ----------------- st.subheader("πŸ—„οΈ Debug DB contents") with st.expander("Show counts"): diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..8e7fa88 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,7 @@ +streamlit>=1.34 +pydeck>=0.8 +pandas>=2.0 +zenoh>=0.11 +folium>=0.15 +streamlit-folium>=0.10 +pyproj>=3.7.2 \ No newline at end of file