[photon-lib] Python support for PNP_DISTANCE_TRIG_SOLVE

[kcooney]

This adds support for PNP_DISTANCE_TRIG_SOLVE in the the python PhotonPoseEstimator, mirroring the implementation in the Java PhotonPoseEstimator.

Changes:

• Add PoseStrategy.PNP_DISTANCE_TRIG_SOLVE
• Add addHeadingData() and resetHeadingData() to PhotonPoseEstimator
• Fix PhotonCameraSim.process() to set ntReceiveTimestampMicros in the result
• Minor readability improvements to PhotonPipelineResult
• Minor test improvements to PhotonPoseEstimatorTest
• Add .vscode/settings.json (to make running python tests in VSCode easier)

Merge checklist:

• Pull Request title is short, imperative summary of proposed changes
• The description documents the what and why
• If this PR changes behavior or adds a feature, user documentation is updated
• If this PR touches photon-serde, all messages have been regenerated and hashes have not changed unexpectedly
• If this PR touches configuration, this is backwards compatible with settings back to v2024.3.1
• If this PR touches pipeline settings or anything related to data exchange, the frontend typing is updated
• If this PR addresses a bug, a regression test for it is added

[Gold856]

Looks mostly good to me. I'm curious as to why the timestamp injection is needed though.

[kcooney]

Looks mostly good to me. I'm curious as to why the timestamp injection is needed though.

@Gold856 by "timestamp injection" do you mean passing a fake timestampFunc to the PhotonCameraSim constructor? If so, without doing that, PhotonPoseEstimator.update() returns None because the code reaches this line (due to the FPGA timestamp value being less than latencyMillis * 1000).

I'm not sure why the Java tests don't have this problem. I tried running ./gradlew :photon-lib:test --tests 'PhotonPoseEstimatorTest.pnpDistanceTrigSolve' and throwing an exception on that line, and the timestamp of the PhotonPipelineResult was 0.379096, so in the Java tests there is ~1.4 seconds of time between when the start of the FPGA clock and time that the code starts pnpDistanceTrigSolve. My guess is this is due to the time it takes for the JVM to start up and JUnit to scan the classpath to find the tests.

Note that pnpDistanceTrigSolve() passes even though it doesn't call PhotonPoseEstimator.invalidatePoseCache() because the FPGA time increases by 23.8ms between the two calls to cameraOneSim.process().

[Gold856]

So, is it just too fast? If it's too fast, I'll accept a small sleep at the start of the test with a comment explaining why to make it work. I really don't like having that timestamp function.

Alternatively, it looks like we use PhotonCameraInjector in order to inject pipeline results, which I suspect is related, since it allows you to specify the timestamp yourself. You could give that a try as well.

[kcooney]

So, is it just too fast?

@Gold856 The problem isn't that the code/tests are "too fast". The estimator gets the current time and is provided the latency, and uses those values to infer the time that the image was captured. The values provided by the test resulted in an image capture time that was negative unless the test started >1s after the HAL was initialized.

If it's too fast, I'll accept a small sleep at the start of the test with a comment explaining why to make it work. I really don't like having that timestamp function.

I strongly dislike adding sleeps to "fix" tests.

I changed the tests to set PhotonPipelineResult.ntReceiveTimestampMicros to avoid the negative timestamps, which allowed me to remove the timestampFunc parameter that I had added.

Alternatively, it looks like we use PhotonCameraInjector in order to inject pipeline results, which I suspect is related, since it allows you to specify the timestamp yourself. You could give that a try as well.

The code that is calling getFPGATimestamp() is in PhotonCameraSim, not PhotonCamera, so I can't fix this in PhotonCameraInjector.

I did a bit of searching, and it looks like I could have the test use wpilib.simulation.setRuntimeType() to have the test use simulated time. I couldn't find any tests in PhotonVision or WPILib that do that, so I didn't try out that approach.