feat(simulator): virtual drone fleet data generator
One process per drone: seeded patrol kinematics, sensor streams at realistic rates, detection events, hot current/ blocks sealed into Hive-partitioned Parquet, and the compact UDP state broadcast with peer RSSI derivation. Scales via docker compose --scale.
This commit is contained in:
@@ -0,0 +1,75 @@
|
||||
"""Kinematics: a seeded patrol route and the drone's motion along it."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import random
|
||||
from dataclasses import dataclass, replace
|
||||
|
||||
from .config import Config
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Pose:
|
||||
ts_ns: int
|
||||
x: float
|
||||
y: float
|
||||
z: float
|
||||
roll: float
|
||||
pitch: float
|
||||
yaw: float
|
||||
vx: float
|
||||
vy: float
|
||||
vz: float
|
||||
|
||||
|
||||
def make_waypoints(cfg: Config, rng: random.Random) -> list[tuple[float, float]]:
|
||||
"""Perimeter patrol with per-drone jitter so drones do not overlap."""
|
||||
s = cfg.area_size_m
|
||||
jitter = rng.uniform(0.05, 0.20) * s
|
||||
corners = [(jitter, jitter), (s - jitter, jitter), (s - jitter, s - jitter), (jitter, s - jitter)]
|
||||
start = rng.randrange(4)
|
||||
return corners[start:] + corners[:start]
|
||||
|
||||
|
||||
def initial_pose(cfg: Config, rng: random.Random, ts_ns: int) -> Pose:
|
||||
wps = make_waypoints(cfg, rng)
|
||||
x, y = wps[0]
|
||||
return Pose(ts_ns=ts_ns, x=x, y=y, z=cfg.cruise_alt_m, roll=0.0, pitch=0.0, yaw=0.0, vx=0.0, vy=0.0, vz=0.0)
|
||||
|
||||
|
||||
def step(pose: Pose, target: tuple[float, float], cfg: Config, rng: random.Random, dt: float, ts_ns: int) -> tuple[Pose, bool]:
|
||||
"""Advance one tick toward the target waypoint. Returns (pose, reached)."""
|
||||
dx, dy = target[0] - pose.x, target[1] - pose.y
|
||||
dist = math.hypot(dx, dy)
|
||||
reached = dist < cfg.cruise_speed_ms * dt * 2
|
||||
|
||||
speed = cfg.cruise_speed_ms * rng.uniform(0.9, 1.1)
|
||||
if dist > 1e-6:
|
||||
vx, vy = dx / dist * speed, dy / dist * speed
|
||||
else:
|
||||
vx, vy = 0.0, 0.0
|
||||
|
||||
yaw = math.degrees(math.atan2(vy, vx))
|
||||
# Small attitude noise around the commanded values
|
||||
roll = rng.gauss(0.0, 1.5)
|
||||
pitch = rng.gauss(2.0, 1.0)
|
||||
z = cfg.cruise_alt_m + rng.gauss(0.0, 0.5)
|
||||
vz = (z - pose.z) / dt if dt > 0 else 0.0
|
||||
|
||||
return (
|
||||
replace(
|
||||
pose,
|
||||
ts_ns=ts_ns,
|
||||
x=pose.x + vx * dt,
|
||||
y=pose.y + vy * dt,
|
||||
z=z,
|
||||
roll=roll,
|
||||
pitch=pitch,
|
||||
yaw=yaw,
|
||||
vx=vx,
|
||||
vy=vy,
|
||||
vz=vz,
|
||||
),
|
||||
reached,
|
||||
)
|
||||
Reference in New Issue
Block a user