Johnny Fernandes
2d23289052
Two deploy-time fixes that take v1 360°-trained BC/RL from 0/n to n/n penned on the canonical 140° LiDAR proto for diff/field: * SheepTracker now supports a consensus stage: new detections start as candidate tracks invisible to get_positions(). A candidate must accumulate consensus_k matches within consensus_radius_m of itself inside a consensus_max_age window to be promoted; otherwise it expires. Real sheep self-confirm within 3 frames (≪0.05 m/step); wall-return cluster centroids jitter beyond 0.3 m as the dog moves and never promote. consensus_k=1 (default) is a no-op so unconfigured callers and HERDING_DEFAULT keep prior behaviour. * HERDING_WEBOTS preset gets consensus_k=3, radius=0.3, max_age=20, plus longer forget_steps=300 and predict_steps=180 so confirmed sheep persist through long FOV-occlusion gaps a narrow 140° cone produces. max_new_tracks_per_step=1 still rate-caps spawn bursts. * shepherd_dog.py BC/RL empty-obs fallback now rotates the desired heading with step_count so the cone actively sweeps the field instead of driving due north into the wall. Verified in headless Webots (HERDING_USE_GT=0, LiDAR only): BC diff/field: 5/5 @ 11698, 10/10 @ 15079 RL diff/field: 5/5 @ 10039, 9/10 @ 18200 (timeout) Strömbom diff/field: 5/5 @ 7528 All previously 0/n. 120 unit tests pass; 9 new consensus tests cover the candidate stage, promotion radius, and one-shot phantom rejection. Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
262 lines
9.4 KiB
Python
262 lines
9.4 KiB
Python
"""Tests for herding/config.py — dataclass construction, defaults, overrides."""
|
|
|
|
import math
|
|
import pytest
|
|
|
|
from herding.config import (
|
|
DetectionConfig,
|
|
DomainRandomConfig,
|
|
HerdingConfig,
|
|
HERDING_DEFAULT,
|
|
HERDING_WEBOTS,
|
|
LidarConfig,
|
|
LIDAR_FULL,
|
|
LIDAR_WEBOTS,
|
|
RobotConfig,
|
|
TrackerConfig,
|
|
)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# LidarConfig
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestLidarConfig:
|
|
def test_defaults_match_full_circle_preset(self):
|
|
assert LidarConfig() == LIDAR_FULL
|
|
|
|
def test_webots_preset(self):
|
|
assert LIDAR_WEBOTS.n_rays == 180
|
|
assert abs(LIDAR_WEBOTS.fov_rad - math.radians(140.0)) < 1e-9
|
|
|
|
def test_frozen(self):
|
|
cfg = LidarConfig()
|
|
with pytest.raises((AttributeError, TypeError)):
|
|
cfg.n_rays = 42 # type: ignore[misc]
|
|
|
|
def test_invalid_n_rays(self):
|
|
with pytest.raises(ValueError):
|
|
LidarConfig(n_rays=0)
|
|
|
|
def test_invalid_fov(self):
|
|
with pytest.raises(ValueError):
|
|
LidarConfig(fov_rad=0.0)
|
|
with pytest.raises(ValueError):
|
|
LidarConfig(fov_rad=math.pi * 3)
|
|
|
|
def test_invalid_max_range(self):
|
|
with pytest.raises(ValueError):
|
|
LidarConfig(max_range=-1.0)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# TrackerConfig
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestTrackerConfig:
|
|
def test_defaults(self):
|
|
cfg = TrackerConfig()
|
|
assert cfg.forget_steps == 200
|
|
assert cfg.max_new_tracks_per_step == 10
|
|
|
|
def test_webots_preset_tighter(self):
|
|
cfg = HERDING_WEBOTS.tracker
|
|
# forget_steps was extended so confirmed sheep tracks survive
|
|
# sparse 140° FOV re-sightings; consensus blocks phantoms from
|
|
# reaching this lifetime.
|
|
assert cfg.forget_steps >= 200
|
|
assert cfg.max_new_tracks_per_step == 1
|
|
assert cfg.pen_latch_depth == 2.0
|
|
|
|
def test_default_consensus_disabled(self):
|
|
cfg = TrackerConfig()
|
|
assert cfg.consensus_k == 1
|
|
|
|
def test_webots_preset_enables_consensus(self):
|
|
cfg = HERDING_WEBOTS.tracker
|
|
assert cfg.consensus_k > 1
|
|
assert cfg.consensus_radius_m > 0.0
|
|
assert cfg.consensus_max_age >= cfg.consensus_k
|
|
|
|
def test_invalid_forget_steps(self):
|
|
with pytest.raises(ValueError):
|
|
TrackerConfig(forget_steps=0)
|
|
|
|
def test_invalid_max_new_tracks(self):
|
|
with pytest.raises(ValueError):
|
|
TrackerConfig(max_new_tracks_per_step=0)
|
|
|
|
def test_invalid_consensus_params(self):
|
|
with pytest.raises(ValueError):
|
|
TrackerConfig(consensus_k=0)
|
|
with pytest.raises(ValueError):
|
|
TrackerConfig(consensus_radius_m=0.0)
|
|
with pytest.raises(ValueError):
|
|
TrackerConfig(consensus_max_age=0)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# DetectionConfig
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestDetectionConfig:
|
|
def test_defaults(self):
|
|
cfg = DetectionConfig()
|
|
assert cfg.wall_reject == 0.5
|
|
|
|
def test_webots_preset_wall_reject(self):
|
|
# wall_reject stays at 0.5 m — 1.0 m was too aggressive near the south gate
|
|
cfg = HERDING_WEBOTS.detection
|
|
assert cfg.wall_reject == 0.5
|
|
|
|
def test_invalid_wall_reject(self):
|
|
with pytest.raises(ValueError):
|
|
DetectionConfig(wall_reject=-0.1)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# RobotConfig
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestRobotConfig:
|
|
def test_max_linear_derived(self):
|
|
cfg = RobotConfig()
|
|
assert abs(cfg.max_linear - cfg.wheel_radius * cfg.max_wheel_omega) < 1e-9
|
|
|
|
def test_default_action_smooth_zero(self):
|
|
assert RobotConfig().action_smooth == 0.0
|
|
|
|
def test_webots_action_smooth(self):
|
|
assert HERDING_WEBOTS.robot.action_smooth == 0.55
|
|
|
|
def test_invalid_action_smooth(self):
|
|
with pytest.raises(ValueError):
|
|
RobotConfig(action_smooth=1.0)
|
|
with pytest.raises(ValueError):
|
|
RobotConfig(action_smooth=-0.1)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# DomainRandomConfig
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestDomainRandomConfig:
|
|
def test_all_zeros_by_default(self):
|
|
cfg = DomainRandomConfig()
|
|
assert cfg.fp_rate == 0.0
|
|
assert cfg.wheel_slip_std == 0.0
|
|
assert cfg.compass_noise_std == 0.0
|
|
|
|
def test_invalid_fp_rate(self):
|
|
with pytest.raises(ValueError):
|
|
DomainRandomConfig(fp_rate=-1.0)
|
|
|
|
def test_invalid_slip_std(self):
|
|
with pytest.raises(ValueError):
|
|
DomainRandomConfig(wheel_slip_std=-0.01)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# HerdingConfig
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestHerdingConfig:
|
|
def test_default_is_herding_default(self):
|
|
assert HerdingConfig() == HERDING_DEFAULT
|
|
|
|
def test_replace_sub_config(self):
|
|
new_cfg = HERDING_WEBOTS.replace(
|
|
domain_random=DomainRandomConfig(fp_rate=2.0)
|
|
)
|
|
assert new_cfg.domain_random.fp_rate == 2.0
|
|
# Other sub-configs unchanged
|
|
assert new_cfg.tracker == HERDING_WEBOTS.tracker
|
|
assert new_cfg.lidar == HERDING_WEBOTS.lidar
|
|
|
|
def test_herding_default_matches_original_module_constants(self):
|
|
"""Verify the default config reproduces the original hardcoded values."""
|
|
from herding.perception.lidar_sim import (
|
|
LIDAR_N_RAYS, LIDAR_FOV, LIDAR_MAX_RANGE, LIDAR_NOISE,
|
|
SHEEP_RADIUS, POST_RADIUS,
|
|
)
|
|
from herding.perception.lidar_perception import (
|
|
GAP_THRESHOLD, MAX_CLUSTER_SPAN, RANGE_HIT_EPS,
|
|
SPLIT_RANGE_GAP, WALL_REJECT, STATIC_REJECT,
|
|
)
|
|
from herding.perception.sheep_tracker import (
|
|
GATE_M, REACQUIRE_GATE_M, REACQUIRE_MIN_AGE, PENNED_GATE_M,
|
|
FORGET_STEPS, PREDICT_STEPS, VELOCITY_CLAMP,
|
|
)
|
|
cfg = HERDING_DEFAULT
|
|
assert cfg.lidar.n_rays == LIDAR_N_RAYS
|
|
assert cfg.lidar.fov_rad == LIDAR_FOV
|
|
assert cfg.lidar.max_range == LIDAR_MAX_RANGE
|
|
assert cfg.lidar.noise_std == LIDAR_NOISE
|
|
assert cfg.lidar.sheep_radius == SHEEP_RADIUS
|
|
assert cfg.lidar.post_radius == POST_RADIUS
|
|
assert cfg.detection.gap_threshold == GAP_THRESHOLD
|
|
assert cfg.detection.max_cluster_span == MAX_CLUSTER_SPAN
|
|
assert cfg.detection.range_hit_eps == RANGE_HIT_EPS
|
|
assert cfg.detection.split_range_gap == SPLIT_RANGE_GAP
|
|
assert cfg.detection.wall_reject == WALL_REJECT
|
|
assert cfg.detection.static_reject == STATIC_REJECT
|
|
assert cfg.tracker.gate_m == GATE_M
|
|
assert cfg.tracker.reacquire_gate_m == REACQUIRE_GATE_M
|
|
assert cfg.tracker.reacquire_min_age == REACQUIRE_MIN_AGE
|
|
assert cfg.tracker.penned_gate_m == PENNED_GATE_M
|
|
assert cfg.tracker.forget_steps == FORGET_STEPS
|
|
assert cfg.tracker.predict_steps == PREDICT_STEPS
|
|
assert cfg.tracker.velocity_clamp == VELOCITY_CLAMP
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# Integration: HerdingEnv honours the config
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestHerdingEnvConfig:
|
|
def test_default_env_unchanged(self):
|
|
"""HerdingEnv() still works with no config — zero behaviour change."""
|
|
from training.herding_env import HerdingEnv
|
|
env = HerdingEnv(n_sheep=1, max_steps=5, difficulty=1.0, seed=0)
|
|
obs, info = env.reset()
|
|
assert obs.shape == (32,)
|
|
obs2, *_ = env.step(env.action_space.sample())
|
|
assert obs2.shape == (32,)
|
|
|
|
def test_webots_config_propagates_action_smooth(self):
|
|
from training.herding_env import HerdingEnv
|
|
env = HerdingEnv(herding_cfg=HERDING_WEBOTS)
|
|
assert env.ACTION_SMOOTH == 0.55
|
|
|
|
def test_webots_config_runs(self):
|
|
from training.herding_env import HerdingEnv
|
|
env = HerdingEnv(
|
|
n_sheep=2, max_steps=10, difficulty=1.0, seed=42,
|
|
herding_cfg=HERDING_WEBOTS,
|
|
)
|
|
obs, _ = env.reset()
|
|
for _ in range(5):
|
|
obs, _, terminated, truncated, _ = env.step(env.action_space.sample())
|
|
assert obs.shape == (32,)
|
|
|
|
def test_domain_random_fp_runs(self):
|
|
from training.herding_env import HerdingEnv
|
|
cfg = HERDING_WEBOTS.replace(
|
|
domain_random=DomainRandomConfig(fp_rate=3.0, fp_std_pos=0.2)
|
|
)
|
|
env = HerdingEnv(n_sheep=2, max_steps=10, difficulty=1.0, seed=7, herding_cfg=cfg)
|
|
env.reset()
|
|
for _ in range(5):
|
|
env.step(env.action_space.sample())
|
|
|
|
def test_domain_random_slip_runs(self):
|
|
from training.herding_env import HerdingEnv
|
|
cfg = HERDING_WEBOTS.replace(
|
|
domain_random=DomainRandomConfig(wheel_slip_std=0.05, compass_noise_std=0.02)
|
|
)
|
|
env = HerdingEnv(n_sheep=1, max_steps=10, difficulty=1.0, seed=3,
|
|
drive_mode="mecanum", herding_cfg=cfg)
|
|
env.reset()
|
|
for _ in range(5):
|
|
env.step(env.action_space.sample())
|