Consensus tracker + active scan close Webots 140° LiDAR gap

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>
This commit is contained in:
Johnny Fernandes
2026-05-16 20:19:11 +00:00
parent 876e14e74f
commit 2d23289052
5 changed files with 312 additions and 44 deletions
+51 -3
View File
@@ -175,6 +175,26 @@ class TrackerConfig:
from permanently consuming tracker slots as false "penned" sheep.
"""
consensus_k: int = 1
"""New tracks must accumulate this many matches before they appear in
``get_positions``. ``1`` (default) disables the candidate stage —
behaviour-identical to the original tracker. ``3-4`` filters one-shot
LiDAR phantoms in Webots while a real sheep promotes within
``consensus_k * timestep`` ≈ 50-65 ms.
"""
consensus_radius_m: float = 0.5
"""Maximum distance (metres) between successive matches for a candidate
to age toward promotion. Tighter than ``gate_m`` so wall-cluster
centroid jitter cannot keep a phantom alive. Real sheep move
≪ 0.05 m / step at max speed so this gate is very loose for them.
"""
consensus_max_age: int = 8
"""A candidate that has not been matched for this many steps is dropped.
Short — phantoms get one window to confirm or die.
"""
def __post_init__(self) -> None:
if self.forget_steps < 1:
raise ValueError(f"forget_steps must be ≥ 1, got {self.forget_steps}")
@@ -182,6 +202,16 @@ class TrackerConfig:
raise ValueError(
f"max_new_tracks_per_step must be ≥ 1, got {self.max_new_tracks_per_step}"
)
if self.consensus_k < 1:
raise ValueError(f"consensus_k must be ≥ 1, got {self.consensus_k}")
if self.consensus_radius_m <= 0.0:
raise ValueError(
f"consensus_radius_m must be > 0, got {self.consensus_radius_m}"
)
if self.consensus_max_age < 1:
raise ValueError(
f"consensus_max_age must be ≥ 1, got {self.consensus_max_age}"
)
# ---------------------------------------------------------------------------
@@ -317,9 +347,13 @@ HERDING_WEBOTS = HerdingConfig(
lidar=LIDAR_WEBOTS,
detection=DetectionConfig(wall_reject=0.5, static_reject=1.2),
tracker=TrackerConfig(
forget_steps=120,
forget_steps=300,
max_new_tracks_per_step=1,
pen_latch_depth=2.0,
predict_steps=180,
consensus_k=3,
consensus_radius_m=0.3,
consensus_max_age=20,
),
robot=RobotConfig(action_smooth=0.55),
)
@@ -329,7 +363,21 @@ Changes vs HERDING_DEFAULT:
* LiDAR: 180 rays / 140° FOV matching ShepherdDog.proto hardware
* Detection: wall_reject kept at 0.5 m (original default; static_reject
handles post FPs; 1.0 m was too aggressive near the south gate)
* Tracker: forget_steps 200 → 60 (~1 s ghost-track lifetime)
max_new_tracks_per_step 10 → 3 (rate-caps FP flooding)
* Tracker:
- consensus_k=3, radius=0.3 m, max_age=20 (~320 ms window): a new
detection must be confirmed by two more nearby detections within
a tight 0.3 m radius to promote. Real sheep barely move
frame-to-frame (≪0.05 m/step) so they easily self-confirm while
the dog is rotating across them; wall-return phantoms whose
cluster centroid jitters by more than 0.3 m as the dog moves
can't accumulate three nearby hits and decay as separate
candidates.
- forget_steps=300 (~4.8 s) + predict_steps=180 (~2.9 s): once a
real sheep is confirmed, it lives in tracker memory long enough
for the policy — trained on 360° full-visibility obs — to plan
while the dog sweeps a sparse cone across the field. Set short
enough that any phantom that does leak through promotion dies
after the dog walks away from the wall that created it.
- max_new_tracks_per_step=1 still rate-caps spawn bursts.
* Robot: action_smooth 0.0 → 0.55 (matches Webots controller EMA)
"""