diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index cd792d7637..5e387f16ab 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/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,7 +144,6 @@ def getAllUnreadResults(self) -> List[PhotonPipelineResult]: for change in changes: byteList = change.value - timestamp = change.time if len(byteList) < 1: pass @@ -152,8 +151,6 @@ def getAllUnreadResults(self) -> List[PhotonPipelineResult]: 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. diff --git a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py index b804d3e0fa..3273a0baf0 100644 --- a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py +++ b/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), diff --git a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py b/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py index 24d5d709c0..e410c1c8e2 100644 --- a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py +++ b/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py @@ -11,8 +11,9 @@ @dataclass class PhotonPipelineMetadata: - # Image capture and NT publish timestamp, in microseconds and in the coprocessor timebase. As - # reported by WPIUtilJNI::now. + # Image capture and NT publish timestamp, in microseconds and in the Time Sync Server's + # timebase (wpi::nt::Now). The robot shall run a server, so this is FPGA-relative on a real + # robot. NTDataPublisher applies the time-sync offset before publishing. captureTimestampMicros: int = -1 publishTimestampMicros: int = -1 @@ -26,9 +27,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 +40,12 @@ 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 captured, in the same time base as + ``wpilib.Timer.getFPGATimestamp()``. + + :returns: The timestamp in seconds. """ - # 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 diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py index 430258dbe9..8ae37f5d11 100644 --- a/photon-lib/py/test/photonPoseEstimator_test.py +++ b/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() diff --git a/photon-lib/py/test/testUtil.py b/photon-lib/py/test/testUtil.py index d2be20ebee..16c5c9c762 100644 --- a/photon-lib/py/test/testUtil.py +++ b/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, diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index a63fe1aafb..fed82effc2 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -34,7 +34,6 @@ #include #include #include -#include #include #include #include @@ -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 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(); diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 1cf1ae714a..c9ae8e6100 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/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 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(), - test2.GetTimestamp().to(), 0.001); -} - TEST(PhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) { photon::PhotonPoseEstimator estimator( wpi::apriltag::AprilTagFieldLayout::LoadField( @@ -651,12 +637,11 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { std::vector{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{ diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java index e5687ca427..be4e73a31b 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java @@ -58,7 +58,9 @@ public double getLatencyMillis() { } /** - * The time that this image was captured, in the coprocessor's time base. + * The time that this image was captured, in microseconds and in the Time Sync Server's time base + * ({@code wpi::nt::Now}). The robot shall run a server, so this is FPGA-relative on a real robot. + * NTDataPublisher applies the time-sync offset before publishing. * * @return The time in microseconds */ @@ -67,7 +69,9 @@ public long getCaptureTimestampMicros() { } /** - * The time that this result was published to NT, in the coprocessor's time base. + * The time that this result was published to NT, in microseconds and in the Time Sync Server's + * time base ({@code wpi::nt::Now}). The robot shall run a server, so this is FPGA-relative on a + * real robot. NTDataPublisher applies the time-sync offset before publishing. * * @return The time in microseconds */ diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 7945276d99..10b1729917 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -168,11 +168,10 @@ public Optional getMultiTagResult() { } /** - * Returns the estimated time the frame was taken, in the Time Sync Server's time base - * (wpi::nt::Now). This is calculated using the estimated offset between Time Sync Server time and - * local time. The robot shall run a server, so the offset shall be 0. + * Returns the estimated time the frame was captured, in the same time base as {@link + * edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}. * - * @return The timestamp in seconds + * @return The timestamp in seconds. */ public double getTimestampSeconds() { return metadata.captureTimestampMicros / 1e6; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h index 4cd9447e33..7ae44cf74d 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -38,25 +38,6 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { PhotonPipelineResult() : Base() {} explicit PhotonPipelineResult(Base&& data) : Base(data) {} - // Don't forget to deal with our ntReceiveTimestamp - PhotonPipelineResult(const PhotonPipelineResult& other) - : Base(other), ntReceiveTimestamp(other.ntReceiveTimestamp) {} - PhotonPipelineResult(PhotonPipelineResult& other) - : Base(other), ntReceiveTimestamp(other.ntReceiveTimestamp) {} - PhotonPipelineResult(PhotonPipelineResult&& other) - : Base(std::move(other)), - ntReceiveTimestamp(std::move(other.ntReceiveTimestamp)) {} - auto& operator=(const PhotonPipelineResult& other) { - Base::operator=(other); - ntReceiveTimestamp = other.ntReceiveTimestamp; - return *this; - } - auto& operator=(PhotonPipelineResult&& other) { - ntReceiveTimestamp = other.ntReceiveTimestamp; - Base::operator=(std::move(other)); - return *this; - } - template explicit PhotonPipelineResult(Args&&... args) : Base{std::forward(args)...} {} @@ -91,16 +72,14 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { } /** - * Returns the estimated time the frame was taken, in the Time Sync Server's - * time base (nt::Now). This is calculated using the estimated offset between - * Time Sync Server time and local time. The robot shall run a server, so the - * offset shall be 0. - * This is much more accurate than using GetLatency() - * @return The timestamp in seconds or -1 if this result was not initiated - * with a timestamp. + * Returns the estimated time the frame was captured, in the same time base as + * wpi::Timer::GetMonotonicTimestamp + * + * @return The timestamp in seconds. */ wpi::units::second_t GetTimestamp() const { - return ntReceiveTimestamp - GetLatency(); + return wpi::units::microsecond_t{ + static_cast(metadata.captureTimestampMicros)}; } /** @@ -121,11 +100,6 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { */ int64_t SequenceID() const { return metadata.sequenceID; } - /** Sets the FPGA timestamp this result was Received by robot code */ - void SetReceiveTimestamp(const wpi::units::second_t timestamp) { - this->ntReceiveTimestamp = timestamp; - } - /** * Returns whether the pipeline has targets. * @return Whether the pipeline has targets. @@ -144,10 +118,6 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { friend bool operator==(PhotonPipelineResult const&, PhotonPipelineResult const&) = default; - // Since we don't trust NT time sync, keep track of when we got this packet - // into robot code - wpi::units::microsecond_t ntReceiveTimestamp = -1_s; - inline static bool HAS_WARNED = false; }; } // namespace photon