Simulated Pose Estimation Only Works When In Motion

[falOn-Dev]

Describe the bug
I'm not sure if this is intended, but when running simulated PhotonVision pose estimation, it only seems to be outputting poses when the robot is in motion. When it's stationary but still sees targets, the Optional<EstimatedRobotPose> starts returning as an empty optional.

To Reproduce
Steps to reproduce the behavior:

1. Simulate my robot code (use the linked branch)
2. Drive around
3. View the isPresent field under RealOutputs/vision

Screenshots / Videos

2024-08-30.09-11-28.mp4

Platform:

• Hardware Platform (ex. Raspberry Pi 4, Windows x64): Windows x64, no co-processor
• Network Configuration (Connection between the Radio and any devices in between, such as a Network Switch): N/A (Sim)
• PhotonVision Version: PhotonLib is v2024.3.1, no PV running on a co-processor
• Browser (with Version) (Chrome, Edge, Firefox, etc.): N/A
• Camera(s) Used: N/A

Additional context
I'm not completely sure this is a bug with PhotonVision, however I've seen another person with a similar issue, and I was asked by Matt to submit this.

[amquake]

photonvision/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

Lines 135 to 144 in 7271c95

	/** Invalidates the pose cache. */
	private void invalidatePoseCache() {
	poseCacheTimestampSeconds = -1;
	}
	private void checkUpdate(Object oldObj, Object newObj) {
	if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) {
	invalidatePoseCache();
	}
	}

photonvision/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

Lines 357 to 366 in 7271c95

	// If the pose cache timestamp was set, and the result is from the same
	// timestamp, return an
	// empty result
	if (poseCacheTimestampSeconds > 0
	&& Math.abs(poseCacheTimestampSeconds - cameraResult.getTimestampSeconds()) < 1e-6) {
	return Optional.empty();
	}
	// Remember the timestamp of the current result used
	poseCacheTimestampSeconds = cameraResult.getTimestampSeconds();

This should be checking if the updated pose is different from the last, or that the timestamp is different. An empty estimate should only be sent if both of those conditions are false. One thing I'm not certain on is if the NT subscriber updates the timestamp for identical values:

photonvision/photon-lib/src/main/java/org/photonvision/PhotonCamera.java

Lines 182 to 185 in 7271c95

	// Set the timestamp of the result.
	// getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
	ret.setTimestampSeconds(
	(resultSubscriber.subscriber.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3);

getLastChange() here possibly not updating the timestamp, which would cause the behavior you see.

Your simulated camera has no noise added to it, which is causing the estimated pose while stationary to be completely stable. This probably wouldn't happen on a real robot.

[gerth2]

One thing I'm not certain on is if the NT subscriber updates the timestamp for identical values:

With NT4 - yes - there should be one publish operation per piece of info in the camera frame. Data can be identical, but timestamp will advance.