""" 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, )