Files
gps/soc/software/fusion/map_player.py
T

527 lines
21 KiB
Python
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
#!/usr/bin/env python3
"""Native Tkinter OpenStreetMap player for raw GPS and ESKF positions."""
from __future__ import annotations
import argparse
import csv
import io
import math
import queue
import threading
import urllib.request
from concurrent.futures import ThreadPoolExecutor
from dataclasses import dataclass
from pathlib import Path
import numpy as np
import tkinter as tk
from PIL import Image, ImageDraw, ImageTk
from tkinter import ttk
from fusion_eskf import enu_to_geodetic
BASE_DIR = Path(__file__).resolve().parent
TILE_SIZE = 256
MAX_MERCATOR_LAT = 85.05112878
@dataclass
class MapSample:
t: float
raw_lat: float
raw_lon: float
raw_height: float
fused_lat: float
fused_lon: float
fused_height: float
accepted: bool
mahalanobis: float
separation_m: float
def load_samples(gps_path: Path, eskf_path: Path) -> list[MapSample]:
with gps_path.open(newline="", encoding="utf-8") as stream:
gps = list(csv.DictReader(stream))
with eskf_path.open(newline="", encoding="utf-8") as stream:
eskf = list(csv.DictReader(stream))
if not gps or len(gps) != len(eskf):
raise ValueError(
f"GPS/ESKF row mismatch: gps={len(gps)}, eskf={len(eskf)}"
)
reference = (
float(np.median([float(row["lat_deg"]) for row in gps])),
float(np.median([float(row["lon_deg"]) for row in gps])),
float(np.median([float(row["height_m"]) for row in gps])),
)
fused_enu = np.array(
[
[float(row["eskf_e_m"]), float(row["eskf_n_m"]), float(row["eskf_u_m"])]
for row in eskf
]
)
fused_lla = enu_to_geodetic(fused_enu, reference)
result = []
for raw, filtered, corrected in zip(gps, eskf, fused_lla):
result.append(
MapSample(
t=float(filtered["t_s"]),
raw_lat=float(raw["lat_deg"]),
raw_lon=float(raw["lon_deg"]),
raw_height=float(raw["height_m"]),
fused_lat=float(corrected[0]),
fused_lon=float(corrected[1]),
fused_height=float(corrected[2]),
accepted=bool(int(filtered["gps_accepted"])),
mahalanobis=float(filtered["mahalanobis2"]),
separation_m=math.hypot(
float(filtered["gps_e_m"]) - float(filtered["eskf_e_m"]),
float(filtered["gps_n_m"]) - float(filtered["eskf_n_m"]),
),
)
)
return result
def load_comparison_samples(path: Path, model: str) -> list[MapSample]:
"""Load fixed/adaptive model_comparison.csv without an intermediate conversion."""
with path.open(newline="", encoding="utf-8") as stream:
rows = list(csv.DictReader(stream))
if not rows:
raise ValueError(f"empty model-comparison CSV: {path}")
prefix = "fixed" if model == "fixed" else "adaptive"
required = {
"t_s", "lat_deg", "lon_deg", "gps_e_m", "gps_n_m",
f"{prefix}_e_m", f"{prefix}_n_m", f"{prefix}_gps_accepted",
f"{prefix}_nis",
}
missing = required.difference(rows[0])
if missing:
raise ValueError(f"comparison CSV missing columns: {sorted(missing)}")
heights = np.array([float(row.get("height_m") or 0.0) for row in rows])
reference = (
float(np.median([float(row["lat_deg"]) for row in rows])),
float(np.median([float(row["lon_deg"]) for row in rows])),
float(np.median(heights)),
)
fused_enu = np.array(
[
[float(row[f"{prefix}_e_m"]), float(row[f"{prefix}_n_m"]), 0.0]
for row in rows
]
)
fused_lla = enu_to_geodetic(fused_enu, reference)
result = []
for row, corrected, raw_height in zip(rows, fused_lla, heights):
result.append(
MapSample(
t=float(row["t_s"]),
raw_lat=float(row["lat_deg"]),
raw_lon=float(row["lon_deg"]),
raw_height=float(raw_height),
fused_lat=float(corrected[0]),
fused_lon=float(corrected[1]),
fused_height=float(corrected[2]),
accepted=bool(int(row[f"{prefix}_gps_accepted"])),
mahalanobis=float(row[f"{prefix}_nis"]),
separation_m=math.hypot(
float(row["gps_e_m"]) - float(row[f"{prefix}_e_m"]),
float(row["gps_n_m"]) - float(row[f"{prefix}_n_m"]),
),
)
)
return result
def latlon_to_world(lat: float, lon: float, zoom: int) -> tuple[float, float]:
lat = max(-MAX_MERCATOR_LAT, min(MAX_MERCATOR_LAT, lat))
scale = TILE_SIZE * (2**zoom)
x = (lon + 180.0) / 360.0 * scale
lat_rad = math.radians(lat)
y = (1.0 - math.asinh(math.tan(lat_rad)) / math.pi) / 2.0 * scale
return x, y
def world_to_latlon(x: float, y: float, zoom: int) -> tuple[float, float]:
scale = TILE_SIZE * (2**zoom)
lon = x / scale * 360.0 - 180.0
mercator = math.pi * (1.0 - 2.0 * y / scale)
lat = math.degrees(math.atan(math.sinh(mercator)))
return lat, lon
class OsmTileStore:
def __init__(self, root: tk.Tk, cache_dir: Path, redraw):
self.root = root
self.cache_dir = cache_dir
self.redraw = redraw
self.cache_dir.mkdir(parents=True, exist_ok=True)
self.executor = ThreadPoolExecutor(max_workers=3, thread_name_prefix="osm")
self.pending: set[tuple[int, int, int]] = set()
self.completed: queue.Queue[tuple[tuple[int, int, int], str | None]] = queue.Queue()
self.closed = False
self.root.after(100, self._poll)
def path(self, z: int, x: int, y: int) -> Path:
return self.cache_dir / str(z) / str(x) / f"{y}.png"
def request(self, z: int, x: int, y: int):
key = (z, x, y)
if key in self.pending or self.path(*key).exists() or self.closed:
return
self.pending.add(key)
self.executor.submit(self._download, key)
def _download(self, key):
z, x, y = key
path = self.path(z, x, y)
error = None
try:
request = urllib.request.Request(
f"https://tile.openstreetmap.org/{z}/{x}/{y}.png",
headers={"User-Agent": "Rocket-GPS-Fusion/1.0 (local engineering viewer)"},
)
with urllib.request.urlopen(request, timeout=12) as response:
content = response.read()
image = Image.open(io.BytesIO(content))
image.verify()
path.parent.mkdir(parents=True, exist_ok=True)
path.write_bytes(content)
except Exception as exc: # network/cache failure is shown in the window
error = str(exc)
self.completed.put((key, error))
def _poll(self):
changed = False
errors = []
while True:
try:
key, error = self.completed.get_nowait()
except queue.Empty:
break
self.pending.discard(key)
changed = True
if error:
errors.append(error)
if changed:
self.redraw(errors[-1] if errors else None)
if not self.closed:
self.root.after(100, self._poll)
def close(self):
self.closed = True
self.executor.shutdown(wait=False, cancel_futures=True)
class GpsMapPlayer:
def __init__(self, root: tk.Tk, samples: list[MapSample], cache_dir: Path):
self.root = root
self.samples = samples
self.index = 0
self.play_job = None
self.zoom = 19
self.center_lat = float(np.median([sample.raw_lat for sample in samples]))
self.center_lon = float(np.median([sample.raw_lon for sample in samples]))
self.drag_start = None
self.internal_slider = False
self.map_photo = None
self.tile_error = None
self.resize_job = None
root.title("GPS / IMU ESKF — OpenStreetMap sample oynatıcı")
root.geometry("1180x780")
root.minsize(760, 520)
controls = ttk.Frame(root, padding=(8, 7))
controls.pack(side=tk.TOP, fill=tk.X)
self.play_button = ttk.Button(controls, text="▶ Oynat", command=self.toggle_play)
self.play_button.pack(side=tk.LEFT)
ttk.Button(controls, text="↺ Başa al", command=self.reset).pack(side=tk.LEFT, padx=(5, 0))
ttk.Button(controls, text="İzleri sığdır", command=self.fit_tracks).pack(side=tk.LEFT, padx=(5, 0))
ttk.Button(controls, text="Konuma git", command=self.center_current).pack(side=tk.LEFT, padx=(5, 0))
ttk.Button(controls, text="Haritayı yenile", command=self.draw).pack(side=tk.LEFT, padx=(5, 0))
ttk.Button(controls, text="−", width=3, command=lambda: self.change_zoom(-1)).pack(side=tk.LEFT, padx=(14, 0))
ttk.Button(controls, text="+", width=3, command=lambda: self.change_zoom(1)).pack(side=tk.LEFT)
ttk.Label(controls, text="Hız:").pack(side=tk.LEFT, padx=(14, 4))
self.speed = tk.StringVar(value="1")
ttk.Combobox(controls, textvariable=self.speed, values=("0.5", "1", "2", "4"), width=4, state="readonly").pack(side=tk.LEFT)
self.tick_label = ttk.Label(controls, text="")
self.tick_label.pack(side=tk.RIGHT)
timeline = ttk.Frame(root, padding=(8, 0, 8, 6))
timeline.pack(side=tk.TOP, fill=tk.X)
self.slider = tk.Scale(
timeline, from_=0, to=len(samples) - 1, resolution=1, orient=tk.HORIZONTAL,
showvalue=False, command=self.on_slider, highlightthickness=0,
)
self.slider.pack(side=tk.LEFT, fill=tk.X, expand=True)
self.time_label = ttk.Label(timeline, width=11, anchor=tk.E)
self.time_label.pack(side=tk.RIGHT)
self.canvas = tk.Canvas(root, bg="#d9dde0", highlightthickness=0)
self.canvas.pack(side=tk.TOP, fill=tk.BOTH, expand=True)
self.info = ttk.Label(root, padding=(9, 6), anchor=tk.W)
self.info.pack(side=tk.BOTTOM, fill=tk.X)
self.canvas.bind("<ButtonPress-1>", self.pan_start)
self.canvas.bind("<B1-Motion>", self.pan_move)
self.canvas.bind("<MouseWheel>", self.mouse_zoom)
self.canvas.bind("<Button-4>", lambda event: self.change_zoom(1))
self.canvas.bind("<Button-5>", lambda event: self.change_zoom(-1))
self.canvas.bind("<Configure>", self.on_resize)
root.bind("<space>", lambda event: self.toggle_play())
root.protocol("WM_DELETE_WINDOW", self.close)
self.tiles = OsmTileStore(root, cache_dir, self.tile_update)
root.after(150, self.fit_tracks)
def tile_update(self, error=None):
if error:
self.tile_error = error
self.draw()
def project(self, lat, lon):
world_x, world_y = latlon_to_world(lat, lon, self.zoom)
center_x, center_y = latlon_to_world(self.center_lat, self.center_lon, self.zoom)
return (
self.canvas.winfo_width() / 2 + world_x - center_x,
self.canvas.winfo_height() / 2 + world_y - center_y,
)
def draw(self):
if not self.canvas.winfo_exists():
return
self.canvas.delete("all")
width = max(1, self.canvas.winfo_width())
height = max(1, self.canvas.winfo_height())
map_image = Image.new("RGB", (width, height), "#d9dde0")
image_draw = ImageDraw.Draw(map_image)
center_x, center_y = latlon_to_world(self.center_lat, self.center_lon, self.zoom)
left, top = center_x - width / 2, center_y - height / 2
count = 2**self.zoom
tx0, tx1 = math.floor(left / TILE_SIZE), math.floor((left + width) / TILE_SIZE)
ty0, ty1 = math.floor(top / TILE_SIZE), math.floor((top + height) / TILE_SIZE)
loaded_tiles = 0
visible_tiles = 0
for tile_y in range(ty0, ty1 + 1):
if tile_y < 0 or tile_y >= count:
continue
for tile_x_unwrapped in range(tx0, tx1 + 1):
visible_tiles += 1
tile_x = tile_x_unwrapped % count
px = round(tile_x_unwrapped * TILE_SIZE - left)
py = round(tile_y * TILE_SIZE - top)
path = self.tiles.path(self.zoom, tile_x, tile_y)
if path.exists():
try:
with Image.open(path) as tile:
map_image.paste(tile.convert("RGB"), (px, py))
loaded_tiles += 1
except Exception:
path.unlink(missing_ok=True)
self.tiles.request(self.zoom, tile_x, tile_y)
else:
image_draw.rectangle((px, py, px + TILE_SIZE, py + TILE_SIZE), fill="#e3e6e8", outline="#cdd1d4")
self.tiles.request(self.zoom, tile_x, tile_y)
self.map_photo = ImageTk.PhotoImage(map_image)
self.canvas.create_image(0, 0, image=self.map_photo, anchor=tk.NW)
shown = self.samples[: self.index + 1]
self.draw_track([(s.raw_lat, s.raw_lon) for s in shown], "#d62728", 2)
self.draw_track([(s.fused_lat, s.fused_lon) for s in shown], "#1769aa", 3)
for sample in shown:
x, y = self.project(sample.raw_lat, sample.raw_lon)
color = "#d62728" if sample.accepted else "#e67e22"
self.canvas.create_oval(x - 2, y - 2, x + 2, y + 2, fill=color, outline="")
x, y = self.project(sample.fused_lat, sample.fused_lon)
self.canvas.create_oval(x - 2, y - 2, x + 2, y + 2, fill="#1769aa", outline="")
current = self.samples[self.index]
self.draw_marker(current.raw_lat, current.raw_lon, "#d62728" if current.accepted else "#e67e22", 8)
self.draw_marker(current.fused_lat, current.fused_lon, "#2196f3", 7)
self.canvas.create_rectangle(10, 10, 205, 77, fill="white", outline="#888")
self.canvas.create_oval(22, 23, 32, 33, fill="#d62728", outline="")
self.canvas.create_text(40, 28, text="Ham GPS", anchor=tk.W)
self.canvas.create_oval(22, 43, 32, 53, fill="#2196f3", outline="")
self.canvas.create_text(40, 48, text="ESKF + ZUPT", anchor=tk.W)
self.canvas.create_oval(22, 61, 32, 71, fill="#e67e22", outline="")
self.canvas.create_text(40, 66, text="Reddedilen GPS", anchor=tk.W)
self.canvas.create_text(width - 8, height - 7, text="© OpenStreetMap contributors", anchor=tk.SE, fill="#333")
status = f"OSM {loaded_tiles}/{visible_tiles} tile"
if self.tile_error and loaded_tiles == 0:
status += f" — bağlantı hatası: {self.tile_error}"
elif loaded_tiles < visible_tiles:
status += " yükleniyor…"
self.canvas.create_rectangle(width - 245, 8, width - 8, 32, fill="white", outline="#999")
self.canvas.create_text(width - 16, 20, text=status, anchor=tk.E, fill="#333")
self.update_labels()
def draw_track(self, coordinates, color, width):
if len(coordinates) > 1:
pixels = [value for coordinate in coordinates for value in self.project(*coordinate)]
self.canvas.create_line(*pixels, fill=color, width=width, smooth=False)
def draw_marker(self, lat, lon, color, radius):
x, y = self.project(lat, lon)
self.canvas.create_oval(x - radius, y - radius, x + radius, y + radius, fill=color, outline="white", width=2)
def update_labels(self):
sample = self.samples[self.index]
self.tick_label.configure(text=f"Sample {self.index + 1} / {len(self.samples)}")
self.time_label.configure(text=f"{sample.t:.3f} s")
gate = "KABUL" if sample.accepted else "RED"
self.info.configure(
text=(
f"GPS {sample.raw_lat:.8f}, {sample.raw_lon:.8f}, h={sample.raw_height:.1f} m | "
f"ESKF {sample.fused_lat:.8f}, {sample.fused_lon:.8f}, h={sample.fused_height:.1f} m | "
f"yatay fark={sample.separation_m:.2f} m GPS={gate} NIS={sample.mahalanobis:.2f}"
)
)
def on_slider(self, value):
if self.internal_slider:
return
requested_index = int(float(value))
# Scale.set() also invokes this callback on some Tk builds. An unchanged
# value is therefore a programmatic playback update, not user scrubbing.
if requested_index == self.index:
return
self.stop()
self.index = requested_index
self.draw()
def set_index(self, index):
self.index = max(0, min(len(self.samples) - 1, index))
self.internal_slider = True
self.slider.set(self.index)
self.internal_slider = False
self.draw()
def toggle_play(self):
if self.play_job is not None:
self.stop()
return
if self.index >= len(self.samples) - 1:
self.set_index(0)
self.play_button.configure(text="⏸ Duraklat")
self.schedule_next()
def schedule_next(self):
if self.index >= len(self.samples) - 1:
self.stop()
return
dt = self.samples[self.index + 1].t - self.samples[self.index].t
delay = max(60, min(2000, int(dt * 1000 / float(self.speed.get()))))
self.play_job = self.root.after(delay, self.play_step)
def play_step(self):
self.play_job = None
self.set_index(self.index + 1)
self.schedule_next()
def stop(self):
if self.play_job is not None:
self.root.after_cancel(self.play_job)
self.play_job = None
self.play_button.configure(text="▶ Oynat")
def reset(self):
self.stop()
self.set_index(0)
def center_current(self):
sample = self.samples[self.index]
self.center_lat = (sample.raw_lat + sample.fused_lat) / 2.0
self.center_lon = (sample.raw_lon + sample.fused_lon) / 2.0
self.zoom = 20
self.draw()
def fit_tracks(self):
width = max(400, self.canvas.winfo_width())
height = max(300, self.canvas.winfo_height())
coordinates = [(s.raw_lat, s.raw_lon) for s in self.samples]
coordinates += [(s.fused_lat, s.fused_lon) for s in self.samples]
self.center_lat = (min(x[0] for x in coordinates) + max(x[0] for x in coordinates)) / 2
self.center_lon = (min(x[1] for x in coordinates) + max(x[1] for x in coordinates)) / 2
for zoom in range(22, 1, -1):
pixels = [latlon_to_world(lat, lon, zoom) for lat, lon in coordinates]
span_x = max(x for x, _ in pixels) - min(x for x, _ in pixels)
span_y = max(y for _, y in pixels) - min(y for _, y in pixels)
if span_x <= width * 0.75 and span_y <= height * 0.75:
self.zoom = zoom
break
self.draw()
def change_zoom(self, delta):
self.zoom = max(2, min(22, self.zoom + delta))
self.draw()
def mouse_zoom(self, event):
self.change_zoom(1 if event.delta > 0 else -1)
def pan_start(self, event):
self.drag_start = (event.x, event.y, *latlon_to_world(self.center_lat, self.center_lon, self.zoom))
def pan_move(self, event):
if self.drag_start is None:
return
start_x, start_y, center_x, center_y = self.drag_start
self.center_lat, self.center_lon = world_to_latlon(
center_x - (event.x - start_x), center_y - (event.y - start_y), self.zoom
)
self.draw()
def on_resize(self, _event):
if self.resize_job is not None:
self.root.after_cancel(self.resize_job)
self.resize_job = self.root.after(80, self.draw)
def close(self):
self.stop()
self.tiles.close()
self.root.destroy()
def parse_args():
parser = argparse.ArgumentParser()
parser.add_argument(
"--comparison", type=Path,
help="fixed/adaptive model_comparison.csv (overrides --gps/--eskf)",
)
parser.add_argument(
"--model", choices=("fixed", "adaptive"), default="adaptive",
help="model shown with --comparison (default: adaptive)",
)
parser.add_argument("--gps", type=Path, default=BASE_DIR / "output/gps_positions.csv")
parser.add_argument("--eskf", type=Path, default=BASE_DIR / "output/eskf_positions.csv")
parser.add_argument("--tile-cache", type=Path, default=BASE_DIR / "output/osm_tile_cache")
parser.add_argument("--check", action="store_true", help="validate inputs and exit without opening a window")
return parser.parse_args()
def main():
args = parse_args()
samples = (
load_comparison_samples(args.comparison, args.model)
if args.comparison
else load_samples(args.gps, args.eskf)
)
if args.check:
print(
f"map data OK: samples={len(samples)} duration={samples[-1].t:.3f}s "
f"accepted={sum(s.accepted for s in samples)} rejected={sum(not s.accepted for s in samples)}"
)
return
root = tk.Tk()
GpsMapPlayer(root, samples, args.tile_cache)
root.mainloop()
if __name__ == "__main__":
main()