fix(photon-lib): align C++/Python getTimestampSeconds on Java's capture-time semantic

photon-lib/py/photonlibpy/photonCamera.py

@@ -24,7 +24,7 @@
 # magical import to make serde stuff work
 import photonlibpy.generated  # noqa
 import wpilib
-from wpilib import RobotController, Timer
+from wpilib import Timer
 
 from .packet import Packet
 from .targeting.photonPipelineResult import PhotonPipelineResult
@@ -144,16 +144,13 @@ def getAllUnreadResults(self) -> List[PhotonPipelineResult]:
 
         for change in changes:
             byteList = change.value
-            timestamp = change.time
 
             if len(byteList) < 1:
                 pass
             else:
                 newResult = PhotonPipelineResult()
                 pkt = Packet(byteList)
                 newResult = PhotonPipelineResult.photonStruct.unpack(pkt)
-                # NT4 allows us to correct the timestamp based on when the message was sent
-                newResult.ntReceiveTimestampMicros = timestamp
                 ret.append(newResult)
 
         return ret
@@ -169,19 +166,14 @@ def getLatestResult(self) -> PhotonPipelineResult:
 
         self._versionCheck()
 
-        now = RobotController.getFPGATime()
         packetWithTimestamp = self._rawBytesEntry.getAtomic()
         byteList = packetWithTimestamp.value
-        packetWithTimestamp.time
 
         if len(byteList) < 1:
             return PhotonPipelineResult()
         else:
             pkt = Packet(byteList)
-            retVal = PhotonPipelineResult.photonStruct.unpack(pkt)
-            # We don't trust NT4 time, hack around
-            retVal.ntReceiveTimestampMicros = now
-            return retVal
+            return PhotonPipelineResult.photonStruct.unpack(pkt)
 
     def getDriverMode(self) -> bool:
         """Returns whether the camera is in driver mode.

photon-lib/py/photonlibpy/simulation/photonCameraSim.py

@@ -436,7 +436,6 @@ def distance(target: VisionTargetSim):
         self.heartbeatCounter += 1
         publishTimestampMicros = wpilib.Timer.getFPGATimestamp() * 1e6
         return PhotonPipelineResult(
-            ntReceiveTimestampMicros=int(publishTimestampMicros + 10),
             metadata=PhotonPipelineMetadata(
                 captureTimestampMicros=int(publishTimestampMicros - latency * 1e6),
                 publishTimestampMicros=int(publishTimestampMicros),

photon-lib/py/photonlibpy/targeting/photonPipelineResult.py

@@ -26,9 +26,6 @@ class PhotonPipelineMetadata:
 
 @dataclass
 class PhotonPipelineResult:
-    # Since we don't trust NT time sync, keep track of when we got this packet into robot code
-    ntReceiveTimestampMicros: int = -1
-
     targets: list[PhotonTrackedTarget] = field(default_factory=list)
     # Python users beware! We don't currently run a Time Sync Server, so these timestamps are in
     # an arbitrary timebase. This is not true in C++ or Java.
@@ -42,15 +39,11 @@ def getLatencyMillis(self) -> float:
 
     def getTimestampSeconds(self) -> float:
         """
-        Returns the estimated time the frame was taken, in the Received system's time base. This is
-        calculated as (NT Receive time (robot base) - (publish timestamp, coproc timebase - capture
-        timestamp, coproc timebase))
+        Returns the estimated time the frame was taken, in the Time Sync Server's time base
+        (wpi::nt::Now). Reads metadata.captureTimestampMicros directly — latency compensation
+        is already baked in by the coprocessor.
         """
-        # TODO - we don't trust NT4 to correctly latency-compensate ntReceiveTimestampMicros
-        latency = (
-            self.metadata.publishTimestampMicros - self.metadata.captureTimestampMicros
-        )
-        return (self.ntReceiveTimestampMicros - latency) / 1e6
+        return self.metadata.captureTimestampMicros / 1e6
 
     def getTargets(self) -> list[PhotonTrackedTarget]:
         return self.targets

photon-lib/py/test/photonPoseEstimator_test.py

@@ -192,9 +192,9 @@ def test_pnpDistanceTrigSolve():
     bestTarget = result.getBestTarget()
     assert bestTarget is not None
     assert bestTarget.fiducialId == 0
-    assert result.ntReceiveTimestampMicros > 0
+    assert result.metadata.captureTimestampMicros > 0
     # Make test independent of the FPGA time.
-    result.ntReceiveTimestampMicros = int(fakeTimestampSecs * 1e6)
+    result.metadata.captureTimestampMicros = int(fakeTimestampSecs * 1e6)
 
     estimator.addHeadingData(
         result.getTimestampSeconds(), realPose.rotation().toRotation2d()
@@ -221,9 +221,9 @@ def test_pnpDistanceTrigSolve():
     bestTarget = result.getBestTarget()
     assert bestTarget is not None
     assert bestTarget.fiducialId == 0
-    assert result.ntReceiveTimestampMicros > 0
+    assert result.metadata.captureTimestampMicros > 0
     # Make test independent of the FPGA time.
-    result.ntReceiveTimestampMicros = int(fakeTimestampSecs * 1e6)
+    result.metadata.captureTimestampMicros = int(fakeTimestampSecs * 1e6)
 
     estimator.addHeadingData(
         result.getTimestampSeconds(), realPose.rotation().toRotation2d()

photon-lib/py/test/testUtil.py

@@ -15,17 +15,13 @@ def __init__(
         *,
         captureTimestampMicros: int,
         pipelineLatencyMicros=2_000,
-        receiveLatencyMicros=1_000,
     ):
         if captureTimestampMicros < 0:
             raise InvalidTestDataException("captureTimestampMicros cannot be negative")
         if pipelineLatencyMicros <= 0:
             raise InvalidTestDataException("pipelineLatencyMicros must be positive")
-        if receiveLatencyMicros < 0:
-            raise InvalidTestDataException("receiveLatencyMicros cannot be negative")
         self._captureTimestampMicros = captureTimestampMicros
         self._pipelineLatencyMicros = pipelineLatencyMicros
-        self._receiveLatencyMicros = receiveLatencyMicros
         self._sequenceID = 0
 
     @property
@@ -54,9 +50,6 @@ def incrementTimeMicros(self, micros: int) -> None:
     def publishTimestampMicros(self) -> int:
         return self._captureTimestampMicros + self.pipelineLatencyMicros
 
-    def receiveTimestampMicros(self) -> int:
-        return self.publishTimestampMicros() + self._receiveLatencyMicros
-
     def toPhotonPipelineMetadata(self) -> PhotonPipelineMetadata:
         return PhotonPipelineMetadata(
             captureTimestampMicros=self.captureTimestampMicros,

photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp

@@ -34,7 +34,6 @@
 #include <opencv2/core/utility.hpp>
 #include <wpi/hal/UsageReporting.hpp>
 #include <wpi/system/Errors.hpp>
-#include <wpi/system/RobotController.hpp>
 #include <wpi/system/Timer.hpp>
 #include <wpi/system/WPILibVersion.hpp>
 #include <wpi/util/json.hpp>
@@ -193,9 +192,6 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
   // Prints warning if not connected
   VerifyVersion();
 
-  // Fill the packet with latest data and populate result.
-  wpi::units::microsecond_t now =
-      wpi::units::microsecond_t(wpi::RobotController::GetFPGATime());
   const auto value = rawBytesEntry.Get();
   if (!value.size()) return PhotonPipelineResult{};
 
@@ -206,8 +202,6 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
 
   CheckTimeSyncOrWarn(result);
 
-  result.SetReceiveTimestamp(now);
-
   return result;
 }
 
@@ -239,11 +233,6 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
 
     CheckTimeSyncOrWarn(result);
 
-    // TODO: NT4 timestamps are still not to be trusted. But it's the best we
-    // can do until we can make time sync more reliable.
-    result.SetReceiveTimestamp(wpi::units::microsecond_t(value.time) -
-                               result.GetLatency());
-
     ret.push_back(result);
   }
 
@@ -265,7 +254,7 @@ void PhotonCamera::CheckTimeSyncOrWarn(photon::PhotonPipelineResult& result) {
     timesyncAlert.SetText(warningText);
     timesyncAlert.Set(true);
 
-    if (wpi::Timer::GetFPGATimestamp() <
+    if (wpi::Timer::GetFPGATimestamp() >
         (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
       prevTimeSyncWarnTime = wpi::Timer::GetFPGATimestamp();
 

photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp

@@ -89,8 +89,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
-  cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(11));
+      photon::PhotonPipelineMetadata{11'000'000, 11'000'000, 2000, 1000},
+      targets, std::nullopt}};
 
   photon::PhotonPoseEstimator estimator(aprilTags, wpi::math::Transform3d{});
 
@@ -132,8 +132,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityIgnoresNonFiducialTargets) {
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
-  cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(11));
+      photon::PhotonPipelineMetadata{11'000'000, 11'000'000, 2000, 1000},
+      targets, std::nullopt}};
 
   photon::PhotonPoseEstimator estimator(aprilTags, wpi::math::Transform3d{});
 
@@ -191,8 +191,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
-  cameraOne.testResult[0].SetReceiveTimestamp(17_s);
+      photon::PhotonPipelineMetadata{17'000'000, 17'000'000, 2000, 1000},
+      targets, std::nullopt}};
 
   photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}});
 
@@ -242,8 +242,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
-  cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(17));
+      photon::PhotonPipelineMetadata{17'000'000, 17'000'000, 2000, 1000},
+      targets, std::nullopt}};
 
   photon::PhotonPoseEstimator estimator(aprilTags, {});
 
@@ -295,8 +295,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
-  cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(17));
+      photon::PhotonPipelineMetadata{17'000'000, 17'000'000, 2000, 1000},
+      targets, std::nullopt}};
 
   photon::PhotonPoseEstimator estimator(aprilTags, {});
 
@@ -334,9 +334,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
           0.4, corners, detectedCorners}};
 
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree,
-      std::nullopt}};
-  cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(21));
+      photon::PhotonPipelineMetadata{21'000'000, 21'000'000, 2000, 1000},
+      targetsThree, std::nullopt}};
 
   for (const auto& result : cameraOne.GetAllUnreadResults()) {
     estimatedPose = estimator.EstimateClosestToReferencePose(result, pose);
@@ -383,7 +382,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
       1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
       targets);
   cameraOne.testResult = {result};
-  cameraOne.testResult[0].SetReceiveTimestamp(17_s);
+  cameraOne.testResult[0].metadata.captureTimestampMicros = 17'000'000LL;
 
   estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
 
@@ -415,7 +414,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
       1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
       targets);
   cameraOne.testResult = {result};
-  cameraOne.testResult[0].SetReceiveTimestamp(18_s);
+  cameraOne.testResult[0].metadata.captureTimestampMicros = 18'000'000LL;
 
   estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
 
@@ -471,8 +470,8 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
-  cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(15));
+      photon::PhotonPipelineMetadata{15'000'000, 15'000'000, 2000, 1000},
+      targets, std::nullopt}};
 
   photon::PhotonPoseEstimator estimator(aprilTags, {});
 
@@ -509,8 +508,8 @@ TEST(PhotonPoseEstimatorTest,
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
-  cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(4));
+      photon::PhotonPipelineMetadata{4'000'000, 4'000'000, 2000, 1000}, targets,
+      std::nullopt}};
 
   photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}});
 
@@ -531,8 +530,8 @@ TEST(PhotonPoseEstimatorTest,
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
-  cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(17));
+      photon::PhotonPipelineMetadata{17'000'000, 17'000'000, 2000, 1000},
+      targets, std::nullopt}};
 
   photon::PhotonPoseEstimator estimator(aprilTags, {});
 
@@ -565,8 +564,8 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) {
 
   cameraOne.test = true;
   cameraOne.testResult = {photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
-  cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(11));
+      photon::PhotonPipelineMetadata{11'000'000, 11'000'000, 2000, 1000},
+      targets, std::nullopt}};
 
   photon::PhotonPoseEstimator estimator(aprilTags, wpi::math::Transform3d{});
 
@@ -592,19 +591,6 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) {
   EXPECT_EQ(1, estimatedPose.value().targetsUsed[0].GetFiducialId());
 }
 
-TEST(PhotonPoseEstimatorTest, CopyResult) {
-  std::vector<photon::PhotonTrackedTarget> targets{};
-
-  auto testResult = photon::PhotonPipelineResult{
-      photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt};
-  testResult.SetReceiveTimestamp(wpi::units::second_t(11));
-
-  auto test2 = testResult;
-
-  EXPECT_NEAR(testResult.GetTimestamp().to<double>(),
-              test2.GetTimestamp().to<double>(), 0.001);
-}
-
 TEST(PhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
   photon::PhotonPoseEstimator estimator(
       wpi::apriltag::AprilTagFieldLayout::LoadField(
@@ -651,12 +637,11 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
       std::vector<int16_t>{8});
 
   photon::PhotonPipelineResult result{
-      photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets,
-      multiTagResult};
+      photon::PhotonPipelineMetadata{15'000'000, 15'009'999, 2000, 100},
+      targets, multiTagResult};
 
   cameraOne.test = true;
   cameraOne.testResult = {result};
-  cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(15));
 
   const wpi::units::radian_t camPitch = 30_deg;
   const wpi::math::Transform3d kRobotToCam{