jalf/TIR_PROJ
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Consensus tracker + active scan close Webots 140° LiDAR gap

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
controllers/shepherd_dog/shepherd_dog.py
+17 -15
View File
@@ -430,9 +430,13 @@ while robot.step(timestep) != -1:
if not sheep_positions:
# BC/RL never saw "empty obs during operation" in training (empty
# obs only happened at episode end), so the policy outputs ~zero
# and the dog gets stuck. Fall back to a fixed scan rotation
# until tracker recovers some sheep.
vx, vy = 0.0, 0.6
# and the dog gets stuck. Fall back to an *active scan*: rotate
# the desired heading slowly so the narrow 140° FOV sweeps the
# field instead of charging in one fixed direction (which
# otherwise drives the dog into the north wall and ends the run).
scan_h = (step_count * 0.015) % (2.0 * math.pi)
vx = 0.5 * math.cos(scan_h)
vy = 0.5 * math.sin(scan_h)
omega = 0.5 if DRIVE_MODE == "mecanum" else 0.0
else:
action = policy_handle.predict(single_obs)
@@ -498,15 +502,13 @@ while robot.step(timestep) != -1:
gt_penned = sum(1 for x, y in _gt_sheep.values()
if is_penned_position(x, y))
gt_total = len(_gt_sheep)
print(f"[dog mode={MODE} drive={DRIVE_MODE}] step={step_count} "
f"GT_penned={gt_penned}/{gt_total} "
f"tracks_active={tracker.n_active()} "
f"tracks_penned={tracker.n_penned()} "
f"detections={len(detections)} "
f"action=({vx:+.2f}, {vy:+.2f}, {omega:+.2f})"
if DRIVE_MODE == "mecanum" else
f"[dog mode={MODE} drive={DRIVE_MODE}] step={step_count} "
f"GT_penned={gt_penned}/{gt_total} "
f"tracks_active={tracker.n_active()} "
f"tracks_penned={tracker.n_penned()} "
f"detections={len(detections)} action=({vx:+.2f}, {vy:+.2f})")
common = (f"[dog mode={MODE} drive={DRIVE_MODE}] step={step_count} "
f"GT_penned={gt_penned}/{gt_total} "
f"tracks_active={tracker.n_active()} "
f"tracks_cand={tracker.n_candidate()} "
f"tracks_penned={tracker.n_penned()} "
f"detections={len(detections)}")
if DRIVE_MODE == "mecanum":
print(f"{common} action=({vx:+.2f}, {vy:+.2f}, {omega:+.2f})")
else:
print(f"{common} action=({vx:+.2f}, {vy:+.2f})")