sonar-vision 0.1.0__py3-none-any.whl

sonar_vision/__init__.py

@@ -0,0 +1,22 @@
+"""Sonar Vision — signal processing and spatial awareness for agents."""
+
+from sonar_vision.signal import Signal
+from sonar_vision.sonar import Sonar, PingResult, SOUND_SPEED_WATER
+from sonar_vision.map import SpatialMap, Obstacle, CellState
+from sonar_vision.tracker import ObjectTracker, Track, Detection
+from sonar_vision.display import SonarDisplay
+
+__all__ = [
+    "Signal",
+    "Sonar",
+    "PingResult",
+    "SOUND_SPEED_WATER",
+    "SpatialMap",
+    "Obstacle",
+    "CellState",
+    "ObjectTracker",
+    "Track",
+    "Detection",
+    "SonarDisplay",
+]
+__version__ = "0.1.0"

sonar_vision/display.py

@@ -0,0 +1,165 @@
+"""SonarDisplay — ASCII radar rendering for sonar data."""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+from typing import Optional
+
+from sonar_vision.map import SpatialMap, CellState, Obstacle
+from sonar_vision.tracker import ObjectTracker, Track
+
+
+# ASCII intensity ramp for signal strength / proximity
+_RAMP = " .:-=+*#%@"
+_RAMP_LEN = len(_RAMP)
+
+
+@dataclass
+class SonarDisplay:
+    """ASCII-based sonar radar display.
+
+    Renders a top-down polar or Cartesian view using characters.
+
+    Attributes:
+        width: Display width in characters.
+        height: Display height in characters.
+        range_m: Maximum display range in meters.
+    """
+
+    width: int = 41
+    height: int = 21
+    range_m: float = 50.0
+
+    # ── radar sweep ───────────────────────────────────────────────
+
+    def render_sweep(
+        self,
+        bearing_angles: list[float],
+        distances: list[float],
+        max_range: float | None = None,
+    ) -> str:
+        """Render a radar sweep from bearing/distance readings.
+
+        Each (bearing, distance) pair places a blip on the display.
+        """
+        rng = max_range or self.range_m
+        grid: list[list[str]] = [
+            [" " for _ in range(self.width)] for _ in range(self.height)
+        ]
+        cx = self.width // 2
+        cy = self.height // 2
+
+        # Draw crosshairs
+        for x in range(self.width):
+            grid[cy][x] = "·" if grid[cy][x] == " " else grid[cy][x]
+        for y in range(self.height):
+            grid[y][cx] = "·" if grid[y][cx] == " " else grid[y][cx]
+
+        # Range rings (25%, 50%, 75%)
+        for frac in (0.25, 0.5, 0.75):
+            r_cells = int(frac * min(cx, cy))
+            for angle_deg in range(0, 360, 5):
+                ar = math.radians(angle_deg)
+                px = cx + int(r_cells * math.cos(ar))
+                py = cy - int(r_cells * math.sin(ar))
+                if 0 <= px < self.width and 0 <= py < self.height:
+                    grid[py][px] = "·"
+
+        # Centre marker (draw after range rings to avoid overwrite)
+        grid[cy][cx] = "+"
+
+        # Plot detections
+        for bearing, dist in zip(bearing_angles, distances):
+            if dist > rng or dist <= 0:
+                continue
+            ar = math.radians(bearing)
+            r_norm = dist / rng
+            px = cx + int(r_norm * cx * math.cos(ar))
+            py = cy - int(r_norm * cy * math.sin(ar))
+            if 0 <= px < self.width and 0 <= py < self.height:
+                intensity = int((1.0 - r_norm) * (_RAMP_LEN - 1))
+                intensity = max(0, min(_RAMP_LEN - 1, intensity))
+                grid[py][px] = _RAMP[intensity]
+
+        lines = ["".join(row) for row in grid]
+        return "\n".join(lines)
+
+    # ── occupancy map ─────────────────────────────────────────────
+
+    def render_map(self, smap: SpatialMap) -> str:
+        """Render a SpatialMap as ASCII."""
+        symbols = {CellState.UNKNOWN: " ", CellState.FREE: ".", CellState.OCCUPIED: "#"}
+        # Downsample map to display size
+        row_scale = smap.rows / self.height
+        col_scale = smap.cols / self.width
+
+        lines: list[str] = []
+        for dr in range(self.height):
+            row_start = int(dr * row_scale)
+            row_end = min(smap.rows, int((dr + 1) * row_scale))
+            line_chars: list[str] = []
+            for dc in range(self.width):
+                col_start = int(dc * col_scale)
+                col_end = min(smap.cols, int((dc + 1) * col_scale))
+                # Pick most interesting cell in block
+                cell = CellState.UNKNOWN
+                for r in range(row_start, row_end):
+                    for c in range(col_start, col_end):
+                        s = smap._grid[r][c]
+                        if s == CellState.OCCUPIED:
+                            cell = s
+                        elif s == CellState.FREE and cell == CellState.UNKNOWN:
+                            cell = s
+                line_chars.append(symbols[cell])
+            lines.append("".join(line_chars))
+        return "\n".join(lines)
+
+    # ── tracker overlay ───────────────────────────────────────────
+
+    def render_tracks(
+        self,
+        tracker: ObjectTracker,
+        cx_m: float = 0.0,
+        cy_m: float = 0.0,
+        now: float | None = None,
+    ) -> str:
+        """Render active tracks relative to a centre point."""
+        grid: list[list[str]] = [
+            [" " for _ in range(self.width)] for _ in range(self.height)
+        ]
+        cx = self.width // 2
+        cy = self.height // 2
+
+        # Crosshairs
+        for x in range(self.width):
+            grid[cy][x] = "·"
+        for y in range(self.height):
+            grid[y][cx] = "·"
+        grid[cy][cx] = "+"
+
+        active = tracker.active_tracks(now)
+        for t in active:
+            dx = t.x - cx_m
+            dy = t.y - cy_m
+            px = cx + int(dx / self.range_m * cx)
+            py = cy - int(dy / self.range_m * cy)
+            if 0 <= px < self.width and 0 <= py < self.height:
+                # Draw track marker and velocity vector
+                marker = str(t.track_id % 10) if t.track_id < 10 else "*"
+                grid[py][px] = marker
+                # Velocity indicator
+                vscale = self.range_m / (tracker.max_velocity or 1.0) * 2
+                vx_px = int(t.vx * vscale / self.range_m * cx)
+                vy_px = int(t.vy * vscale / self.range_m * cy)
+                ex = px + vx_px
+                ey = py - vy_px
+                # Simple line
+                steps = max(abs(ex - px), abs(ey - py), 1)
+                for i in range(1, steps + 1):
+                    lx = px + (ex - px) * i // steps
+                    ly = py + (ey - py) * i // steps
+                    if 0 <= lx < self.width and 0 <= ly < self.height and grid[ly][lx] == " ":
+                        grid[ly][lx] = "~"
+
+        return "\n".join("".join(row) for row in grid)

sonar_vision/map.py

@@ -0,0 +1,230 @@
+"""SpatialMap — obstacle detection, mapping, and spatial queries."""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+from enum import Enum
+from typing import Optional
+
+
+class CellState(Enum):
+    """State of a grid cell."""
+    UNKNOWN = 0
+    FREE = 1
+    OCCUPIED = 2
+
+
+@dataclass
+class Obstacle:
+    """Detected obstacle.
+
+    Attributes:
+        x: X-coordinate in meters.
+        y: Y-coordinate in meters.
+        radius: Approximate radius in meters (0 for point obstacles).
+        confidence: Detection confidence (0–1).
+        label: Optional label.
+    """
+
+    x: float
+    y: float
+    radius: float = 0.0
+    confidence: float = 1.0
+    label: str = ""
+
+
+@dataclass
+class SpatialMap:
+    """2-D occupancy grid map with obstacle management.
+
+    The map covers a rectangular region centred at the origin.
+
+    Attributes:
+        width: Map width in meters.
+        height: Map height in meters.
+        resolution: Cell size in meters.
+    """
+
+    width: float = 100.0
+    height: float = 100.0
+    resolution: float = 1.0
+    _obstacles: list[Obstacle] = field(default_factory=list)
+    _grid: list[list[CellState]] = field(init=False, repr=False)
+
+    def __post_init__(self) -> None:
+        cols = max(1, int(self.width / self.resolution))
+        rows = max(1, int(self.height / self.resolution))
+        self._grid = [[CellState.UNKNOWN for _ in range(cols)] for _ in range(rows)]
+
+    # ── grid helpers ──────────────────────────────────────────────
+
+    @property
+    def cols(self) -> int:
+        return len(self._grid[0]) if self._grid else 0
+
+    @property
+    def rows(self) -> int:
+        return len(self._grid)
+
+    def _world_to_cell(self, x: float, y: float) -> tuple[int, int]:
+        """Convert world coords to cell (row, col)."""
+        col = int((x + self.width / 2.0) / self.resolution)
+        row = int((y + self.height / 2.0) / self.resolution)
+        return row, col
+
+    def _cell_to_world(self, row: int, col: int) -> tuple[float, float]:
+        """Convert cell (row, col) to world coords (centre of cell)."""
+        x = (col + 0.5) * self.resolution - self.width / 2.0
+        y = (row + 0.5) * self.resolution - self.height / 2.0
+        return x, y
+
+    def _in_bounds(self, row: int, col: int) -> bool:
+        return 0 <= row < self.rows and 0 <= col < self.cols
+
+    # ── cell access ───────────────────────────────────────────────
+
+    def get_cell(self, x: float, y: float) -> CellState:
+        """Get the state of the cell at world coordinates (x, y)."""
+        row, col = self._world_to_cell(x, y)
+        if not self._in_bounds(row, col):
+            return CellState.UNKNOWN
+        return self._grid[row][col]
+
+    def set_cell(self, x: float, y: float, state: CellState) -> None:
+        """Set the state of the cell at world coordinates (x, y)."""
+        row, col = self._world_to_cell(x, y)
+        if self._in_bounds(row, col):
+            self._grid[row][col] = state
+
+    # ── obstacles ─────────────────────────────────────────────────
+
+    def add_obstacle(self, obstacle: Obstacle) -> None:
+        """Add an obstacle and mark its cells as occupied."""
+        self._obstacles.append(obstacle)
+        # Mark cells within the obstacle radius (at least the center cell)
+        cx, cy = obstacle.x, obstacle.y
+        r_cells = max(1, int(math.ceil(obstacle.radius / self.resolution))) if obstacle.radius > 0 else 0
+        center_row, center_col = self._world_to_cell(cx, cy)
+        # Always mark the center cell
+        if self._in_bounds(center_row, center_col):
+            self._grid[center_row][center_col] = CellState.OCCUPIED
+        for dr in range(-r_cells, r_cells + 1):
+            for dc in range(-r_cells, r_cells + 1):
+                nr, nc = center_row + dr, center_col + dc
+                if self._in_bounds(nr, nc):
+                    wx, wy = self._cell_to_world(nr, nc)
+                    if obstacle.radius <= 0 or math.hypot(wx - cx, wy - cy) <= obstacle.radius:
+                        self._grid[nr][nc] = CellState.OCCUPIED
+
+    def remove_obstacle(self, index: int) -> None:
+        """Remove obstacle by index and clear its cells."""
+        if 0 <= index < len(self._obstacles):
+            obs = self._obstacles.pop(index)
+            self._mark_area_free(obs)
+
+    def _mark_area_free(self, obs: Obstacle) -> None:
+        r_cells = max(1, int(math.ceil(obs.radius / self.resolution))) if obs.radius > 0 else 1
+        center_row, center_col = self._world_to_cell(obs.x, obs.y)
+        for dr in range(-r_cells, r_cells + 1):
+            for dc in range(-r_cells, r_cells + 1):
+                nr, nc = center_row + dr, center_col + dc
+                if self._in_bounds(nr, nc):
+                    wx, wy = self._cell_to_world(nr, nc)
+                    if obs.radius <= 0 or math.hypot(wx - obs.x, wy - obs.y) <= obs.radius:
+                        self._grid[nr][nc] = CellState.FREE
+
+    @property
+    def obstacles(self) -> list[Obstacle]:
+        return list(self._obstacles)
+
+    def clear(self) -> None:
+        """Clear all obstacles and reset grid."""
+        self._obstacles.clear()
+        for r in range(self.rows):
+            for c in range(self.cols):
+                self._grid[r][c] = CellState.UNKNOWN
+
+    # ── queries ───────────────────────────────────────────────────
+
+    def is_occupied(self, x: float, y: float) -> bool:
+        """Check if position (x, y) is occupied."""
+        return self.get_cell(x, y) == CellState.OCCUPIED
+
+    def is_free(self, x: float, y: float) -> bool:
+        """Check if position (x, y) is known free."""
+        return self.get_cell(x, y) == CellState.FREE
+
+    def nearest_obstacle(self, x: float, y: float) -> Optional[Obstacle]:
+        """Return the nearest obstacle to (x, y), or None."""
+        if not self._obstacles:
+            return None
+        return min(self._obstacles, key=lambda o: math.hypot(o.x - x, o.y - y))
+
+    def obstacles_in_radius(self, x: float, y: float, radius: float) -> list[Obstacle]:
+        """Return obstacles within *radius* meters of (x, y)."""
+        return [o for o in self._obstacles if math.hypot(o.x - x, o.y - y) <= radius]
+
+    def distance_to_nearest(self, x: float, y: float) -> float:
+        """Distance to the nearest occupied cell or obstacle."""
+        obs = self.nearest_obstacle(x, y)
+        if obs is None:
+            return float("inf")
+        return math.hypot(obs.x - x, obs.y - y)
+
+    # ── ray casting ───────────────────────────────────────────────
+
+    def ray_cast(self, ox: float, oy: float, angle_deg: float, max_dist: float) -> Optional[tuple[float, float]]:
+        """Cast a ray from (ox, oy) at *angle_deg* and return first hit (x, y) or None.
+
+        Uses DDA-style stepping at cell resolution.
+        """
+        angle_rad = math.radians(angle_deg)
+        dx = math.cos(angle_rad) * self.resolution * 0.5
+        dy = math.sin(angle_rad) * self.resolution * 0.5
+        steps = int(max_dist / (self.resolution * 0.5))
+        cx, cy = ox, oy
+        for _ in range(steps):
+            if self.is_occupied(cx, cy):
+                return (cx, cy)
+            # Also check out of bounds
+            row, col = self._world_to_cell(cx, cy)
+            if not self._in_bounds(row, col):
+                return None
+            cx += dx
+            cy += dy
+        return None
+
+    # ── statistics ────────────────────────────────────────────────
+
+    def occupancy_count(self) -> dict[CellState, int]:
+        """Count cells by state."""
+        counts = {s: 0 for s in CellState}
+        for row in self._grid:
+            for cell in row:
+                counts[cell] += 1
+        return counts
+
+    def coverage(self) -> float:
+        """Fraction of map that is explored (FREE + OCCUPIED)."""
+        total = self.rows * self.cols
+        if total == 0:
+            return 0.0
+        counts = self.occupancy_count()
+        explored = counts[CellState.FREE] + counts[CellState.OCCUPIED]
+        return explored / total
+
+    # ── mark free from sonar sweep ────────────────────────────────
+
+    def mark_free_ray(self, ox: float, oy: float, angle_deg: float, distance: float) -> None:
+        """Mark cells along a ray as FREE up to *distance* meters."""
+        angle_rad = math.radians(angle_deg)
+        steps = max(1, int(distance / self.resolution))
+        for i in range(steps):
+            t = (i + 0.5) * self.resolution
+            if t > distance:
+                break
+            wx = ox + t * math.cos(angle_rad)
+            wy = oy + t * math.sin(angle_rad)
+            if self.get_cell(wx, wy) != CellState.OCCUPIED:
+                self.set_cell(wx, wy, CellState.FREE)

sonar_vision/signal.py

@@ -0,0 +1,206 @@
+"""Signal processing — sampling, filtering, and frequency analysis."""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+from typing import Sequence
+
+
+@dataclass
+class Signal:
+    """A discrete-time signal with uniform sampling.
+
+    Attributes:
+        samples: Amplitude values.
+        sample_rate: Samples per second (Hz).
+    """
+
+    samples: list[float] = field(default_factory=list)
+    sample_rate: float = 44100.0
+
+    # ── constructors ──────────────────────────────────────────────
+
+    @classmethod
+    def sine(cls, frequency: float, duration: float, sample_rate: float = 44100.0, amplitude: float = 1.0) -> Signal:
+        """Generate a sine-wave signal."""
+        n = int(sample_rate * duration)
+        samples = [amplitude * math.sin(2.0 * math.pi * frequency * i / sample_rate) for i in range(n)]
+        return cls(samples=samples, sample_rate=sample_rate)
+
+    @classmethod
+    def noise(cls, duration: float, sample_rate: float = 44100.0, amplitude: float = 1.0, seed: int | None = None) -> Signal:
+        """Generate uniform random noise."""
+        import random
+        rng = random.Random(seed)
+        n = int(sample_rate * duration)
+        samples = [amplitude * (rng.random() * 2.0 - 1.0) for _ in range(n)]
+        return cls(samples=samples, sample_rate=sample_rate)
+
+    @classmethod
+    def chirp(cls, f0: float, f1: float, duration: float, sample_rate: float = 44100.0, amplitude: float = 1.0) -> Signal:
+        """Generate a linear chirp from *f0* to *f1* Hz."""
+        n = int(sample_rate * duration)
+        rate = (f1 - f0) / duration
+        samples = [
+            amplitude * math.sin(2.0 * math.pi * (f0 * t + 0.5 * rate * t * t))
+            for t in (i / sample_rate for i in range(n))
+        ]
+        return cls(samples=samples, sample_rate=sample_rate)
+
+    # ── properties ────────────────────────────────────────────────
+
+    @property
+    def duration(self) -> float:
+        """Signal duration in seconds."""
+        return len(self.samples) / self.sample_rate if self.sample_rate else 0.0
+
+    @property
+    def n_samples(self) -> int:
+        return len(self.samples)
+
+    # ── sampling / resampling ─────────────────────────────────────
+
+    def resample(self, target_rate: float) -> Signal:
+        """Linear-interpolation resample to *target_rate*."""
+        if target_rate <= 0 or not self.samples:
+            return Signal(samples=[], sample_rate=target_rate)
+        ratio = self.sample_rate / target_rate
+        new_n = max(1, int(len(self.samples) / ratio))
+        new_samples: list[float] = []
+        for i in range(new_n):
+            pos = i * ratio
+            idx = int(pos)
+            frac = pos - idx
+            if idx + 1 < len(self.samples):
+                new_samples.append(self.samples[idx] * (1.0 - frac) + self.samples[idx + 1] * frac)
+            else:
+                new_samples.append(self.samples[min(idx, len(self.samples) - 1)])
+        return Signal(samples=new_samples, sample_rate=target_rate)
+
+    # ── filtering ─────────────────────────────────────────────────
+
+    def lowpass(self, cutoff: float, order: int = 5) -> Signal:
+        """Simple moving-average low-pass filter (approximation).
+
+        Window size is derived from *cutoff* relative to Nyquist.
+        """
+        nyquist = self.sample_rate / 2.0
+        if cutoff <= 0 or cutoff >= nyquist:
+            return Signal(samples=list(self.samples), sample_rate=self.sample_rate)
+        window = max(1, int(nyquist / cutoff) * order)
+        window = min(window, len(self.samples))
+        if window <= 1:
+            return Signal(samples=list(self.samples), sample_rate=self.sample_rate)
+        out: list[float] = []
+        half = window // 2
+        for i in range(len(self.samples)):
+            start = max(0, i - half)
+            end = min(len(self.samples), i + half + 1)
+            out.append(sum(self.samples[start:end]) / (end - start))
+        return Signal(samples=out, sample_rate=self.sample_rate)
+
+    def highpass(self, cutoff: float, order: int = 5) -> Signal:
+        """Cascaded high-pass by subtracting low-pass from original."""
+        lp = self.lowpass(cutoff, order)
+        return Signal(
+            samples=[s - l for s, l in zip(self.samples, lp.samples)],
+            sample_rate=self.sample_rate,
+        )
+
+    def bandpass(self, low_cutoff: float, high_cutoff: float, order: int = 5) -> Signal:
+        """Band-pass via lowpass then highpass."""
+        return self.lowpass(high_cutoff, order).highpass(low_cutoff, order)
+
+    def threshold(self, level: float) -> Signal:
+        """Hard threshold — samples below *level* in absolute value become 0."""
+        return Signal(
+            samples=[s if abs(s) >= level else 0.0 for s in self.samples],
+            sample_rate=self.sample_rate,
+        )
+
+    def envelope(self) -> Signal:
+        """Simple magnitude envelope via sliding maximum of absolute value."""
+        if not self.samples:
+            return Signal(sample_rate=self.sample_rate)
+        window = max(1, int(self.sample_rate * 0.005))  # 5 ms window
+        out: list[float] = []
+        for i in range(len(self.samples)):
+            start = max(0, i - window)
+            out.append(max(abs(s) for s in self.samples[start : i + 1]))
+        return Signal(samples=out, sample_rate=self.sample_rate)
+
+    # ── analysis ──────────────────────────────────────────────────
+
+    def rms(self) -> float:
+        """Root-mean-square amplitude."""
+        if not self.samples:
+            return 0.0
+        return math.sqrt(sum(s * s for s in self.samples) / len(self.samples))
+
+    def peak(self) -> float:
+        """Peak (max absolute) amplitude."""
+        if not self.samples:
+            return 0.0
+        return max(abs(s) for s in self.samples)
+
+    def snr_db(self, noise: Signal) -> float:
+        """Signal-to-noise ratio in dB relative to *noise*."""
+        sig_power = self.rms()
+        noise_power = noise.rms()
+        if noise_power == 0.0:
+            return float("inf") if sig_power > 0 else 0.0
+        return 20.0 * math.log10(sig_power / noise_power)
+
+    def energy(self) -> float:
+        """Total signal energy."""
+        return sum(s * s for s in self.samples)
+
+    def dominant_frequency(self) -> float:
+        """Estimate dominant frequency via zero-crossing count."""
+        if len(self.samples) < 2:
+            return 0.0
+        crossings = 0
+        for i in range(1, len(self.samples)):
+            if self.samples[i - 1] * self.samples[i] < 0:
+                crossings += 1
+        return (crossings / 2.0) * self.sample_rate / len(self.samples)
+
+    def dft_magnitude(self, n: int | None = None) -> list[float]:
+        """Compute DFT magnitude spectrum (naïve O(N²), for small signals).
+
+        Returns list of magnitudes for bins 0 … N/2.
+        """
+        N = n or len(self.samples)
+        N = min(N, len(self.samples))
+        magnitudes: list[float] = []
+        half = N // 2 + 1
+        for k in range(half):
+            re = 0.0
+            im = 0.0
+            for n_idx in range(N):
+                angle = 2.0 * math.pi * k * n_idx / N
+                re += self.samples[n_idx] * math.cos(angle)
+                im -= self.samples[n_idx] * math.sin(angle)
+            magnitudes.append(math.sqrt(re * re + im * im) / N)
+        return magnitudes
+
+    # ── arithmetic ────────────────────────────────────────────────
+
+    def __add__(self, other: Signal) -> Signal:
+        if self.sample_rate != other.sample_rate:
+            raise ValueError("Sample rates must match for addition")
+        n = max(len(self.samples), len(other.samples))
+        a = self.samples + [0.0] * (n - len(self.samples))
+        b = other.samples + [0.0] * (n - len(other.samples))
+        return Signal(samples=[x + y for x, y in zip(a, b)], sample_rate=self.sample_rate)
+
+    def __mul__(self, other: float | Signal) -> Signal:
+        if isinstance(other, Signal):
+            if self.sample_rate != other.sample_rate:
+                raise ValueError("Sample rates must match")
+            return Signal(
+                samples=[a * b for a, b in zip(self.samples, other.samples)],
+                sample_rate=self.sample_rate,
+            )
+        return Signal(samples=[s * other for s in self.samples], sample_rate=self.sample_rate)

sonar_vision/sonar.py

@@ -0,0 +1,173 @@
+"""Sonar — ping/echo simulation and distance estimation."""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+from typing import Optional
+
+from sonar_vision.signal import Signal
+
+
+# Default speed of sound in water (m/s)
+SOUND_SPEED_WATER: float = 1500.0
+SOUND_SPEED_AIR: float = 343.0
+
+
+@dataclass
+class PingResult:
+    """Result of a sonar ping.
+
+    Attributes:
+        distance: Estimated distance in meters.
+        travel_time: Round-trip time in seconds.
+        signal_strength: Received signal amplitude (0–1 normalised).
+        snr_db: Signal-to-noise ratio in decibels.
+        frequency: Ping frequency in Hz.
+    """
+
+    distance: float
+    travel_time: float
+    signal_strength: float
+    snr_db: float
+    frequency: float
+
+
+@dataclass
+class Sonar:
+    """Active sonar model with configurable medium and transducer.
+
+    Attributes:
+        sound_speed: Speed of sound in the medium (m/s).
+        frequency: Transducer centre frequency (Hz).
+        pulse_duration: Transmit pulse length (seconds).
+        max_range: Maximum detectable range (meters).
+        beam_width: Transducer beam width (degrees).
+        source_level: Transmit source level (arbitrary dB scale).
+        noise_level: Ambient noise floor (arbitrary dB scale).
+    """
+
+    sound_speed: float = SOUND_SPEED_WATER
+    frequency: float = 50000.0
+    pulse_duration: float = 0.001
+    max_range: float = 1000.0
+    beam_width: float = 30.0
+    source_level: float = 200.0
+    noise_level: float = 60.0
+
+    # ── transmit ──────────────────────────────────────────────────
+
+    def generate_ping(self, sample_rate: float = 44100.0) -> Signal:
+        """Generate a transmit pulse (tone burst)."""
+        return Signal.sine(
+            frequency=self.frequency,
+            duration=self.pulse_duration,
+            sample_rate=sample_rate,
+        )
+
+    # ── propagation ───────────────────────────────────────────────
+
+    def round_trip_time(self, distance: float) -> float:
+        """Compute round-trip travel time for a given *distance*."""
+        return 2.0 * distance / self.sound_speed
+
+    def distance_from_time(self, travel_time: float) -> float:
+        """Estimate distance from measured round-trip *travel_time*."""
+        return travel_time * self.sound_speed / 2.0
+
+    def spreading_loss(self, distance: float) -> float:
+        """Cylindrical + spherical spreading loss in dB (simplified)."""
+        if distance <= 0:
+            return 0.0
+        return 20.0 * math.log10(max(distance, 1e-12))
+
+    def absorption_loss(self, distance: float, absorption_db_km: float = 10.0) -> float:
+        """Absorption loss in dB using a constant dB/km coefficient.
+
+        The coefficient is configurable but does not vary with frequency in
+        this simplified model; pass an appropriate value for the transducer
+        frequency and water conditions.
+        """
+        return absorption_db_km * distance / 1000.0
+
+    def total_loss(self, distance: float, absorption_db_km: float = 10.0) -> float:
+        """Total one-way transmission loss (spreading + absorption) in dB."""
+        return self.spreading_loss(distance) + self.absorption_loss(distance, absorption_db_km)
+
+    # ── detect ────────────────────────────────────────────────────
+
+    def ping(self, distance: float, target_strength: float = -20.0) -> PingResult:
+        """Simulate a ping at *distance* and return detection result.
+
+        Models two-way transmission loss (outgoing plus return path) plus
+        target strength. Returns ``PingResult`` with a distance estimate
+        that includes small stochastic jitter proportional to range.
+        """
+        if distance <= 0 or distance > self.max_range:
+            return PingResult(
+                distance=float("nan"),
+                travel_time=float("nan"),
+                signal_strength=0.0,
+                snr_db=-float("inf"),
+                frequency=self.frequency,
+            )
+
+        rtt = self.round_trip_time(distance)
+        # Active sonar: sound travels to the target and back, so use two-way loss.
+        two_way_loss = 2.0 * self.total_loss(distance)
+        received_level = self.source_level - two_way_loss + target_strength
+        snr = received_level - self.noise_level
+        strength = max(0.0, min(1.0, 1.0 / (1.0 + math.exp(-(snr - 6.0) / 3.0))))
+
+        # Small measurement jitter proportional to range
+        import random
+        rng = random.Random()
+        jitter = rng.gauss(0, 0.005 * distance)
+        estimated_distance = distance + jitter
+        estimated_rtt = self.round_trip_time(estimated_distance)
+
+        return PingResult(
+            distance=estimated_distance,
+            travel_time=estimated_rtt,
+            signal_strength=strength,
+            snr_db=snr,
+            frequency=self.frequency,
+        )
+
+    def ping_return_signal(
+        self,
+        distance: float,
+        target_strength: float = -20.0,
+        sample_rate: float = 44100.0,
+    ) -> Signal:
+        """Generate a synthetic echo return signal for a target.
+
+        Returns a ``Signal`` containing transmit pulse, silence, and attenuated echo.
+        """
+        if distance <= 0 or distance > self.max_range:
+            return Signal(sample_rate=sample_rate)
+
+        tx = self.generate_ping(sample_rate)
+        rtt_samples = int(self.round_trip_time(distance) * sample_rate)
+
+        # Echo is attenuated by the two-way propagation path; target strength
+        # increases received level, so it reduces the effective loss.
+        loss_db = 2.0 * self.total_loss(distance) - target_strength
+        gain = 10.0 ** (-loss_db / 20.0)
+        gain = max(0.0, min(1.0, gain))
+        echo_samples = [s * gain for s in tx.samples]
+
+        # Build return: tx pulse → silence → echo
+        silence_len = max(0, rtt_samples - len(tx.samples))
+        combined = tx.samples + [0.0] * silence_len + echo_samples
+        return Signal(samples=combined, sample_rate=sample_rate)
+
+    # ── beam geometry ─────────────────────────────────────────────
+
+    def in_beam(self, bearing_offset_deg: float) -> bool:
+        """Check whether a target at *bearing_offset_deg* from boresight is within the beam."""
+        return abs(bearing_offset_deg) <= self.beam_width / 2.0
+
+    def beam_coverage(self) -> float:
+        """Return the angular coverage in degrees of the beam."""
+        return self.beam_width

sonar_vision/tracker.py

@@ -0,0 +1,227 @@
+"""ObjectTracker — multi-object tracking with motion prediction."""
+
+from __future__ import annotations
+
+import math
+import time as _time
+from dataclasses import dataclass, field
+from typing import Optional
+
+
+@dataclass
+class Track:
+    """A tracked object.
+
+    Attributes:
+        track_id: Unique integer ID.
+        x: Current estimated X position (m).
+        y: Current estimated Y position (m).
+        vx: Estimated X velocity (m/s).
+        vy: Estimated Y velocity (m/s).
+        last_seen: Timestamp of last update.
+        detections: Number of detections received.
+        label: Optional semantic label.
+    """
+
+    track_id: int
+    x: float = 0.0
+    y: float = 0.0
+    vx: float = 0.0
+    vy: float = 0.0
+    last_seen: float = 0.0
+    detections: int = 1
+    label: str = ""
+
+
+@dataclass
+class Detection:
+    """A single detection observation.
+
+    Attributes:
+        x: Measured X position (m).
+        y: Measured Y position (m).
+        timestamp: Observation time (epoch seconds). None means use current time.
+        confidence: Detection confidence (0–1).
+        label: Optional label.
+    """
+
+    x: float
+    y: float
+    timestamp: float | None = None
+    confidence: float = 1.0
+    label: str = ""
+
+
+@dataclass
+class ObjectTracker:
+    """Simple multi-object tracker with nearest-neighbour association.
+
+    Attributes:
+        association_gate: Maximum distance for associating a detection
+            with an existing track (meters).
+        max_velocity: Assumed maximum object velocity (m/s) — used
+            for gating and prediction clamping.
+        lost_timeout: Seconds without detection before a track is
+            considered lost.
+        next_id: Counter for assigning track IDs.
+    """
+
+    association_gate: float = 5.0
+    max_velocity: float = 10.0
+    lost_timeout: float = 5.0
+    next_id: int = 1
+    _tracks: dict[int, Track] = field(default_factory=dict)
+
+    # ── track management ──────────────────────────────────────────
+
+    @property
+    def tracks(self) -> list[Track]:
+        """Return all active tracks."""
+        return list(self._tracks.values())
+
+    def get_track(self, track_id: int) -> Optional[Track]:
+        return self._tracks.get(track_id)
+
+    def active_tracks(self, now: float | None = None) -> list[Track]:
+        """Return tracks that have been seen within *lost_timeout*."""
+        now = now or _time.time()
+        return [t for t in self._tracks.values() if (now - t.last_seen) <= self.lost_timeout]
+
+    def lost_tracks(self, now: float | None = None) -> list[Track]:
+        """Return tracks that have timed out."""
+        now = now or _time.time()
+        return [t for t in self._tracks.values() if (now - t.last_seen) > self.lost_timeout]
+
+    def remove_track(self, track_id: int) -> None:
+        self._tracks.pop(track_id, None)
+
+    def prune_lost(self, now: float | None = None) -> int:
+        """Remove all lost tracks. Returns count removed."""
+        lost = self.lost_tracks(now)
+        for t in lost:
+            del self._tracks[t.track_id]
+        return len(lost)
+
+    # ── update ────────────────────────────────────────────────────
+
+    def _create_track(self, det: Detection) -> Track:
+        tid = self.next_id
+        self.next_id += 1
+        t = Track(
+            track_id=tid,
+            x=det.x,
+            y=det.y,
+            last_seen=det.timestamp,
+            label=det.label,
+        )
+        self._tracks[tid] = t
+        return t
+
+    def _update_track(self, track: Track, det: Detection) -> None:
+        dt = det.timestamp - track.last_seen
+        if dt > 0:
+            new_vx = (det.x - track.x) / dt
+            new_vy = (det.y - track.y) / dt
+            # Clamp velocity
+            speed = math.hypot(new_vx, new_vy)
+            if speed > self.max_velocity:
+                scale = self.max_velocity / speed
+                new_vx *= scale
+                new_vy *= scale
+            # Exponential smoothing on velocity
+            alpha = 0.5
+            track.vx = alpha * new_vx + (1.0 - alpha) * track.vx
+            track.vy = alpha * new_vy + (1.0 - alpha) * track.vy
+        track.x = det.x
+        track.y = det.y
+        track.last_seen = det.timestamp
+        track.detections += 1
+
+    def update(self, detections: list[Detection], now: float | None = None) -> list[int]:
+        """Process a batch of detections.
+
+        Returns list of track IDs that were updated or created.
+        """
+        now = now or _time.time()
+        updated_ids: list[int] = []
+
+        # Simple greedy nearest-neighbour association
+        used_tracks: set[int] = set()
+        # Sort detections by confidence descending for greedy matching
+        sorted_dets = sorted(detections, key=lambda d: d.confidence, reverse=True)
+
+        for det in sorted_dets:
+            if det.timestamp is None:
+                det.timestamp = now
+            best_id: Optional[int] = None
+            best_dist = self.association_gate
+            for t in self._tracks.values():
+                if t.track_id in used_tracks:
+                    continue
+                # Predict position
+                dt = det.timestamp - t.last_seen
+                px = t.x + t.vx * dt
+                py = t.y + t.vy * dt
+                d = math.hypot(px - det.x, py - det.y)
+                if d < best_dist:
+                    best_dist = d
+                    best_id = t.track_id
+
+            if best_id is not None:
+                self._update_track(self._tracks[best_id], det)
+                used_tracks.add(best_id)
+                updated_ids.append(best_id)
+            else:
+                t = self._create_track(det)
+                updated_ids.append(t.track_id)
+
+        return updated_ids
+
+    # ── prediction ────────────────────────────────────────────────
+
+    def predict(self, track_id: int, dt: float) -> Optional[tuple[float, float]]:
+        """Predict position of track *track_id* *dt* seconds into the future.
+
+        Returns (x, y) or None if track doesn't exist.
+        """
+        t = self._tracks.get(track_id)
+        if t is None:
+            return None
+        return (t.x + t.vx * dt, t.y + t.vy * dt)
+
+    def predict_all(self, dt: float) -> dict[int, tuple[float, float]]:
+        """Predict positions of all tracks *dt* seconds ahead."""
+        return {
+            tid: (t.x + t.vx * dt, t.y + t.vy * dt)
+            for tid, t in self._tracks.items()
+        }
+
+    # ── queries ───────────────────────────────────────────────────
+
+    def nearest_track(self, x: float, y: float, now: float | None = None) -> Optional[Track]:
+        """Return the nearest active track to (x, y)."""
+        active = self.active_tracks(now)
+        if not active:
+            return None
+        return min(active, key=lambda t: math.hypot(t.x - x, t.y - y))
+
+    def tracks_in_radius(self, x: float, y: float, radius: float, now: float | None = None) -> list[Track]:
+        """Return active tracks within *radius* meters of (x, y)."""
+        return [
+            t for t in self.active_tracks(now)
+            if math.hypot(t.x - x, t.y - y) <= radius
+        ]
+
+    def speed(self, track_id: int) -> float:
+        """Speed of a track in m/s."""
+        t = self._tracks.get(track_id)
+        if t is None:
+            return 0.0
+        return math.hypot(t.vx, t.vy)
+
+    def heading(self, track_id: int) -> float:
+        """Heading of a track in degrees (0 = east, 90 = north)."""
+        t = self._tracks.get(track_id)
+        if t is None or (t.vx == 0 and t.vy == 0):
+            return 0.0
+        return math.degrees(math.atan2(t.vy, t.vx)) % 360.0

sonar_vision-0.1.0.dist-info/METADATA

@@ -0,0 +1,10 @@
+Metadata-Version: 2.4
+Name: sonar-vision
+Version: 0.1.0
+Summary: Pure-Python sonar ping/echo simulation, signal processing, multi-object tracking, and spatial mapping toolkit
+License: MIT
+Requires-Python: >=3.10
+License-File: LICENSE
+Provides-Extra: dev
+Requires-Dist: pytest>=7; extra == "dev"
+Dynamic: license-file

sonar_vision-0.1.0.dist-info/RECORD

@@ -0,0 +1,11 @@
+sonar_vision/__init__.py,sha256=cBkyRSBw7J8EfvvkdxtFUoDiwV--9CG69_jqe7e4pgE,584
+sonar_vision/display.py,sha256=fRsZq4P_fA6W9CqzMjOTl_jfD20eYK2uxR4n0ddaJOw,6251
+sonar_vision/map.py,sha256=ewAmIjttyMj9LRS727obNR7osHm8EhbsMAFEY2fMshc,9350
+sonar_vision/signal.py,sha256=07xj_JPIfQNXOSAJwPKggiMmsXdc7GP-XuD04AZ7PaM,8921
+sonar_vision/sonar.py,sha256=Qw-rSIaUgaobKiDu3zr1p4kQ1BJVG9U31FoBQJdzVws,6835
+sonar_vision/tracker.py,sha256=mWZFRYFdCjFu1fDVup_5z2YSeDRQUVkkiYDFo-BtF7w,8011
+sonar_vision-0.1.0.dist-info/licenses/LICENSE,sha256=J5l0Qtagxszkc1jxXLBi6hMd6dutpirm_6GQAnVMlnY,1075
+sonar_vision-0.1.0.dist-info/METADATA,sha256=8M4FRkMNFYstIcEwlIBMFdV55vUSOczs4uuJJZVZkas,317
+sonar_vision-0.1.0.dist-info/WHEEL,sha256=K260EYznzXsJYBQGqmI8VTxEdiZYNvDZwW9cBh9-_MA,91
+sonar_vision-0.1.0.dist-info/top_level.txt,sha256=NJjD5ZYQRVPR9YgDoHsXxwczqNRxAxaqOUfjmIhuScE,13
+sonar_vision-0.1.0.dist-info/RECORD,,

sonar_vision-0.1.0.dist-info/WHEEL

@@ -0,0 +1,5 @@
+Wheel-Version: 1.0
+Generator: setuptools (83.0.0)
+Root-Is-Purelib: true
+Tag: py3-none-any
+

sonar_vision-0.1.0.dist-info/licenses/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2024 SuperInstance Labs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

sonar_vision-0.1.0.dist-info/top_level.txt

@@ -0,0 +1 @@
+sonar_vision