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:
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())
|
||||
Reference in New Issue
Block a user