Checkpoint 7

tests/conftest.py

@@ -0,0 +1,8 @@
+"""Pytest configuration — ensure the project root is on ``sys.path``."""
+
+import os
+import sys
+
+_PROJECT_ROOT = os.path.normpath(os.path.join(os.path.dirname(__file__), ".."))
+if _PROJECT_ROOT not in sys.path:
+    sys.path.insert(0, _PROJECT_ROOT)

tests/parity_test.py

@@ -1,96 +0,0 @@
-"""Parity smoke-test for the herding env.
-
-Verifies (a) all imports resolve, (b) the env's reset/step contract is
-correct, (c) deterministic seeds give deterministic trajectories, and
-(d) the Strömbom baseline can drive the env without crashing.
-
-Run::
-
-    python -m training.parity_test
-"""
-
-from __future__ import annotations
-
-import os
-import sys
-
-_HERE = os.path.dirname(os.path.abspath(__file__))
-_PROJECT_ROOT = os.path.normpath(os.path.join(_HERE, ".."))
-if _PROJECT_ROOT not in sys.path:
-    sys.path.insert(0, _PROJECT_ROOT)
-
-import numpy as np
-
-from herding.world.geometry import MAX_SHEEP, PEN_ENTRY
-from herding.obs import OBS_DIM
-from herding.control.strombom import compute_action
-from training.herding_env import HerdingEnv
-
-
-def test_obs_action_shapes():
-    env = HerdingEnv(n_sheep=3, seed=0)
-    obs, info = env.reset()
-    assert obs.shape == (OBS_DIM,), obs.shape
-    assert obs.dtype == np.float32
-    obs2, r, term, trunc, info = env.step(np.array([0.5, 0.0], dtype=np.float32))
-    assert obs2.shape == (OBS_DIM,)
-    assert isinstance(r, float)
-    assert isinstance(term, bool) and isinstance(trunc, bool)
-    print("[ok] shapes")
-
-
-def test_reset_determinism():
-    """Reset with the same seed should give the same initial observation.
-
-    We don't require step-determinism — PPO doesn't need it, and chasing
-    bit-exactness through the flocking jitter isn't worth the complexity.
-    """
-    env_a = HerdingEnv(n_sheep=3, seed=42)
-    env_b = HerdingEnv(n_sheep=3, seed=42)
-    obs_a, _ = env_a.reset(seed=42)
-    obs_b, _ = env_b.reset(seed=42)
-    assert np.allclose(obs_a, obs_b), "Reset is non-deterministic for same seed"
-    print("[ok] reset determinism")
-
-
-def test_curriculum_n_sheep_varies():
-    env = HerdingEnv(seed=0)
-    sizes = set()
-    for _ in range(40):
-        _, info = env.reset()
-        sizes.add(info["n_sheep"])
-    assert 1 in sizes
-    assert max(sizes) <= MAX_SHEEP
-    print(f"[ok] curriculum sampling — saw n_sheep in {sorted(sizes)}")
-
-
-def test_strombom_drives_env():
-    """Quick functional check that the analytic baseline can play the env
-    without exploding. Not a success-rate test — just no errors / NaNs."""
-    env = HerdingEnv(n_sheep=2, max_steps=400, seed=1)
-    obs, _ = env.reset()
-    for t in range(400):
-        positions = {f"s{i}": (float(env.sheep_x[i]), float(env.sheep_y[i]))
-                     for i in range(env.n_sheep)
-                     if not env.sheep_penned[i]}
-        if not positions:
-            break
-        vx, vy, _mode = compute_action((env.dog_x, env.dog_y), positions, PEN_ENTRY)
-        obs, r, term, trunc, info = env.step(np.array([vx, vy], dtype=np.float32))
-        assert np.isfinite(obs).all(), f"NaN/Inf in obs at step {t}"
-        assert np.isfinite(r), f"NaN reward at step {t}"
-        if term or trunc:
-            break
-    print(f"[ok] strombom rollout — final n_penned={int(env.sheep_penned.sum())}/{env.n_sheep} after {env.steps} steps")
-
-
-def main():
-    test_obs_action_shapes()
-    test_reset_determinism()
-    test_curriculum_n_sheep_varies()
-    test_strombom_drives_env()
-    print("\nAll parity checks passed.")
-
-
-if __name__ == "__main__":
-    main()

tests/test_control.py

@@ -0,0 +1,164 @@
+"""Control primitives: speed modulation, Strömbom, Sequential, ActiveScan."""
+
+import math
+
+import pytest
+
+from herding.control.active_scan import (
+    EMPTY_DEBOUNCE_STEPS, INITIAL_SCAN_STEPS, ActiveScanTeacher,
+)
+from herding.control.modulation import (
+    MIN_SPEED, SLOW_NEAR_SHEEP, modulate_speed_near_sheep,
+)
+from herding.control.sequential import compute_action as sequential_action
+from herding.control.strombom import (
+    DELTA_DRIVE, F_FACTOR, compute_action as strombom_action,
+)
+from herding.world.geometry import PEN_ENTRY
+
+
+# ---------------------------------------------------------------------------
+# Modulation
+# ---------------------------------------------------------------------------
+
+def test_modulation_empty_input_passthrough():
+    assert modulate_speed_near_sheep(1.0, 0.0, (0.0, 0.0), []) == (1.0, 0.0)
+    assert modulate_speed_near_sheep(1.0, 0.0, (0.0, 0.0), {}) == (1.0, 0.0)
+
+
+def test_modulation_far_sheep_passthrough():
+    vx, vy = modulate_speed_near_sheep(1.0, 0.0, (0.0, 0.0), [(100.0, 0.0)])
+    assert (vx, vy) == (1.0, 0.0)
+
+
+def test_modulation_close_sheep_min_speed():
+    vx, vy = modulate_speed_near_sheep(1.0, 0.0, (0.0, 0.0), [(0.0, 0.0)])
+    assert math.isclose(vx, MIN_SPEED)
+    assert vy == 0.0
+
+
+def test_modulation_preserves_direction():
+    vx, vy = modulate_speed_near_sheep(0.6, 0.8, (0.0, 0.0), [(1.0, 0.0)])
+    ratio = math.hypot(vx, vy)
+    # Direction preserved.
+    assert math.isclose(vx / ratio, 0.6, abs_tol=1e-6)
+    assert math.isclose(vy / ratio, 0.8, abs_tol=1e-6)
+
+
+def test_modulation_linear_ramp_midpoint():
+    vx, _ = modulate_speed_near_sheep(1.0, 0.0, (0.0, 0.0),
+                                      [(SLOW_NEAR_SHEEP / 2, 0.0)])
+    expected = MIN_SPEED + (1.0 - MIN_SPEED) * 0.5
+    assert math.isclose(vx, expected, abs_tol=1e-6)
+
+
+def test_modulation_accepts_dict_input():
+    vx_list, _ = modulate_speed_near_sheep(1.0, 0.0, (0.0, 0.0),
+                                           [(1.0, 0.0)])
+    vx_dict, _ = modulate_speed_near_sheep(1.0, 0.0, (0.0, 0.0),
+                                           {"t0": (1.0, 0.0)})
+    assert math.isclose(vx_list, vx_dict)
+
+
+# ---------------------------------------------------------------------------
+# Strömbom
+# ---------------------------------------------------------------------------
+
+def test_strombom_empty_input_idle():
+    vx, vy, mode = strombom_action((0.0, 0.0), {}, PEN_ENTRY)
+    assert (vx, vy, mode) == (0.0, 0.0, "idle")
+
+
+def test_strombom_tight_flock_drives():
+    # A tight 3-sheep cluster centred at (0, 8): radius < F_FACTOR·√3.
+    sheep = {"s0": (0.0, 8.0), "s1": (0.5, 8.5), "s2": (-0.5, 8.0)}
+    vx, vy, mode = strombom_action((0.0, 0.0), sheep, PEN_ENTRY)
+    assert mode == "drive"
+    assert math.isclose(math.hypot(vx, vy), 1.0, abs_tol=1e-3)
+
+
+def test_strombom_scattered_flock_collects():
+    # Sparse, max radius > F_FACTOR·√n.
+    sheep = {"s0": (10.0, 10.0), "s1": (-10.0, -10.0), "s2": (0.0, 0.0)}
+    _vx, _vy, mode = strombom_action((0.0, 0.0), sheep, PEN_ENTRY)
+    assert mode == "collect"
+
+
+def test_strombom_ignores_already_penned_sheep():
+    """Sheep south of the gate plane are excluded from the active set."""
+    sheep = {
+        "s_active": (5.0, 5.0),
+        "s_penned": (11.5, -20.0),
+    }
+    # With one active sheep, Strömbom drives (radius = 0 < threshold).
+    _vx, _vy, mode = strombom_action((0.0, 0.0), sheep, PEN_ENTRY)
+    assert mode == "drive"
+
+
+# ---------------------------------------------------------------------------
+# Sequential
+# ---------------------------------------------------------------------------
+
+def test_sequential_empty_input_idle():
+    vx, vy, mode = sequential_action((0.0, 0.0), {}, PEN_ENTRY)
+    assert (vx, vy, mode) == (0.0, 0.0, "idle")
+
+
+def test_sequential_targets_closest_to_pen():
+    near = (10.0, -5.0)       # closer to pen entry (11.5, -15)
+    far = (-10.0, 10.0)
+    sheep = {"near": near, "far": far}
+    _vx, _vy, mode = sequential_action((0.0, 0.0), sheep, PEN_ENTRY)
+    assert mode.startswith("drive:near")
+
+
+# ---------------------------------------------------------------------------
+# ActiveScan wrapper
+# ---------------------------------------------------------------------------
+
+def test_active_scan_initial_phase_rotates():
+    teacher = ActiveScanTeacher(strombom_action)
+    # First call → opening rotation regardless of input.
+    vx, vy, mode = teacher((0.0, 0.0), 0.0, {"s0": (5.0, 0.0)}, PEN_ENTRY)
+    assert mode == "scan_initial"
+    assert math.isclose(math.hypot(vx, vy), 1.0, abs_tol=1e-6)
+
+
+def test_active_scan_hands_off_to_base_after_opener():
+    teacher = ActiveScanTeacher(strombom_action, initial_scan_steps=2)
+    # Burn through the opener.
+    for _ in range(2):
+        teacher((0.0, 0.0), 0.0, {"s0": (0.0, 8.0)}, PEN_ENTRY)
+    _vx, _vy, mode = teacher((0.0, 0.0), 0.0, {"s0": (0.0, 8.0)}, PEN_ENTRY)
+    # Either drive (Strömbom mode label) or collect; not scan_initial.
+    assert "scan" not in mode
+
+
+def test_active_scan_holds_last_action_on_brief_empty():
+    teacher = ActiveScanTeacher(strombom_action, initial_scan_steps=1)
+    # Step once (opening), then once with a visible sheep — sets last_action.
+    teacher((0.0, 0.0), 0.0, {}, PEN_ENTRY)
+    teacher((0.0, 0.0), 0.0, {"s0": (0.0, 8.0)}, PEN_ENTRY)
+    last = teacher.last_action
+    # Now a single empty frame → hold.
+    vx, vy, mode = teacher((0.0, 0.0), 0.0, {}, PEN_ENTRY)
+    assert mode == "hold"
+    assert (vx, vy) == last
+
+
+def test_active_scan_explores_after_sustained_empty():
+    teacher = ActiveScanTeacher(strombom_action, initial_scan_steps=1)
+    teacher((0.0, 0.0), 0.0, {}, PEN_ENTRY)  # opener
+    for _ in range(EMPTY_DEBOUNCE_STEPS):
+        last_vx, last_vy, mode = teacher((5.0, 5.0), 0.0, {}, PEN_ENTRY)
+    assert mode in ("explore", "scan_at_centre")
+
+
+def test_active_scan_reset_clears_state():
+    teacher = ActiveScanTeacher(strombom_action, initial_scan_steps=5)
+    for _ in range(3):
+        teacher((0.0, 0.0), 0.0, {}, PEN_ENTRY)
+    assert teacher.step == 3
+    teacher.reset()
+    assert teacher.step == 0
+    assert teacher.empty_streak == 0

tests/test_diffdrive.py

@@ -0,0 +1,84 @@
+"""Differential-drive kinematics and the (vx, vy) → wheel-speed map."""
+
+import math
+
+import pytest
+
+from herding.world.diffdrive import (
+    heading_speed_to_wheels, kinematics_step, velocity_to_wheels,
+)
+
+
+WHEEL_R = 0.038
+WHEEL_B = 0.28
+MAX_OMEGA = 70.0
+MAX_LINEAR = WHEEL_R * MAX_OMEGA
+DT = 0.016
+
+
+def test_kinematics_zero_input_is_identity():
+    x, y, h = kinematics_step(1.0, 2.0, 0.5, 0.0, 0.0, WHEEL_R, WHEEL_B, DT)
+    assert (x, y, h) == (1.0, 2.0, 0.5)
+
+
+def test_kinematics_forward_motion():
+    # Equal wheel speeds → pure translation along the heading.
+    x, y, h = kinematics_step(0.0, 0.0, 0.0, 10.0, 10.0, WHEEL_R, WHEEL_B, DT)
+    assert h == 0.0
+    assert math.isclose(x, 10.0 * WHEEL_R * DT)
+    assert y == 0.0
+
+
+def test_kinematics_pure_rotation():
+    # Opposite wheel speeds → pure rotation, position unchanged.
+    x, y, h = kinematics_step(0.0, 0.0, 0.0, -5.0, 5.0, WHEEL_R, WHEEL_B, DT)
+    assert (x, y) == (0.0, 0.0)
+    assert h > 0.0
+
+
+def test_kinematics_heading_wrapped_to_pi():
+    _, _, h = kinematics_step(0.0, 0.0, math.pi - 0.01, 100.0, -100.0,
+                              WHEEL_R, WHEEL_B, DT)
+    assert -math.pi <= h <= math.pi
+
+
+def test_velocity_to_wheels_zero_velocity():
+    left, right = velocity_to_wheels(0.0, 0.0, 0.0,
+                                     MAX_LINEAR, WHEEL_R, MAX_OMEGA)
+    assert (left, right) == (0.0, 0.0)
+
+
+def test_velocity_to_wheels_aligned_forward():
+    # Target straight ahead → equal positive wheel speeds.
+    left, right = velocity_to_wheels(1.0, 0.0, 0.0,
+                                     MAX_LINEAR, WHEEL_R, MAX_OMEGA, k_turn=4.0)
+    assert math.isclose(left, right, abs_tol=1e-6)
+    assert left > 0.0
+
+
+def test_velocity_to_wheels_perpendicular_target_spins():
+    # Target 90° from heading → forward speed ≈ 0, wheels equal-and-opposite.
+    left, right = velocity_to_wheels(0.0, 1.0, 0.0,
+                                     MAX_LINEAR, WHEEL_R, MAX_OMEGA, k_turn=4.0)
+    assert left + right == pytest.approx(0.0, abs=1e-6)
+    assert right > 0.0  # turning CCW (left of heading is +y for h=0)
+
+
+def test_velocity_to_wheels_clamped_to_max_omega():
+    # Far overshoot — both wheel commands clamped at ±MAX_OMEGA.
+    left, right = velocity_to_wheels(-1.0, 0.0, 0.0,
+                                     MAX_LINEAR, WHEEL_R, MAX_OMEGA, k_turn=20.0)
+    assert -MAX_OMEGA <= left <= MAX_OMEGA
+    assert -MAX_OMEGA <= right <= MAX_OMEGA
+
+
+def test_heading_speed_to_wheels_aligned():
+    left, right = heading_speed_to_wheels(0.0, 10.0, 0.0, MAX_OMEGA)
+    assert math.isclose(left, right, abs_tol=1e-6)
+    assert left > 0.0
+
+
+def test_heading_speed_to_wheels_reverse_target_forwards_zero():
+    left, right = heading_speed_to_wheels(math.pi, 10.0, 0.0, MAX_OMEGA)
+    # cos(π) clamped at 0 → no forward; pure rotation.
+    assert left + right == pytest.approx(0.0, abs=1e-6)

tests/test_env.py

@@ -0,0 +1,108 @@
+"""Gymnasium env: contract, determinism, reward components."""
+
+import math
+
+import numpy as np
+import pytest
+
+from herding.world.geometry import MAX_SHEEP, PEN_ENTRY
+from herding.perception.obs import OBS_DIM
+from herding.control.strombom import compute_action as strombom_action
+from training.herding_env import HerdingEnv
+
+
+def test_env_obs_action_shapes_single_frame():
+    env = HerdingEnv(n_sheep=3, seed=0, use_lidar=False)
+    obs, info = env.reset()
+    assert obs.shape == (OBS_DIM,)
+    assert obs.dtype == np.float32
+    obs, reward, term, trunc, info = env.step(
+        np.array([0.5, 0.0], dtype=np.float32))
+    assert obs.shape == (OBS_DIM,)
+    assert isinstance(reward, float)
+    assert isinstance(term, bool) and isinstance(trunc, bool)
+
+
+def test_env_observation_space_matches_frame_stack():
+    env = HerdingEnv(n_sheep=2, seed=0, use_lidar=False, frame_stack=4)
+    obs, _ = env.reset()
+    assert obs.shape == (OBS_DIM * 4,)
+    assert env.observation_space.shape == (OBS_DIM * 4,)
+
+
+def test_env_reset_determinism_same_seed():
+    a = HerdingEnv(n_sheep=3, seed=42, use_lidar=False)
+    b = HerdingEnv(n_sheep=3, seed=42, use_lidar=False)
+    obs_a, _ = a.reset(seed=42)
+    obs_b, _ = b.reset(seed=42)
+    assert np.allclose(obs_a, obs_b)
+
+
+def test_env_curriculum_samples_full_range():
+    env = HerdingEnv(seed=0, use_lidar=False)
+    sizes = set()
+    for _ in range(40):
+        _, info = env.reset()
+        sizes.add(info["n_sheep"])
+    assert 1 in sizes
+    assert max(sizes) <= MAX_SHEEP
+
+
+def test_env_step_returns_finite_values():
+    env = HerdingEnv(n_sheep=2, max_steps=200, seed=1, use_lidar=False)
+    obs, _ = env.reset()
+    for _ in range(200):
+        action = np.array([0.5, 0.5], dtype=np.float32)
+        obs, reward, term, trunc, _ = env.step(action)
+        assert np.isfinite(obs).all()
+        assert math.isfinite(reward)
+        if term or trunc:
+            break
+
+
+def test_env_options_n_sheep_overrides_curriculum():
+    env = HerdingEnv(seed=0, use_lidar=False)
+    _, info = env.reset(options={"n_sheep": 7})
+    assert info["n_sheep"] == 7
+
+
+def test_env_perceived_positions_lidar_vs_privileged():
+    env_priv = HerdingEnv(n_sheep=3, seed=0, use_lidar=False)
+    env_priv.reset(seed=0)
+    pos_priv = env_priv.perceived_positions()
+    assert len(pos_priv) == 3
+
+    env_lidar = HerdingEnv(n_sheep=3, seed=0, use_lidar=True)
+    env_lidar.reset(seed=0)
+    pos_lidar = env_lidar.perceived_positions()
+    # LiDAR mode returns whatever the tracker has — may be fewer than 3
+    # if sheep are out of FOV / range, but never more.
+    assert len(pos_lidar) <= 3
+
+
+def test_env_set_time_weight_affects_reward():
+    env = HerdingEnv(n_sheep=1, seed=0, use_lidar=False)
+    env.reset(seed=0)
+    _, r_default, *_ = env.step(np.array([0.0, 0.0], dtype=np.float32))
+    env.set_time_weight(-1.0)
+    env.reset(seed=0)
+    _, r_penalised, *_ = env.step(np.array([0.0, 0.0], dtype=np.float32))
+    assert r_penalised < r_default
+
+
+def test_env_strombom_rollout_moves_dog():
+    env = HerdingEnv(n_sheep=2, max_steps=400, seed=1, use_lidar=False)
+    env.reset()
+    start = (env.dog_x, env.dog_y)
+    for _ in range(400):
+        positions = env.perceived_positions()
+        if not positions:
+            break
+        vx, vy, _ = strombom_action(
+            (env.dog_x, env.dog_y), positions, PEN_ENTRY)
+        obs, _r, term, trunc, _ = env.step(
+            np.array([vx, vy], dtype=np.float32))
+        if term or trunc:
+            break
+    displacement = math.hypot(env.dog_x - start[0], env.dog_y - start[1])
+    assert displacement > 0.05

tests/test_geometry.py

@@ -0,0 +1,75 @@
+"""Geometric predicates and constants."""
+
+import math
+
+from herding.world.geometry import (
+    FIELD_X, FIELD_Y, GATE_X, GATE_Y, MAX_SHEEP, PEN_ENTRY, PEN_X, PEN_Y,
+    distance_to_pen_entry, in_field, in_gate_corridor, in_pen,
+    is_penned_position,
+)
+
+
+def test_field_dimensions():
+    assert FIELD_X == (-15.0, 15.0)
+    assert FIELD_Y == (-15.0, 15.0)
+
+
+def test_pen_geometry():
+    assert PEN_X == (10.0, 13.0)
+    assert PEN_Y == (-22.0, -15.0)
+    assert PEN_ENTRY == (11.5, -15.0)
+    assert GATE_X == PEN_X
+    assert GATE_Y == -15.0
+
+
+def test_in_pen_strict_interior():
+    assert in_pen(11.5, -18.0)
+    assert not in_pen(10.0, -18.0)        # boundary excluded
+    assert not in_pen(11.5, -15.0)        # gate plane excluded
+    assert not in_pen(0.0, 0.0)
+
+
+def test_in_field_with_margin():
+    assert in_field(0.0, 0.0)
+    assert in_field(14.0, 14.0)
+    assert not in_field(15.5, 0.0)
+    assert in_field(14.4, 0.0, margin=0.5)
+    assert not in_field(14.6, 0.0, margin=0.5)
+
+
+def test_in_gate_corridor():
+    assert in_gate_corridor(11.5, -18.0)
+    assert in_gate_corridor(10.0, -15.0)
+    assert not in_gate_corridor(11.5, -10.0)
+    assert not in_gate_corridor(5.0, -18.0)
+
+
+def test_is_penned_position_latches_below_gate():
+    # In the gate column and south of the gate plane → penned.
+    assert is_penned_position(11.5, -15.0)
+    assert is_penned_position(10.5, -18.0)
+    assert is_penned_position(12.5, -22.0)
+    # Above the gate plane → not yet.
+    assert not is_penned_position(11.5, -14.9)
+    # Outside the gate column → not penned even if south.
+    assert not is_penned_position(0.0, -16.0)
+    assert not is_penned_position(14.0, -16.0)
+
+
+def test_is_penned_position_latch_margin():
+    # Slight tolerance on the gate column.
+    assert is_penned_position(9.9, -15.5)
+    assert is_penned_position(13.1, -15.5)
+    assert not is_penned_position(9.7, -15.5)
+
+
+def test_distance_to_pen_entry():
+    assert distance_to_pen_entry(*PEN_ENTRY) == 0.0
+    assert math.isclose(distance_to_pen_entry(11.5, -10.0), 5.0)
+    assert math.isclose(distance_to_pen_entry(0.0, 0.0),
+                        math.hypot(11.5, 15.0))
+
+
+def test_max_sheep_positive_int():
+    assert isinstance(MAX_SHEEP, int)
+    assert MAX_SHEEP >= 1

tests/test_obs.py

@@ -0,0 +1,71 @@
+"""Observation builder — shape, normalisation, order invariance."""
+
+import math
+
+import numpy as np
+import pytest
+
+from herding.perception.obs import OBS_DIM, build_obs
+
+
+def test_obs_shape_and_dtype():
+    obs = build_obs((0.0, 0.0), 0.0, [(5.0, 5.0)], [False])
+    assert obs.shape == (OBS_DIM,)
+    assert obs.dtype == np.float32
+
+
+def test_obs_no_active_sheep_terminal():
+    # All sheep penned → flock-summary fields zero, count zero.
+    obs = build_obs((0.0, 0.0), 0.0, [(1.0, 1.0), (2.0, 2.0)], [True, True])
+    assert obs[19] == 0.0
+    # Aggregate fields (CoM, radius, std, vectors) should all be zero.
+    assert np.allclose(obs[4:12], 0.0)
+
+
+def test_obs_dog_pose_normalised():
+    obs = build_obs((15.0, -15.0), math.pi / 2, [(0.0, 0.0)], [False])
+    assert math.isclose(obs[0], 1.0)
+    assert math.isclose(obs[1], -1.0)
+    assert math.isclose(obs[2], math.cos(math.pi / 2), abs_tol=1e-6)
+    assert math.isclose(obs[3], math.sin(math.pi / 2), abs_tol=1e-6)
+
+
+def test_obs_order_invariance():
+    """Sheep order in the input list must not affect the observation."""
+    sheep = [(3.0, 2.0), (-5.0, 1.0), (0.0, 8.0)]
+    p = [False] * 3
+    a = build_obs((0.0, 0.0), 0.0, sheep, p)
+    b = build_obs((0.0, 0.0), 0.0, list(reversed(sheep)), list(reversed(p)))
+    assert np.allclose(a, b)
+
+
+def test_obs_count_field_normalised_by_n_max():
+    sheep = [(1.0, 1.0)] * 5
+    p = [False] * 5
+    obs = build_obs((0.0, 0.0), 0.0, sheep, p, n_max=10)
+    assert math.isclose(obs[19], 0.5)
+
+
+def test_obs_polar_histogram_sums_to_one():
+    sheep = [(1.0, 0.0), (-1.0, 0.0), (0.0, 1.0), (0.0, -1.0)]
+    obs = build_obs((0.0, 0.0), 0.0, sheep, [False] * 4)
+    assert math.isclose(float(obs[20:28].sum()), 1.0, abs_tol=1e-6)
+
+
+def test_obs_named_channels_closest_rearmost():
+    # Channels 28..29 = (closest_to_pen - dog) / 15
+    # Channels 30..31 = (rearmost - dog) / 15
+    pen_x, pen_y = 11.5, -15.0
+    near = (pen_x + 1.0, pen_y + 1.0)
+    far = (-10.0, 10.0)
+    obs = build_obs((0.0, 0.0), 0.0, [near, far], [False, False])
+    tol = 1e-5
+    assert math.isclose(obs[28], near[0] / 15.0, abs_tol=tol)
+    assert math.isclose(obs[29], near[1] / 15.0, abs_tol=tol)
+    assert math.isclose(obs[30], far[0] / 15.0, abs_tol=tol)
+    assert math.isclose(obs[31], far[1] / 15.0, abs_tol=tol)
+
+
+def test_obs_pen_vector_zero_at_pen_entry():
+    obs = build_obs((11.5, -15.0), 0.0, [(0.0, 0.0)], [False])
+    assert math.isclose(obs[14], 0.0)         # distance to pen

tests/test_perception.py

@@ -0,0 +1,163 @@
+"""LiDAR simulation + perception pipeline + multi-target tracker."""
+
+import math
+
+import numpy as np
+import pytest
+
+from herding.perception.lidar_perception import (
+    STATIC_REJECT, detections_from_scan,
+)
+from herding.perception.lidar_sim import (
+    LIDAR_MAX_RANGE, LIDAR_N_RAYS, SHEEP_RADIUS, ray_angles, simulate_scan,
+)
+from herding.perception.sheep_tracker import (
+    FORGET_STEPS, GATE_M, MAX_ACTIVE_TRACKS, REACQUIRE_GATE_M,
+    REACQUIRE_MIN_AGE, SheepTracker,
+)
+
+
+# ---------------------------------------------------------------------------
+# Sim
+# ---------------------------------------------------------------------------
+
+def test_simulate_scan_shape_and_dtype():
+    ranges = simulate_scan(0.0, 0.0, 0.0, [(5.0, 0.0)], noise=0.0)
+    assert ranges.shape == (LIDAR_N_RAYS,)
+    assert ranges.dtype == np.float32
+
+
+def test_simulate_scan_no_sheep_far_from_walls():
+    # Dog at origin, no sheep, walls all ≥ 15 m away → all rays at max.
+    ranges = simulate_scan(0.0, 0.0, 0.0, [], noise=0.0)
+    # Walls (east/west at ±15) are beyond LIDAR_MAX_RANGE=12, so no hits.
+    assert (ranges == LIDAR_MAX_RANGE).all()
+
+
+def test_simulate_scan_sheep_in_front_returns_centre_hit():
+    # Sheep dead ahead at 5 m. Centre ray should hit ~ 5 - SHEEP_RADIUS.
+    ranges = simulate_scan(0.0, 0.0, 0.0, [(5.0, 0.0)], noise=0.0)
+    centre = ranges[LIDAR_N_RAYS // 2]
+    assert math.isclose(float(centre), 5.0 - SHEEP_RADIUS, abs_tol=0.01)
+
+
+def test_simulate_scan_sheep_behind_dog_not_hit():
+    ranges = simulate_scan(0.0, 0.0, 0.0, [(-5.0, 0.0)], noise=0.0)
+    assert (ranges == LIDAR_MAX_RANGE).all()
+
+
+def test_simulate_scan_wall_hit():
+    # Dog 1 m south of the north wall, facing north → centre ray ≈ 1 m.
+    ranges = simulate_scan(0.0, 14.0, math.pi / 2, [], noise=0.0)
+    centre = ranges[LIDAR_N_RAYS // 2]
+    assert math.isclose(float(centre), 1.0, abs_tol=0.01)
+
+
+# ---------------------------------------------------------------------------
+# Perception
+# ---------------------------------------------------------------------------
+
+def test_detections_recover_sheep_position():
+    sheep = [(5.0, 0.0), (3.0, 1.0)]
+    ranges = simulate_scan(0.0, 0.0, 0.0, sheep, noise=0.0)
+    det = detections_from_scan(ranges, 0.0, 0.0, 0.0)
+    assert len(det) == 2
+    # Centroid bias is corrected to within ~5 cm.
+    for truth in sheep:
+        assert any(math.hypot(d[0] - truth[0], d[1] - truth[1]) < 0.1
+                   for d in det)
+
+
+def test_detections_filter_gate_post():
+    # An empty scene at the dog right next to a gate post produces no
+    # detections — the static-feature filter drops the post return.
+    ranges = simulate_scan(11.5, -10.0, -math.pi / 2, [], noise=0.0)
+    det = detections_from_scan(ranges, 11.5, -10.0, -math.pi / 2)
+    for cx, cy in det:
+        assert math.hypot(cx - 10.0, cy + 15.0) > STATIC_REJECT
+        assert math.hypot(cx - 13.0, cy + 15.0) > STATIC_REJECT
+
+
+def test_detections_empty_scan_returns_nothing():
+    assert detections_from_scan(np.array([], dtype=np.float32),
+                                0.0, 0.0, 0.0) == []
+
+
+# ---------------------------------------------------------------------------
+# Tracker
+# ---------------------------------------------------------------------------
+
+def test_tracker_creates_track_for_new_detection():
+    t = SheepTracker()
+    t.update([(5.0, 0.0)])
+    assert t.n_active() == 1
+
+
+def test_tracker_associates_close_detections():
+    """A small movement within the gate keeps the same track."""
+    t = SheepTracker()
+    t.update([(5.0, 0.0)])
+    t.update([(5.5, 0.0)])
+    assert t.n_active() == 1
+
+
+def test_tracker_spawns_new_track_far_detection():
+    t = SheepTracker()
+    t.update([(5.0, 0.0)])
+    t.update([(-5.0, 0.0)])           # well outside the gate
+    assert t.n_active() == 2
+
+
+def test_tracker_reacquisition_for_stale_track():
+    """A stale track within the wider re-acquisition gate rebinds rather
+    than spawning a duplicate."""
+    t = SheepTracker()
+    t.update([(0.0, 0.0)])
+    # Let it go stale.
+    for _ in range(REACQUIRE_MIN_AGE):
+        t.update([])
+    # Re-emerges within REACQUIRE_GATE but outside the primary GATE.
+    offset = (GATE_M + REACQUIRE_GATE_M) / 2.0
+    t.update([(offset, 0.0)])
+    assert t.n_active() == 1
+
+
+def test_tracker_forgets_stale_tracks():
+    t = SheepTracker()
+    t.update([(0.0, 0.0)])
+    for _ in range(FORGET_STEPS + 1):
+        t.update([])
+    assert t.n_active() == 0
+
+
+def test_tracker_penned_position_promotes_track():
+    t = SheepTracker()
+    t.update([(11.5, -16.0)])         # spawn inside the pen column
+    # is_penned_position is True for this point.
+    assert t.n_penned() == 1
+    assert t.n_active() == 0
+
+
+def test_tracker_penned_tracks_persist():
+    t = SheepTracker()
+    t.update([(11.5, -16.0)])
+    for _ in range(FORGET_STEPS * 2):
+        t.update([])
+    # Penned tracks are not forgotten.
+    assert t.n_penned() == 1
+
+
+def test_tracker_caps_active_set():
+    t = SheepTracker()
+    # Spawn more than the cap, each well outside the others' gates.
+    for k in range(MAX_ACTIVE_TRACKS + 5):
+        t.update([(k * (GATE_M + 1.0), 0.0)])
+    assert t.n_active() <= MAX_ACTIVE_TRACKS
+
+
+def test_tracker_reset_clears_state():
+    t = SheepTracker()
+    t.update([(0.0, 0.0)])
+    t.reset()
+    assert t.n_active() == 0
+    assert t.step == 0