Optimize vision pipeline: reduce PhotonVision CPU load

[dmeglan]

Summary

Reduces CPU time spent in the vision subsystem periodic, addressing intermittent loop overruns caused by PhotonVision processing.

Changes

1. Skip getAllUnreadResults() when camera is disconnected (biggest win)

Before: Called every cycle regardless of connection state. When disconnected, photonlibpy's _versionCheck() fires wpilib.reportWarning() every 5 seconds, which constructs a full Python stack trace each time. On the roboRIO's ARM CPU, generating stack traces is expensive (~5-10ms per warning).

After: Early return when isConnected() is false — zero CAN/NT traffic, zero Transform3d math, zero warning overhead.

2. Reduce result processing cap from 10 to 2

Before: allResults[-10:] — processes up to 10 queued results per cycle.

After: allResults[-2:] — processes at most 2 results per cycle.

PhotonVision's pipeline runs at 30-90 FPS while the robot loop runs at 50 Hz. Results accumulate between cycles, especially during vision processing spikes. Each result requires Transform3d inversions, Pose3d construction, bitmask encoding, and observation object creation. At 10 results with multi-tag solving, this can spike to >15ms of vision processing alone.

The most recent 2 results are sufficient — older results have increasingly stale timestamps and the pose estimator already handles latency compensation.

3. Cache tag poses and field bounds at init

Before: kApriltagFieldLayout.getTagPose(tagId) called per tag per cycle (in both IO and subsystem), getFieldLength()/getFieldWidth() called per observation for bounds checking.

After: Tag poses cached in a dict at __init__, field bounds cached as floats. Lookups become dict .get() calls instead of method calls through the WPILib AprilTagFieldLayout.

4. Fix single-tag fiducial ID bitmask encoding (bug fix)

Before: Single-tag observations passed target.fiducialId directly as tagsList (e.g., tag 5 → value 5).

After: Correctly encodes as 1 << (target.fiducialId - 1) (e.g., tag 5 → value 16, bit 4 set).

This bug meant the issubset check in robotstate.py that gates hub estimator updates could not work correctly for single-tag observations. The hub estimator may have been receiving non-hub tag data, corrupting hub-relative pose estimates. (Same fix as PR #101 but for both turreted and non-turreted paths.)

5. Limit bitmask iteration to 22 tags

Before: for tagId in range(32) — iterates all 32 bits.

After: for tagId in range(22) — only 22 tags exist on the field. Saves 10 iterations × 2 (regular + turreted) × per-observation.

6. Conditional logging — skip empty frames

Before: 14 Logger.recordOutput() calls per camera every cycle, even when no tags were seen (serializing empty Pose3d lists).

After: Only logs when there are actual observations. On frames with no visible tags (which is most frames when driving away from tags), this eliminates all 14+ serialization calls.

Files Changed

File	Change
src/subsystems/vision/visioniophoton.py	Early return on disconnect, result cap 10→2, tag pose cache, bitmask fix
src/subsystems/vision/visionsubsystem.py	Tag pose cache, field bounds cache, bitmask 32→22, conditional logging

Expected Impact

On cycles with no visible tags (common during driving):

• Before: ~5-15ms in vision processing (version check warnings, empty list serialization)
• After: ~0.5ms (early return + skip logging)

On cycles with visible tags:

• Before: Up to 10 results × full processing pipeline
• After: At most 2 results × same pipeline with cached lookups

Test Plan

• Deploy and verify vision still provides pose estimates when tags are visible
• Check Timing/RobotPeriodic/SchedulerUpdateMS in AdvantageScope — vision-heavy cycles should be shorter
• Verify no PhotonVision warning spam in console when camera is disconnected
• Confirm hub estimator correctly filters non-hub tags (single-tag bitmask fix)
• Test with camera connected and disconnected mid-match