feat(moulin-mapper): simulateur capteurs + pipeline SLAM Python (JSONL stream → trajectoire + nuage .ply)
- simulator.py: flux JSONL réaliste (Ping360 angle/dist, IMU, heading, depth, altitude) + vérité terrain - slam.py: dead-reckoning + scan-to-map ICP 2D (cKDTree) + fermeture de boucle - process.py: ingestion streaming ligne-par-ligne → trajectory.csv + map_2d.csv + cloud.ply - stream_replay.py: rejoue le flux (vision streaming remote) - SCHEMA.md: contrat format données ROV réel↔sim - RMS dead-reckoning 0.386m → scan-matching 0.188m (2x)
This commit is contained in:
Flag
314
pipeline/simulator.py
Normal file
314
pipeline/simulator.py
Normal file
@@ -0,0 +1,314 @@
|
||||
"""
|
||||
simulator.py — Génère un flux JSONL réaliste pour le moulin-mapper.
|
||||
|
||||
Produit :
|
||||
../data/sim/run_L.jsonl — flux capteurs (même format que le vrai ROV)
|
||||
../data/sim/run_L_truth.csv — vérité terrain (x, y, heading_deg, z, t)
|
||||
|
||||
Usage : python3 simulator.py [--out-dir ../data/sim] [--sweep-period 4.0]
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
import csv
|
||||
import numpy as np
|
||||
|
||||
# Assure que room.py est importable depuis pipeline/
|
||||
import sys
|
||||
sys.path.insert(0, os.path.dirname(__file__))
|
||||
from room import range_to_walls
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Paramètres capteurs
|
||||
# ---------------------------------------------------------------------------
|
||||
PING360_STEPS = 400 # pas/tour (vrai Ping360)
|
||||
DEG_PER_STEP = 360.0 / PING360_STEPS # 0.9°/pas
|
||||
PING360_NOISE_M = 0.03 # bruit mesure sonar (m RMS)
|
||||
PING360_DROPOUT_PROB = 0.03 # probabilité de retour nul
|
||||
|
||||
# IMU : biais accéléro réaliste MEMS (BlueRobotics Navigator)
|
||||
# Petits biais : la vraie dérive vient de leur intégration double
|
||||
IMU_ACCEL_NOISE = 0.02 # m/s² (bruit blanc accéléro)
|
||||
IMU_ACCEL_BIAS_F = 0.015 # m/s² biais avant (axe body X)
|
||||
IMU_ACCEL_BIAS_L = -0.010 # m/s² biais latéral (axe body Y)
|
||||
IMU_GYRO_NOISE = 0.001 # rad/s (bruit gyro)
|
||||
IMU_GYRO_BIAS = 0.003 # rad/s (biais lacet → dérive heading si on n'a pas de magnéto)
|
||||
|
||||
HEADING_NOISE_DEG = 0.4 # bruit magnéto (deg RMS)
|
||||
HEADING_DRIFT_RATE = 0.01 # deg/s dérive très lente magnéto (réaliste)
|
||||
|
||||
ALTITUDE_NOISE_M = 0.04 # bruit Ping1D
|
||||
ALTITUDE_DROPOUT_PROB = 0.04
|
||||
ROV_Z = 1.5 # profondeur ROV sous surface (m)
|
||||
ROV_ALT = 0.5 # hauteur ROV au-dessus du fond (m)
|
||||
DEPTH_NOISE_M = 0.015 # bruit capteur pression
|
||||
|
||||
GRAVITY = 9.81 # m/s²
|
||||
|
||||
# Vitesse de translation ROV (m/s) — BlueROV2 lent dans une chambre étroite
|
||||
ROV_SPEED = 0.3 # m/s (vitesse de croisière)
|
||||
# Taux de rotation max (deg/s) en virage
|
||||
ROV_TURN_RATE = 20.0 # deg/s
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Trajectoire scriptée avec cinématique lisse
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def smooth_trajectory(dt: float = 0.05) -> np.ndarray:
|
||||
"""
|
||||
Trajectoire ROV avec vitesses lisses (pas de saut d'accélération).
|
||||
Le ROV se déplace à vitesse constante ROV_SPEED, tourne progressivement.
|
||||
Retourne array (N, 4) : [t, x, y, heading_deg]
|
||||
|
||||
Trajectoire en L couvrant la chambre, avec retour près du départ (boucle).
|
||||
"""
|
||||
# Waypoints (x_target, y_target) — le ROV va d'un point à l'autre
|
||||
# à vitesse constante, en tournant progressivement vers la destination
|
||||
waypoints = [
|
||||
(4.0, 1.0), # départ
|
||||
(4.0, 4.5), # avance Nord
|
||||
(1.5, 4.5), # tourne Ouest
|
||||
(1.5, 7.5), # monte dans couloir
|
||||
(1.5, 5.5), # redescend (demi-tour)
|
||||
(1.5, 2.0), # descend Sud
|
||||
(5.5, 2.0), # traverse Est
|
||||
(5.5, 4.5), # remonte Nord
|
||||
(4.0, 4.5), # revient vers le milieu
|
||||
(4.0, 1.2), # retour près du départ (fermeture boucle)
|
||||
]
|
||||
|
||||
records = []
|
||||
t = 0.0
|
||||
x, y = waypoints[0]
|
||||
heading_deg = 0.0 # Nord au départ
|
||||
|
||||
for target_x, target_y in waypoints[1:]:
|
||||
# Direction vers la cible
|
||||
dx = target_x - x
|
||||
dy = target_y - y
|
||||
dist = np.sqrt(dx**2 + dy**2)
|
||||
if dist < 0.01:
|
||||
continue
|
||||
|
||||
# Cap cible (convention 0°=Nord, 90°=Est)
|
||||
target_heading = np.rad2deg(np.arctan2(dx, dy)) % 360
|
||||
|
||||
# Phase 1 : rotation sur place vers le cap cible
|
||||
dh = ((target_heading - heading_deg + 180) % 360) - 180
|
||||
turn_steps = max(1, int(abs(dh) / (ROV_TURN_RATE * dt)))
|
||||
for j in range(turn_steps):
|
||||
heading_deg += np.sign(dh) * ROV_TURN_RATE * dt
|
||||
heading_deg %= 360
|
||||
records.append([t, x, y, heading_deg])
|
||||
t += dt
|
||||
|
||||
heading_deg = target_heading
|
||||
|
||||
# Phase 2 : avance vers la cible à vitesse constante
|
||||
travel_time = dist / ROV_SPEED
|
||||
move_steps = max(1, int(travel_time / dt))
|
||||
for j in range(move_steps):
|
||||
frac = (j + 1) / move_steps
|
||||
x_cur = x + dx * frac
|
||||
y_cur = y + dy * frac
|
||||
records.append([t, x_cur, y_cur, heading_deg])
|
||||
t += dt
|
||||
|
||||
x, y = target_x, target_y
|
||||
|
||||
return np.array(records)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Générateur principal
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def simulate(
|
||||
out_dir: str = "../data/sim",
|
||||
sweep_period: float = 4.0,
|
||||
dt: float = 0.05,
|
||||
rng_seed: int = 42,
|
||||
) -> None:
|
||||
rng = np.random.default_rng(rng_seed)
|
||||
os.makedirs(out_dir, exist_ok=True)
|
||||
|
||||
jsonl_path = os.path.join(out_dir, "run_L.jsonl")
|
||||
truth_path = os.path.join(out_dir, "run_L_truth.csv")
|
||||
|
||||
# Trajectoire vraie (lisse)
|
||||
traj = smooth_trajectory(dt=dt)
|
||||
N = len(traj)
|
||||
print(f"Trajectoire : {N} pas, durée totale {traj[-1, 0]:.1f} s")
|
||||
|
||||
# Vitesse en repère corps (forward, lateral)
|
||||
# On dérive la position puis projette dans le repère corps
|
||||
px = traj[:, 1]
|
||||
py = traj[:, 2]
|
||||
heading_true = traj[:, 3]
|
||||
|
||||
# Vitesses monde
|
||||
vx_world = np.gradient(px, traj[:, 0])
|
||||
vy_world = np.gradient(py, traj[:, 0])
|
||||
|
||||
# Accélérations monde → lisser (ROV a inertie)
|
||||
ax_world_raw = np.gradient(vx_world, traj[:, 0])
|
||||
ay_world_raw = np.gradient(vy_world, traj[:, 0])
|
||||
|
||||
# Filtrage léger des pics aux transitions (moyenne glissante 5 pts)
|
||||
from numpy.lib.stride_tricks import sliding_window_view
|
||||
def smooth5(arr):
|
||||
out = arr.copy()
|
||||
out[2:-2] = sliding_window_view(arr, 5).mean(axis=1)
|
||||
return out
|
||||
|
||||
ax_world = smooth5(ax_world_raw)
|
||||
ay_world = smooth5(ay_world_raw)
|
||||
|
||||
# Projection accélération monde → corps
|
||||
h_rad_arr = np.deg2rad(heading_true)
|
||||
sin_h = np.sin(h_rad_arr)
|
||||
cos_h = np.cos(h_rad_arr)
|
||||
|
||||
# Convention : avant ROV = (sin h, cos h) en ENU (cap 0°=+Y Nord)
|
||||
# axe corps X = avant, axe corps Y = gauche
|
||||
ax_body_true = ax_world * sin_h + ay_world * cos_h
|
||||
ay_body_true = -ax_world * cos_h + ay_world * sin_h
|
||||
|
||||
# Vitesse corps vraie (forward, lateral)
|
||||
# vx_world, vy_world déjà calculés — projection dans repère corps
|
||||
vf_true = vx_world * sin_h + vy_world * cos_h # avant
|
||||
vl_true = -vx_world * cos_h + vy_world * sin_h # latéral
|
||||
|
||||
# Dérivée de heading pour le gyro
|
||||
dh_arr = np.gradient(heading_true, traj[:, 0])
|
||||
# court chemin sur la dérivée angulaire
|
||||
dh_arr = ((dh_arr + 180) % 360) - 180
|
||||
gz_true_arr = np.deg2rad(dh_arr)
|
||||
|
||||
# Ping360 : balayage continu
|
||||
# step_per_dt = (PING360_STEPS / sweep_period) * dt → degrés par step de traj
|
||||
step_deg_per_dt = (PING360_STEPS / sweep_period) * dt * DEG_PER_STEP
|
||||
current_ping_angle = 0.0
|
||||
|
||||
# Dérive magnéto cumulative
|
||||
heading_drift = 0.0
|
||||
|
||||
# Origine = position initiale du ROV (pour que le DR (0,0) corresponde à la vérité)
|
||||
x0_room = traj[0, 1]
|
||||
y0_room = traj[0, 2]
|
||||
|
||||
records = []
|
||||
truth_rows = []
|
||||
|
||||
for i in range(N):
|
||||
t_i = traj[i, 0]
|
||||
x_i = traj[i, 1]
|
||||
y_i = traj[i, 2]
|
||||
h_i = heading_true[i]
|
||||
|
||||
# --- heading mesuré (magnéto) ---
|
||||
heading_drift += HEADING_DRIFT_RATE * dt
|
||||
h_meas = (h_i + rng.normal(0, HEADING_NOISE_DEG) + heading_drift) % 360
|
||||
|
||||
# --- angle Ping360 (relatif au ROV) ---
|
||||
ping_angle_rel = current_ping_angle % 360.0
|
||||
current_ping_angle += step_deg_per_dt
|
||||
|
||||
# angle monde = cap vrai + angle_relatif
|
||||
ping_angle_world = (h_i + ping_angle_rel) % 360.0
|
||||
|
||||
# --- raycast → distance vraie ---
|
||||
d_true = range_to_walls(x_i, y_i, ping_angle_world)
|
||||
|
||||
# --- bruit + dropout ---
|
||||
if d_true is None or rng.random() < PING360_DROPOUT_PROB:
|
||||
ping_dist = 0.0
|
||||
else:
|
||||
ping_dist = max(0.0, d_true + rng.normal(0, PING360_NOISE_M))
|
||||
|
||||
# --- IMU ---
|
||||
# Accélération corps vraie + biais + bruit
|
||||
ax_m = ax_body_true[i] + IMU_ACCEL_BIAS_F + rng.normal(0, IMU_ACCEL_NOISE)
|
||||
ay_m = ay_body_true[i] + IMU_ACCEL_BIAS_L + rng.normal(0, IMU_ACCEL_NOISE)
|
||||
az_m = GRAVITY + rng.normal(0, IMU_ACCEL_NOISE)
|
||||
|
||||
# Gyro lacet : taux de rotation vrai + biais + bruit
|
||||
gz_m = gz_true_arr[i] + IMU_GYRO_BIAS + rng.normal(0, IMU_GYRO_NOISE)
|
||||
gx_m = rng.normal(0, IMU_GYRO_NOISE)
|
||||
gy_m = rng.normal(0, IMU_GYRO_NOISE)
|
||||
|
||||
# --- altitude (Ping1D) ---
|
||||
if rng.random() < ALTITUDE_DROPOUT_PROB:
|
||||
alt_m = 0.0
|
||||
else:
|
||||
alt_m = max(0.0, ROV_ALT + rng.normal(0, ALTITUDE_NOISE_M))
|
||||
|
||||
# --- profondeur ---
|
||||
depth_m = max(0.0, ROV_Z + rng.normal(0, DEPTH_NOISE_M))
|
||||
|
||||
# --- vitesse corps (thruster controller / DVL simplifié) ---
|
||||
# Bruit de vitesse : erreur ~3% de la vitesse max (0.3 m/s) = ~0.01 m/s
|
||||
# Biais vitesse : représente les courants / asymétrie thrusters
|
||||
VF_BIAS = 0.02 # m/s biais forward constant
|
||||
VF_NOISE = 0.015 # m/s bruit mesure vitesse
|
||||
vf_m = float(vf_true[i]) + VF_BIAS + rng.normal(0, VF_NOISE)
|
||||
vl_m = float(vl_true[i]) + rng.normal(0, VF_NOISE * 0.5)
|
||||
|
||||
record = {
|
||||
"t": round(float(t_i), 4),
|
||||
"ping360_angle": round(float(ping_angle_rel), 2),
|
||||
"ping360_distance": round(float(ping_dist), 4),
|
||||
"heading": round(float(h_meas), 3),
|
||||
"altitude": round(float(alt_m), 4),
|
||||
"depth": round(float(depth_m), 4),
|
||||
"ax": round(float(ax_m), 5),
|
||||
"ay": round(float(ay_m), 5),
|
||||
"az": round(float(az_m), 5),
|
||||
"gx": round(float(gx_m), 6),
|
||||
"gy": round(float(gy_m), 6),
|
||||
"gz": round(float(gz_m), 6),
|
||||
"vf": round(float(vf_m), 4), # vitesse corps forward (m/s) — thruster model
|
||||
"vl": round(float(vl_m), 4), # vitesse corps latérale (m/s)
|
||||
}
|
||||
records.append(record)
|
||||
# Vérité en coordonnées relatives au départ (même repère que le DR qui commence à 0,0)
|
||||
truth_rows.append([float(t_i), float(x_i - x0_room), float(y_i - y0_room), float(h_i), ROV_Z])
|
||||
|
||||
# --- Écriture JSONL ---
|
||||
with open(jsonl_path, "w") as f:
|
||||
for rec in records:
|
||||
f.write(json.dumps(rec) + "\n")
|
||||
|
||||
# --- Vérité terrain CSV ---
|
||||
with open(truth_path, "w", newline="") as f:
|
||||
writer = csv.writer(f)
|
||||
writer.writerow(["t", "x", "y", "heading_deg", "z"])
|
||||
for row in truth_rows:
|
||||
writer.writerow([f"{v:.6f}" for v in row])
|
||||
|
||||
size_mb = os.path.getsize(jsonl_path) / 1e6
|
||||
print(f"JSONL : {jsonl_path} ({size_mb:.2f} Mo, {len(records)} lignes)")
|
||||
print(f"Truth : {truth_path} ({len(truth_rows)} lignes)")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# CLI
|
||||
# ---------------------------------------------------------------------------
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Simulateur capteurs moulin-mapper")
|
||||
parser.add_argument("--out-dir", default="../data/sim")
|
||||
parser.add_argument("--sweep-period", type=float, default=4.0,
|
||||
help="Durée d'un sweep Ping360 complet (s)")
|
||||
parser.add_argument("--dt", type=float, default=0.05,
|
||||
help="Pas de temps simulation (s)")
|
||||
parser.add_argument("--seed", type=int, default=42)
|
||||
args = parser.parse_args()
|
||||
|
||||
simulate(
|
||||
out_dir=args.out_dir,
|
||||
sweep_period=args.sweep_period,
|
||||
dt=args.dt,
|
||||
rng_seed=args.seed,
|
||||
)
|
||||
Reference in New Issue
Block a user