diff --git a/adas/adas_pothole_detector.py b/adas/adas_pothole_detector.py new file mode 100644 index 0000000..84a98cc --- /dev/null +++ b/adas/adas_pothole_detector.py @@ -0,0 +1,190 @@ +#!/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 # g spike threshold +MIN_SPEED_MPS = 4.0 +WINDOW_SEC = 0.5 +REFRACTORY_SEC = 1.5 # min time between events +MERGE_DIST_M = 5.0 # potholes within 5m = same +SEVERITY_HIGH = 3.5 + +# --------- State ---------- +latest_pose: Dict = {} +imu_buf = deque() +last_event_ts = 0.0 +last_event_xy = None +lock = threading.Lock() + +# --------- Utils ---------- +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 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) + + # Anchor: Graça / Alfama in Lisbon + base_lat, base_lon = 38.711046, -9.138637 + + # 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()), + "lat": lat, + "lon": lon, + "yaw": msg.get("yaw", 0.0), + "speed_mps": msg.get("speed_mps", 0.0) + } + + with lock: + latest_pose = msg_converted + 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)) + + az_hp = hp_z.step(az) + + with lock: + 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 latest_pose + +def severity_from_spike(spike: float) -> str: + return "HIGH" if abs(spike) >= SEVERITY_HIGH else "LOW" + +# --------- Detection Loop ----------# --------- Detection Loop ---------- +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 + + lat, lon = pose["lat"], pose["lon"] + 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] + + # only trigger if above threshold + if max_spike < ACC_SPIKE_THRESHOLD: + continue + + # 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: + 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..1e2e6c7 --- /dev/null +++ b/adas/fake_publisher_Carla.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 +import json +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) + + pos_x = 0.0 + ts = time.time() + + while pos_x <= ROAD_LENGTH_M: + ts = time.time() + + # 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": pos_x, + "y": 0.0, + "lat": lat, + "lon": lon, + "yaw": 0.0, + "speed_mps": CAR_SPEED, + } + pose_pub.put(json.dumps(pose)) + print("📡 Published pose", pose) + + # 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-sim", + "hz": 100, + "acc": {"x": 0.2, "y": 0.0, "z": acc_z}, + } + imu_pub.put(json.dumps(imu)) + print("📡 Published imu", imu) + + 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 bf3ea6b..f7f18e5 100644 --- a/infotainment/heatmap.py +++ b/infotainment/heatmap.py @@ -1,16 +1,52 @@ +#!/usr/bin/env python3 import json +import time +import sqlite3 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" + +# Same anchor & scale as the ADAS node +ANCHOR_LAT = 38.711046 +ANCHOR_LON = -9.138637 +METERS_TO_DEGREES = 1e-5 + +# ----------------- +# DB Setup +# ----------------- +def init_db(): + conn = sqlite3.connect(DB_PATH, check_same_thread=False) + cur = conn.cursor() + cur.execute("PRAGMA journal_mode=WAL;") + + 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 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}") + conn.commit() + st.session_state["db_cleared"] = True + + return conn + +conn = init_db() # ----------------- # Helpers @@ -24,90 +60,261 @@ 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 + + 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)) + + 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( + "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 + + # 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"]), # no conversion + float(msg["lon"]), # no conversion + 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 init_zenoh() +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") + + # Path points (already lat/lon) + 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)] + + # Vehicle path segments + 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({ + "sourcePosition": [a["lon"], a["lat"]], + "targetPosition": [b["lon"], b["lat"]], + "color": [red, green, 0] + }) + + # Latest car position (safe fallback to anchor) + if path_points: + car_lat, car_lon = path_points[-1]["lat"], path_points[-1]["lon"] + else: + car_lat, car_lon = ANCHOR_LAT, ANCHOR_LON + + # 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}) - # 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) + # ✅ 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 + ) - # fresh heatmap layer each rerun - m = folium.Map(location=[38.7169, -9.1390], zoom_start=15) + layers = [] + if segments: + layers.append(pdk.Layer( + "LineLayer", + data=segments, + get_source_position="sourcePosition", + get_target_position="targetPosition", + get_color="color", + get_width=3 + )) - 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 - ] + 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 heat_data: - HeatMap(heat_data).add_to(m) + 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 # ----------------- -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()) diff --git a/requirements.txt b/requirements.txt index 00963ea..8e7fa88 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,6 +1,7 @@ -pip~=21.2.4 -attrs~=25.3.0 -wheel~=0.37.0 -numpy~=2.0.2 -setuptools~=58.0.4 -eclipse-zenoh>=0.11.0 \ No newline at end of file +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