refactor code with orangepi-server

orangepi-server/orange_pi_visualizer.py

@@ -0,0 +1,457 @@
+#!/usr/bin/env python3
+"""
+ESP32-S3 LD2453 Radar Visualizer for Orange Pi
+===============================================
+- Kết nối WebSocket tới ESP32 để nhận dữ liệu radar realtime
+- Vẽ zone (3 vùng hình chữ nhật) và vị trí các target (up to 3)
+- Hiển thị tọa độ, tốc độ, khoảng cách của từng target
+- Hỗ trợ chạy headless (chỉ log) hoặc GUI (tkinter/pygame)
+
+Sử dụng:
+  python orange_pi_visualizer.py <ESP32_IP>              # GUI mode (tkinter)
+  python orange_pi_visualizer.py <ESP32_IP> --headless   # Chỉ in console
+  python orange_pi_visualizer.py <ESP32_IP> --mode pygame # Dùng pygame (đồ họa đẹp hơn)
+"""
+
+import argparse
+import asyncio
+import json
+import sys
+import time
+from collections import deque
+from dataclasses import dataclass, field
+from typing import List, Optional, Tuple
+
+# ============================================================
+# WebSocket client
+# ============================================================
+try:
+    import websockets
+except ImportError:
+    print("Loi: Can install websockets: pip install websockets")
+    sys.exit(1)
+
+
+@dataclass
+class Target:
+    slot: int
+    valid: bool = False
+    x_mm: int = 0
+    y_mm: int = 0
+    speed_cm_s: int = 0
+    distance_mm: float = 0.0
+    angle_deg: float = 0.0
+    last_seen: float = 0.0
+
+
+@dataclass
+class Zone:
+    id: int
+    x1: int = 0
+    y1: int = 0
+    x2: int = 0
+    y2: int = 0
+    occupied: bool = False
+
+
+@dataclass
+class RadarState:
+    node_id: str = "RADAR_A"
+    sequence: int = 0
+    timestamp_ms: int = 0
+    target_count: int = 0
+    targets: List[Target] = field(default_factory=lambda: [Target(i + 1) for i in range(3)])
+    zones: List[Zone] = field(default_factory=lambda: [Zone(i) for i in range(1, 4)])
+    frames_received: int = 0
+    last_update: float = 0.0
+
+    def update_from_frame(self, frame: dict):
+        self.node_id = frame.get("node_id", self.node_id)
+        self.sequence = frame.get("sequence", 0)
+        self.timestamp_ms = frame.get("timestamp", 0)
+        self.target_count = frame.get("target_count", 0)
+        now = time.time()
+
+        targets_data = frame.get("targets", [])
+        for t_data in targets_data:
+            slot = t_data.get("id", 0)
+            if 1 <= slot <= 3:
+                t = self.targets[slot - 1]
+                t.slot = slot
+                t.valid = t_data.get("valid", False)
+                t.x_mm = t_data.get("x", 0)
+                t.y_mm = t_data.get("y", 0)
+                t.speed_cm_s = t_data.get("speed", 0)
+                t.distance_mm = float(t_data.get("distance", 0))
+                t.angle_deg = float(t_data.get("angle", 0))
+                if t.valid:
+                    t.last_seen = now
+
+        self.frames_received += 1
+        self.last_update = now
+
+
+# ============================================================
+# GUI 1: Tkinter (mặc định, có sẵn trên hầu hết hệ thống)
+# ============================================================
+class TkinterVisualizer:
+    # Canvas size (pixel)
+    CANVAS_W = 900
+    CANVAS_H = 700
+
+    # Radar coordinate range (mm) — map vào canvas
+    ROOM_X_MIN = -5000  # 5m bên trái radar
+    ROOM_X_MAX = 5000   # 5m bên phải
+    ROOM_Y_MIN = 0      # radar
+    ROOM_Y_MAX = 7000   # 7m hướng về phía trước
+
+    # Colors
+    BG_COLOR = "#1e1e2e"
+    GRID_COLOR = "#313244"
+    AXIS_COLOR = "#89b4fa"
+    ZONE_COLORS = ["#a6e3a1", "#fab387", "#f38ba8", "#89dceb"]
+    ZONE_OCCUPIED_COLORS = ["#40a02b", "#fe640b", "#d20f39", "#04a5e5"]
+    TARGET_COLORS = ["#f9e2af", "#cba6f7", "#74c7ec"]
+
+    def __init__(self, state: RadarState):
+        self.state = state
+        try:
+            import tkinter as tk
+        except ImportError:
+            print("Loi: tkinter khong co san. Hay cai: sudo apt install python3-tk")
+            sys.exit(1)
+        self.tk = tk
+        self.root = tk.Tk()
+        self.root.title(f"ESP32 LD2453 Radar — {state.node_id}")
+        self.root.configure(bg=self.BG_COLOR)
+        self.root.protocol("WM_DELETE_WINDOW", self._on_close)
+        self._running = True
+
+        self._build_ui()
+
+    def _build_ui(self):
+        # Header label
+        self.header_var = self.tk.StringVar(value="Dang cho du lieu...")
+        lbl = self.tk.Label(
+            self.root, textvariable=self.header_var,
+            font=("Consolas", 12, "bold"),
+            fg="#cdd6f4", bg=self.BG_COLOR, pady=8
+        )
+        lbl.pack(fill="x")
+
+        # Canvas
+        self.canvas = self.tk.Canvas(
+            self.root, width=self.CANVAS_W, height=self.CANVAS_H,
+            bg=self.BG_COLOR, highlightthickness=0
+        )
+        self.canvas.pack(padx=10, pady=5)
+
+        # Status bar
+        self.status_var = self.tk.StringVar(value="Frames: 0")
+        self.tk.Label(
+            self.root, textvariable=self.status_var,
+            font=("Consolas", 10),
+            fg="#a6adc8", bg=self.BG_COLOR, pady=6
+        ).pack(fill="x")
+
+    def _mm_to_pixel(self, x_mm: int, y_mm: int) -> Tuple[float, float]:
+        """Map coordinate radar -> canvas pixel.
+        Radar goc toa do: radar o giua canh duoi. Tang Y la huong vao phia truoc.
+        """
+        x_ratio = (x_mm - self.ROOM_X_MIN) / (self.ROOM_X_MAX - self.ROOM_X_MIN)
+        # Dao Y: canvas (0,0) o goc tren, room Y=0 o goc duoi (radar)
+        y_ratio = 1.0 - (y_mm - self.ROOM_Y_MIN) / (self.ROOM_Y_MAX - self.ROOM_Y_MIN)
+        # De lai margin
+        margin_x = 60
+        margin_y = 60
+        px = margin_x + x_ratio * (self.CANVAS_W - 2 * margin_x)
+        py = margin_y + y_ratio * (self.CANVAS_H - 2 * margin_y)
+        return px, py
+
+    def _draw_background(self):
+        c = self.canvas
+        # Grid lines every 1m
+        for x_mm in range(self.ROOM_X_MIN, self.ROOM_X_MAX + 1, 1000):
+            x1, y1 = self._mm_to_pixel(x_mm, self.ROOM_Y_MIN)
+            x2, y2 = self._mm_to_pixel(x_mm, self.ROOM_Y_MAX)
+            c.create_line(x1, y1, x2, y2, fill=self.GRID_COLOR)
+            if x_mm != 0:
+                c.create_text(x1, self.CANVAS_H - 30, text=f"{x_mm//1000}m", fill="#6c7086", font=("Consolas", 8))
+        for y_mm in range(self.ROOM_Y_MIN, self.ROOM_Y_MAX + 1, 1000):
+            x1, y1 = self._mm_to_pixel(self.ROOM_X_MIN, y_mm)
+            x2, y2 = self._mm_to_pixel(self.ROOM_X_MAX, y_mm)
+            c.create_line(x1, y1, x2, y2, fill=self.GRID_COLOR)
+            c.create_text(25, y1, text=f"{y_mm//1000}m", fill="#6c7086", font=("Consolas", 8))
+
+        # Radar position (center bottom edge)
+        rx, ry = self._mm_to_pixel(0, 0)
+        c.create_text(rx, ry + 25, text="RADAR", fill="#f38ba8", font=("Consolas", 10, "bold"))
+        c.create_oval(rx - 8, ry - 8, rx + 8, ry + 8, fill="#f38ba8", outline="#f38ba8")
+
+        # Y axis line
+        ax, ay = self._mm_to_pixel(0, self.ROOM_Y_MAX)
+        c.create_line(rx, ry, ax, ay, fill=self.AXIS_COLOR, dash=(4, 2))
+
+    def _draw_zones(self, zones: List[Zone]):
+        c = self.canvas
+        for i, z in enumerate(zones):
+            x1, y1 = self._mm_to_pixel(z.x1, z.y1)
+            x2, y2 = self._mm_to_pixel(z.x2, z.y2)
+            # Tkinter can x1<=x2, y1<=y2
+            px1, px2 = sorted([x1, x2])
+            py1, py2 = sorted([y1, y2])
+            color = self.ZONE_OCCUPIED_COLORS[i] if z.occupied else self.ZONE_COLORS[i]
+            stipple = "gray25" if not z.occupied else ""
+            c.create_rectangle(
+                px1, py1, px2, py2,
+                outline=color, width=2, fill=color, stipple=stipple
+            )
+            # Zone label
+            lx = (px1 + px2) / 2
+            ly = (py1 + py2) / 2
+            status = "CO" if z.occupied else "TRONG"
+            c.create_text(
+                lx, ly,
+                text=f"Zone{z.id}\n{status}",
+                fill="#1e1e2e" if z.occupied else "#45475a",
+                font=("Consolas", 10, "bold")
+            )
+
+    def _draw_targets(self, targets: List[Target]):
+        c = self.canvas
+        now = time.time()
+        for i, t in enumerate(targets):
+            if not t.valid:
+                # Show faded marker if recently seen
+                if now - t.last_seen < 3.0:
+                    px, py = self._mm_to_pixel(t.x_mm, t.y_mm)
+                    c.create_oval(px - 5, py - 5, px + 5, py + 5, outline="#585b70", dash=(2, 2))
+                continue
+
+            px, py = self._mm_to_pixel(t.x_mm, t.y_mm)
+            color = self.TARGET_COLORS[i]
+
+            # Target dot
+            r = 8
+            c.create_oval(px - r, py - r, px + r, py + r, fill=color, outline="white", width=2)
+
+            # Velocity vector (speed)
+            if t.speed_cm_s != 0:
+                scale = 2.0  # pixel per cm/s
+                dx = (t.x_mm / max(1, t.distance_mm)) * t.speed_cm_s * scale if t.distance_mm else 0
+                dy = (t.y_mm / max(1, t.distance_mm)) * t.speed_cm_s * scale if t.distance_mm else 0
+                c.create_line(px, py, px + dx, py - dy, fill=color, width=2, arrow=self.tk.LAST)
+
+            # Label
+            label = f"T{t.slot}\n{abs(t.x_mm)}, {t.y_mm}mm\n{abs(t.speed_cm_s)}cm/s"
+            c.create_text(
+                px + 15, py - 25, anchor="w",
+                text=label, fill="white",
+                font=("Consolas", 9, "bold"),
+                tags=f"tlabel{t.slot}"
+            )
+
+    def draw(self):
+        self.canvas.delete("all")
+        self._draw_background()
+        self._draw_zones(self.state.zones)
+        self._draw_targets(self.state.targets)
+
+        s = self.state
+        self.header_var.set(
+            f"Node: {s.node_id}  |  Sequence: {s.sequence}  |  Targets: {s.target_count}/3"
+        )
+        self.status_var.set(
+            f"Frames received: {s.frames_received}   |   "
+            f"Last update: {time.time() - s.last_update:.1f}s ago"
+        )
+
+    def _on_close(self):
+        self._running = False
+        self.root.destroy()
+
+    def should_run(self) -> bool:
+        return self._running
+
+    def update(self):
+        """Call periodically from main loop."""
+        if self._running:
+            self.draw()
+            self.root.update_idletasks()
+            self.root.update()
+
+
+# ============================================================
+# Console Headless Mode
+# ============================================================
+class ConsoleVisualizer:
+    def __init__(self, state: RadarState):
+        self.state = state
+        self._last_print = 0.0
+
+    def update(self):
+        now = time.time()
+        if now - self._last_print < 0.5:
+            return
+        self._last_print = now
+
+        s = self.state
+        print("\033[2J\033[H", end="")  # Clear screen
+        print("=" * 60)
+        print(f"  ESP32 LD2453 — Node: {s.node_id}   Seq: {s.sequence}")
+        print("=" * 60)
+        print(f"  Frames received: {s.frames_received}")
+        print(f"  Target count    : {s.target_count}/3")
+        if s.last_update:
+            print(f"  Last update     : {now - s.last_update:.1f}s ago")
+        print("-" * 60)
+        for t in s.targets:
+            if t.valid:
+                status = "DETECTED"
+            elif now - t.last_seen < 3.0:
+                status = f"RECENT ({now-t.last_seen:.0f}s)"
+            else:
+                status = "FREE"
+            print(f"  Slot {t.slot}: {status}")
+            if t.valid:
+                print(f"    X={t.x_mm:5d}mm  Y={t.y_mm:5d}mm  "
+                      f"Speed={t.speed_cm_s:+3d}cm/s  "
+                      f"Dist={int(t.distance_mm):5d}mm  "
+                      f"Angle={t.angle_deg:+.1f}°")
+        print("-" * 60)
+        for z in s.zones:
+            occ = "X CO" if z.occupied else "  TRONG"
+            print(f"  Zone {z.id}: {occ}  [{z.x1:+5d},{z.y1:+5d}] -> [{z.x2:+5d},{z.y2:+5d}]")
+        print("=" * 60)
+
+    def should_run(self) -> bool:
+        return True
+
+    def draw(self):
+        pass
+
+
+# ============================================================
+# Fetch zones from ESP32 REST API
+# ============================================================
+async def fetch_zones(state: RadarState, esp32_host: str):
+    try:
+        import aiohttp
+    except ImportError:
+        print("Canh bao: aiohttp chua cai. Zone se dung default. (pip install aiohttp)")
+        return
+
+    url = f"http://{esp32_host}/zones"
+    try:
+        async with aiohttp.ClientSession(timeout=aiohttp.ClientTimeout(total=3)) as session:
+            async with session.get(url) as resp:
+                if resp.status == 200:
+                    data = await resp.json()
+                    for z_data in data:
+                        idx = z_data.get("id", 0)
+                        if 1 <= idx <= 3:
+                            z = state.zones[idx - 1]
+                            z.id = idx
+                            z.x1 = int(z_data.get("x1", z.x1))
+                            z.y1 = int(z_data.get("y1", z.y1))
+                            z.x2 = int(z_data.get("x2", z.x2))
+                            z.y2 = int(z_data.get("y2", z.y2))
+                            z.occupied = bool(z_data.get("occupied", False))
+                    print(f"Da lay {len(data)} zone tu ESP32")
+                else:
+                    print(f"GET zones tra ve status {resp.status}")
+    except Exception as e:
+        print(f"Khong the lay zones: {e}")
+
+
+# ============================================================
+# Update zone occupancy from target positions
+# (redundant — ESP32 already reports zones, but kept for validation)
+# ============================================================
+def update_zone_occupancy_from_targets(state: RadarState):
+    for z in state.zones:
+        z.occupied = False
+        for t in state.targets:
+            if not t.valid:
+                continue
+            if z.x1 <= t.x_mm <= z.x2 and z.y1 <= t.y_mm <= z.y2:
+                z.occupied = True
+                break
+
+
+# ============================================================
+# Main async loop
+# ============================================================
+async def ws_client_loop(esp32_host: str, visualizer, state: RadarState):
+    uri = f"ws://{esp32_host}/ws"
+    reconnect_delay = 2.0
+
+    while visualizer.should_run():
+        print(f"Dang ket noi den {uri}...")
+        try:
+            async with websockets.connect(uri, ping_interval=30, ping_timeout=10) as ws:
+                print(f"Da ket noi WebSocket den ESP32 ({esp32_host})")
+                # Fetch zones one time after connect
+                await fetch_zones(state, esp32_host)
+
+                async for message in ws:
+                    if not visualizer.should_run():
+                        break
+                    try:
+                        frame = json.loads(message)
+                        state.update_from_frame(frame)
+                        update_zone_occupancy_from_targets(state)
+                        visualizer.update()
+                    except json.JSONDecodeError:
+                        print(f"Loi parse JSON: {message[:80]}...")
+                        continue
+                # Graceful disconnect
+                print("WebSocket dong. Dang ket noi lai...")
+        except websockets.exceptions.ConnectionClosed as e:
+            print(f"Mat ket noi WebSocket ({e}), dang thu lai sau {reconnect_delay}s...")
+            await asyncio.sleep(reconnect_delay)
+        except OSError as e:
+            print(f"Khong the ket noi: {e}. Thu lai sau {reconnect_delay}s...")
+            await asyncio.sleep(reconnect_delay)
+        except KeyboardInterrupt:
+            print("Dung lai...")
+            break
+        except Exception as e:
+            print(f"Loi WebSocket: {type(e).__name__}: {e}")
+            await asyncio.sleep(reconnect_delay)
+
+
+def main():
+    parser = argparse.ArgumentParser(description="ESP32 LD2453 Radar Visualizer")
+    parser.add_argument("host", help="ESP32 IP address (e.g. 192.168.1.100)")
+    parser.add_argument("--headless", action="store_true", help="Chi in ra console, khong can GUI")
+    parser.add_argument("--mode", choices=["tkinter", "pygame"], default="tkinter",
+                        help="Loaị GUI (mac dinh: tkinter)")
+    args = parser.parse_args()
+
+    state = RadarState()
+    # Default zones (giong ESP32)
+    state.zones[0] = Zone(1, x1=-4000, y1=1, x2=-1, y2=4000)
+    state.zones[1] = Zone(2, x1=1, y1=1, x2=4000, y2=4000)
+    state.zones[2] = Zone(3, x1=-4001, y1=4001, x2=4001, y2=6000)
+
+    if args.headless:
+        vis = ConsoleVisualizer(state)
+    elif args.mode == "pygame":
+        print("Pygame mode chua implement, chuyen sang tkinter...")
+        vis = TkinterVisualizer(state)
+    else:
+        vis = TkinterVisualizer(state)
+
+    print(f"ESP32-S3 LD2453 Radar Visualizer")
+    print(f"ESP32 host: {args.host}")
+    print(f"Mode: {'headless console' if args.headless else 'tkinter GUI'}")
+
+    try:
+        asyncio.run(ws_client_loop(args.host, vis, state))
+    except KeyboardInterrupt:
+        print("\nThoat.")
+
+
+if __name__ == "__main__":
+    main()