feat(server): ingest temps réel WS + GUI live + client PC
Serveur FastAPI reçoit le flux JSONL (sim ou ROV réel) sur /ws/ingest, SLAM incrémental, rediffuse carte+poses sur /ws/live, GUI live et export PLY. Déployé Docker sur caddy-net, exposé /moulin-live/. Client PC stream_client.py. Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
This commit is contained in:
Flag
8
server/Dockerfile
Normal file
8
server/Dockerfile
Normal file
@@ -0,0 +1,8 @@
|
||||
FROM python:3.11-slim
|
||||
WORKDIR /app
|
||||
COPY requirements.txt .
|
||||
RUN pip install --no-cache-dir -r requirements.txt "numpy<2.0"
|
||||
COPY . .
|
||||
ENV MOULIN_TOKEN=moulin-2026
|
||||
EXPOSE 8211
|
||||
CMD ["uvicorn","app:app","--host","0.0.0.0","--port","8211"]
|
||||
66
server/README.md
Normal file
66
server/README.md
Normal file
@@ -0,0 +1,66 @@
|
||||
# moulin-mapper server
|
||||
|
||||
## Lancement local
|
||||
|
||||
```bash
|
||||
cd server/
|
||||
python -m venv .venv
|
||||
source .venv/bin/activate
|
||||
pip install -r requirements.txt
|
||||
|
||||
uvicorn app:app --host 0.0.0.0 --port 8211
|
||||
```
|
||||
|
||||
GUI accessible sur : http://localhost:8211/
|
||||
|
||||
## Variables d'environnement
|
||||
|
||||
| Variable | Défaut | Description |
|
||||
|----------|--------|-------------|
|
||||
| `MOULIN_TOKEN` | `moulin-2026` | Token authentification ingest WS |
|
||||
|
||||
## Endpoints
|
||||
|
||||
| Endpoint | Description |
|
||||
|----------|-------------|
|
||||
| `GET /` | GUI live (HTML statique) |
|
||||
| `WS /ws/ingest?token=…` | Reçoit le flux JSONL du client ROV/sim |
|
||||
| `WS /ws/live` | Navigateur s'abonne aux mises à jour temps réel |
|
||||
| `POST /session/reset` | Remet la session à zéro (form: token) |
|
||||
| `GET /healthz` | Santé + compteurs |
|
||||
| `GET /cloud.ply` | Export nuage 3D courant |
|
||||
|
||||
## Déploiement derrière Caddy (préfixe `/moulin-live/`)
|
||||
|
||||
Exemple Caddyfile :
|
||||
```
|
||||
handle /moulin-live/* {
|
||||
uri strip_prefix /moulin-live
|
||||
reverse_proxy localhost:8211
|
||||
}
|
||||
```
|
||||
|
||||
**Important** : la GUI construit les URLs WS depuis `window.location.pathname`,
|
||||
donc le préfixe de déploiement est automatiquement inclus.
|
||||
Ne PAS utiliser d'URLs en dur dans le code client.
|
||||
|
||||
## Lancer le client stream (sur le PC de Flag)
|
||||
|
||||
```bash
|
||||
cd client/
|
||||
pip install -r requirements.txt
|
||||
|
||||
# Simulation rapide (test)
|
||||
python stream_client.py \
|
||||
--file ../../data/sim/run_L.jsonl \
|
||||
--url wss://laboratoire.freeboxos.fr/moulin-live/ws/ingest \
|
||||
--token moulin-2026 \
|
||||
--speed 0
|
||||
|
||||
# Temps réel
|
||||
python stream_client.py \
|
||||
--file ../../data/sim/run_L.jsonl \
|
||||
--url wss://laboratoire.freeboxos.fr/moulin-live/ws/ingest \
|
||||
--token moulin-2026 \
|
||||
--speed 1.0
|
||||
```
|
||||
228
server/app.py
Normal file
228
server/app.py
Normal file
@@ -0,0 +1,228 @@
|
||||
"""
|
||||
app.py — Serveur FastAPI moulin-mapper.
|
||||
|
||||
Lance avec : uvicorn app:app --host 0.0.0.0 --port 8211
|
||||
Déployé derrière préfixe /moulin-live/ via reverse-proxy Caddy.
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import json
|
||||
import os
|
||||
import time
|
||||
from typing import Set
|
||||
|
||||
import numpy as np
|
||||
from fastapi import FastAPI, WebSocket, WebSocketDisconnect, HTTPException, Form, Request
|
||||
from fastapi.responses import HTMLResponse, PlainTextResponse, JSONResponse
|
||||
from fastapi.staticfiles import StaticFiles
|
||||
|
||||
from slam_incremental import IncrementalSLAM
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Config
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
TOKEN = os.environ.get("MOULIN_TOKEN", "moulin-2026")
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# État global de session
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
slam = IncrementalSLAM()
|
||||
session_records: list = [] # tous les enregistrements bruts reçus
|
||||
session_start_time: float = time.time()
|
||||
debit_window: list = [] # timestamps récents pour calcul débit
|
||||
|
||||
# Clients /ws/live connectés
|
||||
live_clients: Set[WebSocket] = set()
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# App FastAPI (root_path vide — le préfixe est géré par Caddy côté client)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
app = FastAPI(title="moulin-mapper live")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Broadcast aux clients /ws/live
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
async def broadcast(payload: dict) -> None:
|
||||
if not live_clients:
|
||||
return
|
||||
msg = json.dumps(payload)
|
||||
dead = set()
|
||||
for ws in list(live_clients):
|
||||
try:
|
||||
await ws.send_text(msg)
|
||||
except Exception:
|
||||
dead.add(ws)
|
||||
live_clients.difference_update(dead)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# WS /ws/ingest — reçoit le flux du client ROV/sim
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@app.websocket("/ws/ingest")
|
||||
async def ws_ingest(websocket: WebSocket, token: str = ""):
|
||||
if token != TOKEN:
|
||||
await websocket.close(code=4001)
|
||||
return
|
||||
|
||||
await websocket.accept()
|
||||
try:
|
||||
while True:
|
||||
raw = await websocket.receive_text()
|
||||
try:
|
||||
data = json.loads(raw)
|
||||
except json.JSONDecodeError:
|
||||
continue
|
||||
|
||||
# Accepte un seul record ou une liste
|
||||
records = data if isinstance(data, list) else [data]
|
||||
|
||||
sweep_completed = False
|
||||
for rec in records:
|
||||
session_records.append(rec)
|
||||
debit_window.append(time.time())
|
||||
# Nettoie la fenêtre (5 secondes)
|
||||
cutoff = time.time() - 5.0
|
||||
while debit_window and debit_window[0] < cutoff:
|
||||
debit_window.pop(0)
|
||||
|
||||
completed = slam.add_record(rec)
|
||||
if completed:
|
||||
sweep_completed = True
|
||||
|
||||
if sweep_completed:
|
||||
delta = slam.get_sweep_delta()
|
||||
delta["debit"] = len(debit_window) / 5.0
|
||||
await broadcast(delta)
|
||||
|
||||
except WebSocketDisconnect:
|
||||
pass
|
||||
except Exception as e:
|
||||
print(f"[ingest] erreur: {e}")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# WS /ws/live — navigateur s'abonne
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@app.websocket("/ws/live")
|
||||
async def ws_live(websocket: WebSocket):
|
||||
await websocket.accept()
|
||||
live_clients.add(websocket)
|
||||
|
||||
# Envoie l'état complet courant dès la connexion
|
||||
try:
|
||||
snapshot = slam.get_state_snapshot()
|
||||
snapshot["debit"] = len(debit_window) / 5.0
|
||||
await websocket.send_text(json.dumps(snapshot))
|
||||
|
||||
# Garde la connexion ouverte jusqu'à déconnexion
|
||||
while True:
|
||||
try:
|
||||
# ping keepalive (pas de timeout natif FastAPI WS)
|
||||
await asyncio.wait_for(websocket.receive_text(), timeout=30.0)
|
||||
except asyncio.TimeoutError:
|
||||
await websocket.send_text(json.dumps({"type": "ping"}))
|
||||
except WebSocketDisconnect:
|
||||
pass
|
||||
except Exception:
|
||||
pass
|
||||
finally:
|
||||
live_clients.discard(websocket)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# POST /session/reset
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@app.post("/session/reset")
|
||||
async def session_reset(token: str = Form(...)):
|
||||
if token != TOKEN:
|
||||
raise HTTPException(status_code=403, detail="Token invalide")
|
||||
slam.reset()
|
||||
session_records.clear()
|
||||
debit_window.clear()
|
||||
await broadcast({"type": "reset"})
|
||||
return {"status": "ok"}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# GET /healthz
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@app.get("/healthz")
|
||||
async def healthz():
|
||||
return {
|
||||
"status": "ok",
|
||||
"records": slam.records_count,
|
||||
"sweeps": slam.sweeps_done,
|
||||
"map_points": len(slam.imap),
|
||||
"loop_closures": slam.loop_closures,
|
||||
"live_clients": len(live_clients),
|
||||
}
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# GET /cloud.ply — export nuage 3D (contour extrudé altitude/depth)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@app.get("/cloud.ply")
|
||||
async def cloud_ply():
|
||||
map_arr = slam.imap.get_array()
|
||||
if len(map_arr) == 0:
|
||||
raise HTTPException(status_code=204, detail="Carte vide")
|
||||
|
||||
# Extrude les points 2D sur l'intervalle [depth - altitude, depth]
|
||||
# Utilise les dernières valeurs connues depuis les enregistrements
|
||||
depth = 1.5
|
||||
altitude = 0.5
|
||||
if session_records:
|
||||
last = session_records[-1]
|
||||
depth = last.get("depth", 1.5)
|
||||
altitude = last.get("altitude", 0.5)
|
||||
|
||||
z_top = -depth + altitude # fond (proche du bas)
|
||||
z_bot = -depth # surface de l'eau (z=0)
|
||||
|
||||
pts_3d = []
|
||||
for pt in map_arr:
|
||||
pts_3d.append((pt[0], pt[1], z_top))
|
||||
pts_3d.append((pt[0], pt[1], z_bot))
|
||||
|
||||
lines = [
|
||||
"ply",
|
||||
"format ascii 1.0",
|
||||
f"element vertex {len(pts_3d)}",
|
||||
"property float x",
|
||||
"property float y",
|
||||
"property float z",
|
||||
"end_header",
|
||||
]
|
||||
for p in pts_3d:
|
||||
lines.append(f"{p[0]:.4f} {p[1]:.4f} {p[2]:.4f}")
|
||||
|
||||
content = "\n".join(lines) + "\n"
|
||||
from fastapi.responses import Response
|
||||
return Response(
|
||||
content=content,
|
||||
media_type="application/octet-stream",
|
||||
headers={"Content-Disposition": "attachment; filename=moulin_cloud.ply"},
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Fichiers statiques + page principale
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@app.get("/", response_class=HTMLResponse)
|
||||
async def index():
|
||||
static_path = os.path.join(os.path.dirname(__file__), "static", "index.html")
|
||||
with open(static_path, "r", encoding="utf-8") as f:
|
||||
return f.read()
|
||||
|
||||
app.mount("/static", StaticFiles(directory=os.path.join(os.path.dirname(__file__), "static")), name="static")
|
||||
46
server/client/README.md
Normal file
46
server/client/README.md
Normal file
@@ -0,0 +1,46 @@
|
||||
# moulin-mapper — client stream
|
||||
|
||||
## Prérequis
|
||||
|
||||
```bash
|
||||
pip install -r requirements.txt
|
||||
```
|
||||
|
||||
## Commande (simulation rapide)
|
||||
|
||||
```bash
|
||||
python stream_client.py \
|
||||
--file ../data/sim/run_L.jsonl \
|
||||
--url wss://laboratoire.freeboxos.fr/moulin-live/ws/ingest \
|
||||
--token moulin-2026 \
|
||||
--speed 0
|
||||
```
|
||||
|
||||
## Commande temps réel
|
||||
|
||||
```bash
|
||||
python stream_client.py \
|
||||
--file ../data/sim/run_L.jsonl \
|
||||
--url wss://laboratoire.freeboxos.fr/moulin-live/ws/ingest \
|
||||
--token moulin-2026 \
|
||||
--speed 1.0
|
||||
```
|
||||
|
||||
## Bridge ROV (futur)
|
||||
|
||||
```bash
|
||||
rov_bridge | python stream_client.py \
|
||||
--stdin \
|
||||
--url wss://laboratoire.freeboxos.fr/moulin-live/ws/ingest \
|
||||
--token moulin-2026
|
||||
```
|
||||
|
||||
## Options
|
||||
|
||||
| Option | Défaut | Description |
|
||||
|--------|--------|-------------|
|
||||
| `--file FILE` | — | Fichier JSONL à envoyer |
|
||||
| `--stdin` | — | Lit depuis stdin (bridge ROV) |
|
||||
| `--url URL` | `ws://127.0.0.1:8211/ws/ingest` | URL WebSocket du serveur |
|
||||
| `--token TOKEN` | `moulin-2026` | Token d'authentification |
|
||||
| `--speed FLOAT` | `1.0` | 0=max, 1=temps réel, 2=2× plus vite |
|
||||
1
server/client/requirements.txt
Normal file
1
server/client/requirements.txt
Normal file
@@ -0,0 +1 @@
|
||||
websockets>=12.0
|
||||
102
server/client/stream_client.py
Normal file
102
server/client/stream_client.py
Normal file
@@ -0,0 +1,102 @@
|
||||
"""
|
||||
stream_client.py — Envoie un flux JSONL vers le serveur moulin-mapper.
|
||||
|
||||
Usage:
|
||||
python stream_client.py --file run_L.jsonl --url ws://127.0.0.1:8211/ws/ingest --token moulin-2026 --speed 1.0
|
||||
|
||||
--speed 0 : aussi vite que possible (pas de délai)
|
||||
--speed 1 : temps réel (deltas de t respectés)
|
||||
--stdin : lit depuis stdin au lieu d'un fichier (bridge ROV)
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import sys
|
||||
import time
|
||||
import asyncio
|
||||
|
||||
try:
|
||||
import websockets
|
||||
except ImportError:
|
||||
print("Installe d'abord : pip install websockets")
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
async def stream(url: str, token: str, lines, speed: float):
|
||||
# Ajoute le token comme query param
|
||||
sep = "&" if "?" in url else "?"
|
||||
full_url = f"{url}{sep}token={token}"
|
||||
|
||||
t_prev = None
|
||||
sent = 0
|
||||
t_file_prev = None
|
||||
|
||||
print(f"[moulin-stream] connexion → {full_url}")
|
||||
|
||||
while True:
|
||||
try:
|
||||
async with websockets.connect(full_url, ping_interval=20, ping_timeout=30) as ws:
|
||||
print("[moulin-stream] connecté")
|
||||
for line in lines:
|
||||
line = line.strip()
|
||||
if not line:
|
||||
continue
|
||||
try:
|
||||
rec = json.loads(line)
|
||||
except json.JSONDecodeError:
|
||||
continue
|
||||
|
||||
t_rec = rec.get("t", 0.0)
|
||||
|
||||
# Timing
|
||||
if speed > 0 and t_file_prev is not None:
|
||||
dt_file = t_rec - t_file_prev
|
||||
if dt_file > 0:
|
||||
await asyncio.sleep(dt_file / speed)
|
||||
|
||||
t_file_prev = t_rec
|
||||
|
||||
await ws.send(json.dumps(rec))
|
||||
sent += 1
|
||||
|
||||
if sent % 100 == 0:
|
||||
print(f"\r[moulin-stream] {sent} lignes envoyées, t={t_rec:.1f}s", end="", flush=True)
|
||||
|
||||
print(f"\n[moulin-stream] terminé — {sent} enregistrements envoyés")
|
||||
return
|
||||
|
||||
except (websockets.exceptions.ConnectionClosed,
|
||||
ConnectionRefusedError,
|
||||
OSError) as e:
|
||||
print(f"\n[moulin-stream] connexion perdue ({e}), reconnexion dans 3s…")
|
||||
await asyncio.sleep(3.0)
|
||||
# Redémarre depuis le début si on a perdu la connexion
|
||||
# (les données sont déjà lues depuis un itérateur — on ne peut pas rembobiner stdin)
|
||||
print("[moulin-stream] AVERTISSEMENT: relance depuis le début du fichier")
|
||||
break
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description="Stream JSONL vers moulin-mapper")
|
||||
src = parser.add_mutually_exclusive_group(required=True)
|
||||
src.add_argument("--file", help="Fichier JSONL à envoyer")
|
||||
src.add_argument("--stdin", action="store_true", help="Lit depuis stdin")
|
||||
parser.add_argument("--url", default="ws://127.0.0.1:8211/ws/ingest",
|
||||
help="URL WebSocket du serveur (ex: wss://lab.freeboxos.fr/moulin-live/ws/ingest)")
|
||||
parser.add_argument("--token", default="moulin-2026", help="Token d'authentification")
|
||||
parser.add_argument("--speed", type=float, default=1.0,
|
||||
help="Multiplicateur de vitesse (0=max, 1=temps réel, 2=2×)")
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.file:
|
||||
with open(args.file, "r") as f:
|
||||
lines = f.readlines()
|
||||
print(f"[moulin-stream] fichier: {args.file} ({len(lines)} lignes)")
|
||||
else:
|
||||
lines = sys.stdin
|
||||
|
||||
asyncio.run(stream(args.url, args.token, lines, args.speed))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
6
server/requirements.txt
Normal file
6
server/requirements.txt
Normal file
@@ -0,0 +1,6 @@
|
||||
fastapi>=0.111.0
|
||||
uvicorn[standard]>=0.30.0
|
||||
numpy>=1.24.0
|
||||
scipy>=1.11.0
|
||||
websockets>=12.0
|
||||
python-multipart>=0.0.9
|
||||
86
server/room.py
Normal file
86
server/room.py
Normal file
@@ -0,0 +1,86 @@
|
||||
"""
|
||||
room.py — Géométrie chambre de moulin + raycast 2D. Copie standalone.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from typing import Optional, List, Tuple
|
||||
|
||||
ROOM_POLYGON = np.array([
|
||||
[0.0, 0.0],
|
||||
[8.0, 0.0],
|
||||
[8.0, 5.0],
|
||||
[3.0, 5.0],
|
||||
[3.0, 8.0],
|
||||
[0.0, 8.0],
|
||||
], dtype=float)
|
||||
|
||||
NICHE_WALLS = np.array([
|
||||
[[0.0, 4.0], [3.0, 4.0]],
|
||||
[[0.0, 2.0], [3.0, 2.0]],
|
||||
], dtype=float)
|
||||
|
||||
|
||||
def _polygon_to_segments(poly: np.ndarray) -> List[Tuple[np.ndarray, np.ndarray]]:
|
||||
segs = []
|
||||
n = len(poly)
|
||||
for i in range(n):
|
||||
segs.append((poly[i], poly[(i + 1) % n]))
|
||||
return segs
|
||||
|
||||
|
||||
def get_all_segments() -> List[Tuple[np.ndarray, np.ndarray]]:
|
||||
segs = _polygon_to_segments(ROOM_POLYGON)
|
||||
for wall in NICHE_WALLS:
|
||||
segs.append((wall[0], wall[1]))
|
||||
return segs
|
||||
|
||||
|
||||
_ALL_SEGMENTS = get_all_segments()
|
||||
|
||||
|
||||
def _ray_segment_intersect(
|
||||
origin: np.ndarray,
|
||||
direction: np.ndarray,
|
||||
p0: np.ndarray,
|
||||
p1: np.ndarray,
|
||||
) -> Optional[float]:
|
||||
dx = direction[0]
|
||||
dy = direction[1]
|
||||
ex = p1[0] - p0[0]
|
||||
ey = p1[1] - p0[1]
|
||||
denom = -dx * ey + ex * dy
|
||||
if abs(denom) < 1e-12:
|
||||
return None
|
||||
bx = p0[0] - origin[0]
|
||||
by = p0[1] - origin[1]
|
||||
t = (-bx * ey + ex * by) / denom
|
||||
u = (dx * by - bx * dy) / denom
|
||||
if t >= 0.0 and 0.0 <= u <= 1.0:
|
||||
return t
|
||||
return None
|
||||
|
||||
|
||||
def range_to_walls(
|
||||
x: float,
|
||||
y: float,
|
||||
bearing_world_deg: float,
|
||||
max_range: float = 30.0,
|
||||
) -> Optional[float]:
|
||||
bearing_rad = np.deg2rad(bearing_world_deg)
|
||||
direction = np.array([np.sin(bearing_rad), np.cos(bearing_rad)], dtype=float)
|
||||
origin = np.array([x, y], dtype=float)
|
||||
min_t = None
|
||||
for p0, p1 in _ALL_SEGMENTS:
|
||||
t = _ray_segment_intersect(origin, direction, p0, p1)
|
||||
if t is not None and t > 1e-6:
|
||||
if min_t is None or t < min_t:
|
||||
min_t = t
|
||||
if min_t is not None and min_t <= max_range:
|
||||
return min_t
|
||||
return None
|
||||
|
||||
|
||||
def get_room_bounds() -> Tuple[float, float, float, float]:
|
||||
pts = ROOM_POLYGON
|
||||
return float(pts[:, 0].min()), float(pts[:, 0].max()), \
|
||||
float(pts[:, 1].min()), float(pts[:, 1].max())
|
||||
400
server/slam_incremental.py
Normal file
400
server/slam_incremental.py
Normal file
@@ -0,0 +1,400 @@
|
||||
"""
|
||||
slam_incremental.py — SLAM incrémental alimenté enregistrement par enregistrement.
|
||||
|
||||
Adapté depuis pipeline/slam.py pour une utilisation serveur (pas de batch).
|
||||
Appel: une seule méthode add_record(rec) par message reçu.
|
||||
Quand un sweep est complété → ICP + fermeture de boucle auto.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial import cKDTree
|
||||
from typing import List, Optional, Tuple, Dict
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Dead-reckoning
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class DeadReckoning:
|
||||
def __init__(self, alpha_magneto: float = 0.3, tau: float = 2.0):
|
||||
self.x = 0.0
|
||||
self.y = 0.0
|
||||
self.h_gyro = 0.0
|
||||
self.heading_deg = 0.0
|
||||
self.vf = 0.0
|
||||
self.vl = 0.0
|
||||
self.t_prev = None
|
||||
self.alpha = alpha_magneto
|
||||
self.tau = tau
|
||||
|
||||
def reset(self):
|
||||
self.x = 0.0
|
||||
self.y = 0.0
|
||||
self.h_gyro = 0.0
|
||||
self.heading_deg = 0.0
|
||||
self.vf = 0.0
|
||||
self.vl = 0.0
|
||||
self.t_prev = None
|
||||
|
||||
def update(self, t: float, heading_deg: float,
|
||||
ax: float, ay: float, gz: float,
|
||||
vf: float = None, vl: float = 0.0) -> Tuple[float, float, float]:
|
||||
if self.t_prev is None:
|
||||
self.t_prev = t
|
||||
self.h_gyro = heading_deg
|
||||
self.heading_deg = heading_deg
|
||||
return self.x, self.y, self.heading_deg
|
||||
|
||||
dt = t - self.t_prev
|
||||
if dt <= 0:
|
||||
return self.x, self.y, self.heading_deg
|
||||
self.t_prev = t
|
||||
|
||||
self.h_gyro = (self.h_gyro + np.rad2deg(gz) * dt) % 360
|
||||
dh = ((heading_deg - self.h_gyro + 180) % 360) - 180
|
||||
self.h_gyro = (self.h_gyro + self.alpha * dh) % 360
|
||||
self.heading_deg = self.h_gyro
|
||||
|
||||
if vf is not None:
|
||||
self.vf = vf
|
||||
self.vl = vl
|
||||
else:
|
||||
decay = np.exp(-dt / self.tau)
|
||||
self.vf = self.vf * decay + ax * dt
|
||||
self.vl = 0.0
|
||||
|
||||
h_rad = np.deg2rad(self.heading_deg)
|
||||
vx = self.vf * np.sin(h_rad) - self.vl * np.cos(h_rad)
|
||||
vy = self.vf * np.cos(h_rad) + self.vl * np.sin(h_rad)
|
||||
self.x += vx * dt
|
||||
self.y += vy * dt
|
||||
return self.x, self.y, self.heading_deg
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Accumulation de sweep
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class SweepAccumulator:
|
||||
def __init__(self):
|
||||
self.current_sweep: List[Dict] = []
|
||||
self.last_angle = None
|
||||
self.sweeps_done = 0
|
||||
|
||||
def reset(self):
|
||||
self.current_sweep = []
|
||||
self.last_angle = None
|
||||
self.sweeps_done = 0
|
||||
|
||||
def add_beam(self, angle_deg: float, distance: float,
|
||||
pose: Tuple[float, float, float]) -> Optional[List[Dict]]:
|
||||
beam = {"angle_deg": angle_deg, "distance": distance, "pose": pose}
|
||||
|
||||
if (self.last_angle is not None
|
||||
and self.last_angle > 180
|
||||
and angle_deg < 90):
|
||||
completed = self.current_sweep.copy()
|
||||
self.current_sweep = [beam]
|
||||
self.last_angle = angle_deg
|
||||
self.sweeps_done += 1
|
||||
if len(completed) >= 20:
|
||||
return completed
|
||||
return None
|
||||
|
||||
self.current_sweep.append(beam)
|
||||
self.last_angle = angle_deg
|
||||
return None
|
||||
|
||||
|
||||
def sweep_to_world_points(sweep: List[Dict]) -> np.ndarray:
|
||||
points = []
|
||||
for beam in sweep:
|
||||
d = beam["distance"]
|
||||
if d <= 0.01:
|
||||
continue
|
||||
angle_rel = beam["angle_deg"]
|
||||
px, py, hdg = beam["pose"]
|
||||
angle_world = (hdg + angle_rel) % 360.0
|
||||
a_rad = np.deg2rad(angle_world)
|
||||
wx = px + d * np.sin(a_rad)
|
||||
wy = py + d * np.cos(a_rad)
|
||||
points.append([wx, wy])
|
||||
if not points:
|
||||
return np.zeros((0, 2))
|
||||
return np.array(points)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# ICP 2D
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def icp_2d(
|
||||
source_pts: np.ndarray,
|
||||
map_pts: np.ndarray,
|
||||
max_iter: int = 20,
|
||||
tol: float = 1e-4,
|
||||
max_dist: float = 0.5,
|
||||
) -> Tuple[float, float, float, bool]:
|
||||
if len(source_pts) < 5 or len(map_pts) < 5:
|
||||
return 0.0, 0.0, 0.0, False
|
||||
|
||||
src = source_pts.copy()
|
||||
tree = cKDTree(map_pts)
|
||||
dx_total = 0.0
|
||||
dy_total = 0.0
|
||||
dtheta_total = 0.0
|
||||
|
||||
for _ in range(max_iter):
|
||||
dists, idxs = tree.query(src, k=1, workers=-1)
|
||||
mask = dists < max_dist
|
||||
if mask.sum() < 5:
|
||||
return dx_total, dy_total, dtheta_total, False
|
||||
|
||||
src_m = src[mask]
|
||||
dst_m = map_pts[idxs[mask]]
|
||||
c_src = src_m.mean(axis=0)
|
||||
c_dst = dst_m.mean(axis=0)
|
||||
A = src_m - c_src
|
||||
B = dst_m - c_dst
|
||||
H = A.T @ B
|
||||
U, S, Vt = np.linalg.svd(H)
|
||||
R = Vt.T @ U.T
|
||||
if np.linalg.det(R) < 0:
|
||||
Vt[-1, :] *= -1
|
||||
R = Vt.T @ U.T
|
||||
t = c_dst - R @ c_src
|
||||
src = (R @ src.T).T + t
|
||||
dx_total += t[0]
|
||||
dy_total += t[1]
|
||||
dtheta_total += np.arctan2(R[1, 0], R[0, 0])
|
||||
if np.abs(t).max() < tol and abs(np.arctan2(R[1, 0], R[0, 0])) < tol:
|
||||
return dx_total, dy_total, dtheta_total, True
|
||||
|
||||
return dx_total, dy_total, dtheta_total, True
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Carte incrémentale
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class IncrementalMap:
|
||||
def __init__(self, voxel_size: float = 0.05):
|
||||
self.points: List[np.ndarray] = []
|
||||
self.voxel_size = voxel_size
|
||||
self._voxel_set = set()
|
||||
|
||||
def reset(self):
|
||||
self.points = []
|
||||
self._voxel_set = set()
|
||||
|
||||
def add_points(self, pts: np.ndarray) -> int:
|
||||
"""Retourne le nombre de nouveaux points ajoutés."""
|
||||
added = 0
|
||||
for pt in pts:
|
||||
key = (int(pt[0] / self.voxel_size), int(pt[1] / self.voxel_size))
|
||||
if key not in self._voxel_set:
|
||||
self._voxel_set.add(key)
|
||||
self.points.append(pt)
|
||||
added += 1
|
||||
return added
|
||||
|
||||
def get_array(self) -> np.ndarray:
|
||||
if not self.points:
|
||||
return np.zeros((0, 2))
|
||||
return np.array(self.points)
|
||||
|
||||
def __len__(self):
|
||||
return len(self.points)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Fermeture de boucle
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class LoopClosureDetector:
|
||||
def __init__(self, min_time: float = 15.0, dist_thresh: float = 0.8):
|
||||
self.min_time = min_time
|
||||
self.dist_thresh = dist_thresh
|
||||
self.history: List[Tuple[float, float, float, int]] = []
|
||||
|
||||
def reset(self):
|
||||
self.history = []
|
||||
|
||||
def add_pose(self, t: float, x: float, y: float, sweep_idx: int) -> None:
|
||||
self.history.append((t, x, y, sweep_idx))
|
||||
|
||||
def check(self, t: float, x: float, y: float) -> Optional[int]:
|
||||
for (t_old, x_old, y_old, idx_old) in self.history:
|
||||
if t - t_old < self.min_time:
|
||||
continue
|
||||
dist = np.sqrt((x - x_old)**2 + (y - y_old)**2)
|
||||
if dist < self.dist_thresh:
|
||||
return idx_old
|
||||
return None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Orchestrateur SLAM incrémental
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class IncrementalSLAM:
|
||||
"""
|
||||
Reçoit les enregistrements un par un via add_record().
|
||||
Après chaque sweep complet → ICP + fermeture de boucle.
|
||||
Expose: pose_dr, pose_corrected, map, trajectory, loop_closures.
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
self.dr = DeadReckoning()
|
||||
self.sweep_acc = SweepAccumulator()
|
||||
self.imap = IncrementalMap()
|
||||
self.lcd = LoopClosureDetector()
|
||||
|
||||
# Pose courante (x, y, heading_deg)
|
||||
self.pose_dr = (0.0, 0.0, 0.0)
|
||||
self.pose_corrected = (0.0, 0.0, 0.0)
|
||||
|
||||
# Trajectoire : liste de (t, x, y, h) — corrigée
|
||||
self.trajectory: List[Tuple[float, float, float, float]] = []
|
||||
|
||||
# Stats
|
||||
self.records_count = 0
|
||||
self.sweeps_done = 0
|
||||
self.loop_closures = 0
|
||||
self.last_t = 0.0
|
||||
|
||||
# Points ajoutés au dernier sweep (pour delta live)
|
||||
self._last_new_points: np.ndarray = np.zeros((0, 2))
|
||||
|
||||
def reset(self):
|
||||
self.dr.reset()
|
||||
self.sweep_acc.reset()
|
||||
self.imap.reset()
|
||||
self.lcd.reset()
|
||||
self.pose_dr = (0.0, 0.0, 0.0)
|
||||
self.pose_corrected = (0.0, 0.0, 0.0)
|
||||
self.trajectory = []
|
||||
self.records_count = 0
|
||||
self.sweeps_done = 0
|
||||
self.loop_closures = 0
|
||||
self.last_t = 0.0
|
||||
self._last_new_points = np.zeros((0, 2))
|
||||
|
||||
def add_record(self, rec: dict) -> bool:
|
||||
"""
|
||||
Traite un enregistrement. Retourne True si un sweep vient d'être complété
|
||||
(le client /ws/live recevra alors un delta).
|
||||
"""
|
||||
t = rec["t"]
|
||||
self.records_count += 1
|
||||
self.last_t = t
|
||||
|
||||
# DR
|
||||
x, y, h = self.dr.update(
|
||||
t=t,
|
||||
heading_deg=rec["heading"],
|
||||
ax=rec["ax"],
|
||||
ay=rec["ay"],
|
||||
gz=rec["gz"],
|
||||
vf=rec.get("vf"),
|
||||
vl=rec.get("vl", 0.0),
|
||||
)
|
||||
self.pose_dr = (x, y, h)
|
||||
|
||||
# Sweep accumulation
|
||||
sweep = self.sweep_acc.add_beam(
|
||||
angle_deg=rec["ping360_angle"],
|
||||
distance=rec["ping360_distance"],
|
||||
pose=(x, y, h),
|
||||
)
|
||||
|
||||
if sweep is None:
|
||||
return False
|
||||
|
||||
# --- Sweep complet ---
|
||||
self.sweeps_done = self.sweep_acc.sweeps_done
|
||||
new_pts = sweep_to_world_points(sweep)
|
||||
|
||||
# ICP si la carte n'est pas vide
|
||||
map_arr = self.imap.get_array()
|
||||
dx_icp = dy_icp = dtheta_icp = 0.0
|
||||
if len(map_arr) >= 5 and len(new_pts) >= 5:
|
||||
dx_icp, dy_icp, dtheta_icp, _ = icp_2d(new_pts, map_arr)
|
||||
# Appliquer correction ICP à la pose courante
|
||||
x_c = x + dx_icp
|
||||
y_c = y + dy_icp
|
||||
h_c = (h + np.rad2deg(dtheta_icp)) % 360
|
||||
self.pose_corrected = (x_c, y_c, h_c)
|
||||
# Recalculer les points corrigés pour la carte
|
||||
corrected_sweep = [
|
||||
{
|
||||
"angle_deg": b["angle_deg"],
|
||||
"distance": b["distance"],
|
||||
"pose": (
|
||||
b["pose"][0] + dx_icp,
|
||||
b["pose"][1] + dy_icp,
|
||||
b["pose"][2] + np.rad2deg(dtheta_icp),
|
||||
)
|
||||
}
|
||||
for b in sweep
|
||||
]
|
||||
map_pts_new = sweep_to_world_points(corrected_sweep)
|
||||
else:
|
||||
self.pose_corrected = (x, y, h)
|
||||
map_pts_new = new_pts
|
||||
|
||||
# Ajouter les points à la carte
|
||||
added = self.imap.add_points(map_pts_new)
|
||||
self._last_new_points = map_pts_new if added > 0 else np.zeros((0, 2))
|
||||
|
||||
# Trajectoire
|
||||
xc, yc, hc = self.pose_corrected
|
||||
self.trajectory.append((t, xc, yc, hc))
|
||||
|
||||
# Fermeture de boucle
|
||||
loop_old_idx = self.lcd.check(t, xc, yc)
|
||||
if loop_old_idx is not None:
|
||||
self.loop_closures += 1
|
||||
# Redistribution linéaire correction sur dernières poses
|
||||
n_loop = len(self.trajectory) - loop_old_idx
|
||||
if n_loop > 1:
|
||||
corr = np.array([
|
||||
self.trajectory[loop_old_idx][1] - xc,
|
||||
self.trajectory[loop_old_idx][2] - yc,
|
||||
])
|
||||
for i in range(loop_old_idx, len(self.trajectory)):
|
||||
alpha = (i - loop_old_idx) / n_loop
|
||||
tt, xx, yy, hh = self.trajectory[i]
|
||||
self.trajectory[i] = (tt, xx + alpha * corr[0], yy + alpha * corr[1], hh)
|
||||
|
||||
self.lcd.add_pose(t, xc, yc, len(self.trajectory) - 1)
|
||||
|
||||
return True
|
||||
|
||||
def get_state_snapshot(self) -> dict:
|
||||
"""Retourne l'état complet (pour connexion initiale /ws/live)."""
|
||||
map_arr = self.imap.get_array()
|
||||
return {
|
||||
"type": "snapshot",
|
||||
"records": self.records_count,
|
||||
"sweeps": self.sweeps_done,
|
||||
"loop_closures": self.loop_closures,
|
||||
"last_t": self.last_t,
|
||||
"pose_dr": list(self.pose_dr),
|
||||
"pose_corrected": list(self.pose_corrected),
|
||||
"map_points": map_arr.tolist(),
|
||||
"trajectory": [list(p) for p in self.trajectory],
|
||||
}
|
||||
|
||||
def get_sweep_delta(self) -> dict:
|
||||
"""Retourne le delta après un sweep (nouveaux points + pose actuelle)."""
|
||||
return {
|
||||
"type": "delta",
|
||||
"records": self.records_count,
|
||||
"sweeps": self.sweeps_done,
|
||||
"loop_closures": self.loop_closures,
|
||||
"last_t": self.last_t,
|
||||
"pose_dr": list(self.pose_dr),
|
||||
"pose_corrected": list(self.pose_corrected),
|
||||
"new_map_points": self._last_new_points.tolist(),
|
||||
"trajectory_tail": [list(p) for p in self.trajectory[-5:]],
|
||||
}
|
||||
487
server/static/index.html
Normal file
487
server/static/index.html
Normal file
@@ -0,0 +1,487 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="fr">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||
<title>Moulin Mapper — Live</title>
|
||||
<style>
|
||||
* { box-sizing: border-box; margin: 0; padding: 0; }
|
||||
body {
|
||||
background: #0d1117;
|
||||
color: #c9d1d9;
|
||||
font-family: 'Consolas', 'Monaco', monospace;
|
||||
font-size: 13px;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
height: 100vh;
|
||||
overflow: hidden;
|
||||
}
|
||||
header {
|
||||
background: #161b22;
|
||||
border-bottom: 1px solid #30363d;
|
||||
padding: 8px 16px;
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 16px;
|
||||
flex-shrink: 0;
|
||||
}
|
||||
header h1 { font-size: 15px; font-weight: 600; color: #58a6ff; letter-spacing: 1px; }
|
||||
#conn-dot { width: 10px; height: 10px; border-radius: 50%; background: #f85149; flex-shrink: 0; }
|
||||
#conn-dot.ok { background: #3fb950; }
|
||||
#conn-label { font-size: 12px; }
|
||||
.main {
|
||||
display: flex;
|
||||
flex: 1;
|
||||
overflow: hidden;
|
||||
}
|
||||
/* Panneau gauche: canvas */
|
||||
#canvas-wrap {
|
||||
flex: 1;
|
||||
position: relative;
|
||||
background: #010409;
|
||||
}
|
||||
canvas {
|
||||
display: block;
|
||||
width: 100%;
|
||||
height: 100%;
|
||||
}
|
||||
/* Panneau droit: stats + contrôles */
|
||||
aside {
|
||||
width: 260px;
|
||||
background: #161b22;
|
||||
border-left: 1px solid #30363d;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 0;
|
||||
overflow-y: auto;
|
||||
flex-shrink: 0;
|
||||
}
|
||||
.section {
|
||||
padding: 12px 14px;
|
||||
border-bottom: 1px solid #21262d;
|
||||
}
|
||||
.section h2 {
|
||||
font-size: 11px;
|
||||
text-transform: uppercase;
|
||||
letter-spacing: 1px;
|
||||
color: #8b949e;
|
||||
margin-bottom: 8px;
|
||||
}
|
||||
.stat-row {
|
||||
display: flex;
|
||||
justify-content: space-between;
|
||||
padding: 3px 0;
|
||||
border-bottom: 1px solid #0d1117;
|
||||
}
|
||||
.stat-row:last-child { border-bottom: none; }
|
||||
.stat-label { color: #8b949e; }
|
||||
.stat-value { color: #f0f6fc; font-weight: 600; }
|
||||
.stat-value.accent { color: #58a6ff; }
|
||||
.stat-value.warn { color: #e3b341; }
|
||||
input[type=text] {
|
||||
width: 100%;
|
||||
background: #0d1117;
|
||||
border: 1px solid #30363d;
|
||||
border-radius: 4px;
|
||||
color: #c9d1d9;
|
||||
padding: 5px 8px;
|
||||
font-family: inherit;
|
||||
font-size: 12px;
|
||||
margin-bottom: 8px;
|
||||
}
|
||||
input[type=text]:focus { outline: none; border-color: #58a6ff; }
|
||||
button {
|
||||
width: 100%;
|
||||
padding: 6px 10px;
|
||||
border-radius: 4px;
|
||||
border: 1px solid;
|
||||
cursor: pointer;
|
||||
font-family: inherit;
|
||||
font-size: 12px;
|
||||
font-weight: 600;
|
||||
margin-bottom: 6px;
|
||||
}
|
||||
.btn-danger {
|
||||
background: #21262d;
|
||||
border-color: #f85149;
|
||||
color: #f85149;
|
||||
}
|
||||
.btn-danger:hover { background: #3d1c1c; }
|
||||
.btn-primary {
|
||||
background: #21262d;
|
||||
border-color: #58a6ff;
|
||||
color: #58a6ff;
|
||||
text-decoration: none;
|
||||
display: block;
|
||||
text-align: center;
|
||||
}
|
||||
.btn-primary:hover { background: #1c2a3d; }
|
||||
#log {
|
||||
font-size: 11px;
|
||||
color: #8b949e;
|
||||
max-height: 120px;
|
||||
overflow-y: auto;
|
||||
padding: 8px 14px;
|
||||
flex: 1;
|
||||
word-break: break-all;
|
||||
}
|
||||
#log p { margin-bottom: 2px; line-height: 1.4; }
|
||||
#log p.err { color: #f85149; }
|
||||
#log p.ok { color: #3fb950; }
|
||||
.legend {
|
||||
display: flex; flex-direction: column; gap: 4px;
|
||||
font-size: 11px; color: #8b949e;
|
||||
}
|
||||
.legend-item { display: flex; align-items: center; gap: 6px; }
|
||||
.leg-dot { width: 10px; height: 10px; border-radius: 50%; flex-shrink: 0; }
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
|
||||
<header>
|
||||
<h1>MOULIN MAPPER</h1>
|
||||
<div id="conn-dot"></div>
|
||||
<span id="conn-label">Déconnecté</span>
|
||||
</header>
|
||||
|
||||
<div class="main">
|
||||
<div id="canvas-wrap">
|
||||
<canvas id="map-canvas"></canvas>
|
||||
</div>
|
||||
|
||||
<aside>
|
||||
<div class="section">
|
||||
<h2>Stats</h2>
|
||||
<div class="stat-row"><span class="stat-label">Enregistrements</span><span class="stat-value accent" id="s-records">0</span></div>
|
||||
<div class="stat-row"><span class="stat-label">Sweeps</span><span class="stat-value" id="s-sweeps">0</span></div>
|
||||
<div class="stat-row"><span class="stat-label">Points carte</span><span class="stat-value" id="s-points">0</span></div>
|
||||
<div class="stat-row"><span class="stat-label">Fermetures boucle</span><span class="stat-value warn" id="s-loops">0</span></div>
|
||||
<div class="stat-row"><span class="stat-label">Dernier t</span><span class="stat-value" id="s-t">—</span></div>
|
||||
<div class="stat-row"><span class="stat-label">Débit</span><span class="stat-value" id="s-debit">0</span></div>
|
||||
</div>
|
||||
|
||||
<div class="section">
|
||||
<h2>Pose ROV</h2>
|
||||
<div class="stat-row"><span class="stat-label">x (m)</span><span class="stat-value" id="s-x">0.00</span></div>
|
||||
<div class="stat-row"><span class="stat-label">y (m)</span><span class="stat-value" id="s-y">0.00</span></div>
|
||||
<div class="stat-row"><span class="stat-label">cap (°)</span><span class="stat-value" id="s-h">0.0</span></div>
|
||||
</div>
|
||||
|
||||
<div class="section">
|
||||
<h2>Légende</h2>
|
||||
<div class="legend">
|
||||
<div class="legend-item"><div class="leg-dot" style="background:#58a6ff;"></div><span>Points carte (murs)</span></div>
|
||||
<div class="legend-item"><div class="leg-dot" style="background:#3fb950;"></div><span>Trajectoire corrigée</span></div>
|
||||
<div class="legend-item"><div class="leg-dot" style="background:#f0f6fc; border-radius:0;"></div><span>ROV courant</span></div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="section">
|
||||
<h2>Contrôles</h2>
|
||||
<input type="text" id="token-input" placeholder="Token (défaut: moulin-2026)" value="moulin-2026">
|
||||
<button class="btn-danger" onclick="resetSession()">Reset session</button>
|
||||
<a id="ply-link" class="btn-primary" href="./cloud.ply" download="moulin_cloud.ply">Télécharger nuage .ply</a>
|
||||
</div>
|
||||
|
||||
<div class="section" style="flex:1;padding:0;">
|
||||
<div id="log"></div>
|
||||
</div>
|
||||
</aside>
|
||||
</div>
|
||||
|
||||
<script>
|
||||
// ---------------------------------------------------------------------------
|
||||
// Données en mémoire
|
||||
// ---------------------------------------------------------------------------
|
||||
let mapPoints = []; // [[x, y], ...]
|
||||
let trajectory = []; // [[t, x, y, h], ...]
|
||||
let poseCorrect = [0, 0, 0];
|
||||
let stats = { records: 0, sweeps: 0, loop_closures: 0, last_t: 0, debit: 0 };
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Canvas + viewport
|
||||
// ---------------------------------------------------------------------------
|
||||
const canvas = document.getElementById('map-canvas');
|
||||
const ctx = canvas.getContext('2d');
|
||||
|
||||
let viewX = 4.0; // centre monde visible
|
||||
let viewY = 4.0;
|
||||
let viewScale = 50; // px/m
|
||||
let dragging = false;
|
||||
let dragStart = null;
|
||||
let dragView = null;
|
||||
|
||||
function worldToCanvas(wx, wy) {
|
||||
const cx = canvas.width / 2;
|
||||
const cy = canvas.height / 2;
|
||||
return [
|
||||
cx + (wx - viewX) * viewScale,
|
||||
cy - (wy - viewY) * viewScale,
|
||||
];
|
||||
}
|
||||
|
||||
function resizeCanvas() {
|
||||
const wrap = document.getElementById('canvas-wrap');
|
||||
canvas.width = wrap.clientWidth;
|
||||
canvas.height = wrap.clientHeight;
|
||||
draw();
|
||||
}
|
||||
window.addEventListener('resize', resizeCanvas);
|
||||
|
||||
// Zoom molette
|
||||
canvas.addEventListener('wheel', (e) => {
|
||||
e.preventDefault();
|
||||
const factor = e.deltaY > 0 ? 0.85 : 1.18;
|
||||
viewScale = Math.max(5, Math.min(500, viewScale * factor));
|
||||
draw();
|
||||
}, { passive: false });
|
||||
|
||||
// Pan souris
|
||||
canvas.addEventListener('mousedown', (e) => {
|
||||
dragging = true;
|
||||
dragStart = [e.clientX, e.clientY];
|
||||
dragView = [viewX, viewY];
|
||||
});
|
||||
canvas.addEventListener('mousemove', (e) => {
|
||||
if (!dragging) return;
|
||||
const dx = (e.clientX - dragStart[0]) / viewScale;
|
||||
const dy = (e.clientY - dragStart[1]) / viewScale;
|
||||
viewX = dragView[0] - dx;
|
||||
viewY = dragView[1] + dy;
|
||||
draw();
|
||||
});
|
||||
canvas.addEventListener('mouseup', () => { dragging = false; });
|
||||
canvas.addEventListener('mouseleave', () => { dragging = false; });
|
||||
|
||||
function draw() {
|
||||
ctx.clearRect(0, 0, canvas.width, canvas.height);
|
||||
|
||||
// Grille légère
|
||||
ctx.strokeStyle = '#1c2128';
|
||||
ctx.lineWidth = 1;
|
||||
const gridStep = 1; // 1m
|
||||
const xMin = viewX - canvas.width / (2 * viewScale) - gridStep;
|
||||
const xMax = viewX + canvas.width / (2 * viewScale) + gridStep;
|
||||
const yMin = viewY - canvas.height / (2 * viewScale) - gridStep;
|
||||
const yMax = viewY + canvas.height / (2 * viewScale) + gridStep;
|
||||
for (let gx = Math.floor(xMin); gx <= xMax; gx += gridStep) {
|
||||
ctx.beginPath();
|
||||
const [px, ] = worldToCanvas(gx, 0);
|
||||
ctx.moveTo(px, 0); ctx.lineTo(px, canvas.height);
|
||||
ctx.stroke();
|
||||
}
|
||||
for (let gy = Math.floor(yMin); gy <= yMax; gy += gridStep) {
|
||||
ctx.beginPath();
|
||||
const [, py] = worldToCanvas(0, gy);
|
||||
ctx.moveTo(0, py); ctx.lineTo(canvas.width, py);
|
||||
ctx.stroke();
|
||||
}
|
||||
|
||||
// Axes origine
|
||||
ctx.strokeStyle = '#21262d';
|
||||
ctx.lineWidth = 1;
|
||||
const [ox, oy] = worldToCanvas(0, 0);
|
||||
ctx.beginPath(); ctx.moveTo(ox, 0); ctx.lineTo(ox, canvas.height); ctx.stroke();
|
||||
ctx.beginPath(); ctx.moveTo(0, oy); ctx.lineTo(canvas.width, oy); ctx.stroke();
|
||||
|
||||
// Points carte (murs)
|
||||
const ptRadius = Math.max(1, viewScale * 0.04);
|
||||
ctx.fillStyle = 'rgba(88, 166, 255, 0.7)';
|
||||
for (const [wx, wy] of mapPoints) {
|
||||
const [px, py] = worldToCanvas(wx, wy);
|
||||
ctx.beginPath();
|
||||
ctx.arc(px, py, ptRadius, 0, 2 * Math.PI);
|
||||
ctx.fill();
|
||||
}
|
||||
|
||||
// Trajectoire
|
||||
if (trajectory.length > 1) {
|
||||
ctx.strokeStyle = 'rgba(63, 185, 80, 0.8)';
|
||||
ctx.lineWidth = 1.5;
|
||||
ctx.beginPath();
|
||||
const [x0, y0] = worldToCanvas(trajectory[0][1], trajectory[0][2]);
|
||||
ctx.moveTo(x0, y0);
|
||||
for (let i = 1; i < trajectory.length; i++) {
|
||||
const [px, py] = worldToCanvas(trajectory[i][1], trajectory[i][2]);
|
||||
ctx.lineTo(px, py);
|
||||
}
|
||||
ctx.stroke();
|
||||
}
|
||||
|
||||
// ROV courant
|
||||
const [rx, ry] = worldToCanvas(poseCorrect[0], poseCorrect[1]);
|
||||
const hRad = (90 - poseCorrect[2]) * Math.PI / 180; // cap → angle canvas
|
||||
const arrowLen = Math.max(8, viewScale * 0.3);
|
||||
ctx.save();
|
||||
ctx.translate(rx, ry);
|
||||
ctx.rotate(-hRad);
|
||||
ctx.fillStyle = '#f0f6fc';
|
||||
ctx.strokeStyle = '#f0f6fc';
|
||||
ctx.lineWidth = 1.5;
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(0, -arrowLen);
|
||||
ctx.lineTo(arrowLen * 0.5, arrowLen * 0.5);
|
||||
ctx.lineTo(0, 0);
|
||||
ctx.lineTo(-arrowLen * 0.5, arrowLen * 0.5);
|
||||
ctx.closePath();
|
||||
ctx.fill();
|
||||
ctx.restore();
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Stats UI
|
||||
// ---------------------------------------------------------------------------
|
||||
function updateStats(data) {
|
||||
if (data.records !== undefined) {
|
||||
stats.records = data.records;
|
||||
stats.sweeps = data.sweeps;
|
||||
stats.loop_closures = data.loop_closures;
|
||||
stats.last_t = data.last_t;
|
||||
stats.debit = data.debit || 0;
|
||||
}
|
||||
document.getElementById('s-records').textContent = stats.records;
|
||||
document.getElementById('s-sweeps').textContent = stats.sweeps;
|
||||
document.getElementById('s-loops').textContent = stats.loop_closures;
|
||||
document.getElementById('s-t').textContent = stats.last_t ? stats.last_t.toFixed(1) + 's' : '—';
|
||||
document.getElementById('s-debit').textContent = stats.debit.toFixed(1) + ' rec/s';
|
||||
document.getElementById('s-points').textContent = mapPoints.length;
|
||||
|
||||
if (data.pose_corrected) {
|
||||
poseCorrect = data.pose_corrected;
|
||||
document.getElementById('s-x').textContent = poseCorrect[0].toFixed(2);
|
||||
document.getElementById('s-y').textContent = poseCorrect[1].toFixed(2);
|
||||
document.getElementById('s-h').textContent = poseCorrect[2].toFixed(1);
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Gestion message WS
|
||||
// ---------------------------------------------------------------------------
|
||||
function handleMessage(data) {
|
||||
if (data.type === 'snapshot') {
|
||||
// État complet
|
||||
mapPoints = data.map_points || [];
|
||||
trajectory = data.trajectory || [];
|
||||
poseCorrect = data.pose_corrected || [0, 0, 0];
|
||||
updateStats(data);
|
||||
draw();
|
||||
log('Snapshot reçu — ' + mapPoints.length + ' pts, ' + trajectory.length + ' poses', 'ok');
|
||||
} else if (data.type === 'delta') {
|
||||
// Nouveaux points
|
||||
const newPts = data.new_map_points || [];
|
||||
for (const pt of newPts) mapPoints.push(pt);
|
||||
// Trajectoire : ajout des dernières poses
|
||||
const tail = data.trajectory_tail || [];
|
||||
for (const pose of tail) {
|
||||
// Évite les doublons par t
|
||||
if (trajectory.length === 0 || trajectory[trajectory.length - 1][0] < pose[0]) {
|
||||
trajectory.push(pose);
|
||||
}
|
||||
}
|
||||
updateStats(data);
|
||||
draw();
|
||||
} else if (data.type === 'reset') {
|
||||
mapPoints = [];
|
||||
trajectory = [];
|
||||
poseCorrect = [0, 0, 0];
|
||||
stats = { records: 0, sweeps: 0, loop_closures: 0, last_t: 0, debit: 0 };
|
||||
updateStats({});
|
||||
draw();
|
||||
log('Session réinitialisée', 'ok');
|
||||
} else if (data.type === 'ping') {
|
||||
// keepalive silencieux
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Connexion WS /ws/live
|
||||
// ---------------------------------------------------------------------------
|
||||
let ws = null;
|
||||
let reconnectTimer = null;
|
||||
|
||||
function wsConnect() {
|
||||
const base = location.pathname.replace(/\/[^/]*$/, '').replace(/\/$/, '');
|
||||
const proto = location.protocol === 'https:' ? 'wss:' : 'ws:';
|
||||
const url = proto + '//' + location.host + base + '/ws/live';
|
||||
|
||||
log('Connexion ' + url + '…');
|
||||
ws = new WebSocket(url);
|
||||
|
||||
ws.onopen = () => {
|
||||
document.getElementById('conn-dot').classList.add('ok');
|
||||
document.getElementById('conn-label').textContent = 'Connecté';
|
||||
log('WS ouvert', 'ok');
|
||||
if (reconnectTimer) { clearTimeout(reconnectTimer); reconnectTimer = null; }
|
||||
};
|
||||
|
||||
ws.onmessage = (e) => {
|
||||
try {
|
||||
handleMessage(JSON.parse(e.data));
|
||||
} catch (err) {
|
||||
console.error('parse WS:', err);
|
||||
}
|
||||
};
|
||||
|
||||
ws.onclose = () => {
|
||||
document.getElementById('conn-dot').classList.remove('ok');
|
||||
document.getElementById('conn-label').textContent = 'Déconnecté — reconnexion dans 3s…';
|
||||
log('WS fermé, tentative dans 3s…', 'err');
|
||||
reconnectTimer = setTimeout(wsConnect, 3000);
|
||||
};
|
||||
|
||||
ws.onerror = (e) => {
|
||||
log('Erreur WS', 'err');
|
||||
};
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Reset session
|
||||
// ---------------------------------------------------------------------------
|
||||
async function resetSession() {
|
||||
const token = document.getElementById('token-input').value.trim() || 'moulin-2026';
|
||||
const base = location.pathname.replace(/\/[^/]*$/, '').replace(/\/$/, '');
|
||||
try {
|
||||
const form = new FormData();
|
||||
form.append('token', token);
|
||||
const resp = await fetch(base + '/session/reset', { method: 'POST', body: form });
|
||||
if (resp.ok) {
|
||||
log('Reset OK', 'ok');
|
||||
} else {
|
||||
log('Reset KO: ' + resp.status, 'err');
|
||||
}
|
||||
} catch (e) {
|
||||
log('Reset erreur: ' + e.message, 'err');
|
||||
}
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Bouton PLY — chemin relatif
|
||||
// ---------------------------------------------------------------------------
|
||||
(function() {
|
||||
const base = location.pathname.replace(/\/[^/]*$/, '').replace(/\/$/, '');
|
||||
document.getElementById('ply-link').href = base + '/cloud.ply';
|
||||
})();
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Log
|
||||
// ---------------------------------------------------------------------------
|
||||
function log(msg, cls) {
|
||||
const el = document.getElementById('log');
|
||||
const p = document.createElement('p');
|
||||
if (cls) p.className = cls;
|
||||
const now = new Date();
|
||||
p.textContent = now.toTimeString().slice(0, 8) + ' ' + msg;
|
||||
el.appendChild(p);
|
||||
el.scrollTop = el.scrollHeight;
|
||||
// Garde les 100 dernières lignes
|
||||
while (el.children.length > 100) el.removeChild(el.firstChild);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Init
|
||||
// ---------------------------------------------------------------------------
|
||||
resizeCanvas();
|
||||
wsConnect();
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
Reference in New Issue
Block a user