diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index b3a0cd6842..ea878361a8 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -126,7 +126,7 @@ def getLatestResult(self) -> PhotonPipelineResult: pkt = Packet(byteList) retVal = PhotonPipelineResult.photonStruct.unpack(pkt) # We don't trust NT4 time, hack around - retVal.ntRecieveTimestampMicros = now + retVal.ntReceiveTimestampMicros = now return retVal def getDriverMode(self) -> bool: diff --git a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py b/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py index 11f51c1cde..1209ed9add 100644 --- a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py +++ b/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py @@ -21,7 +21,7 @@ class PhotonPipelineMetadata: @dataclass class PhotonPipelineResult: # Since we don't trust NT time sync, keep track of when we got this packet into robot code - ntRecieveTimestampMicros: int = -1 + ntReceiveTimestampMicros: int = -1 targets: list[PhotonTrackedTarget] = field(default_factory=list) metadata: PhotonPipelineMetadata = field(default_factory=PhotonPipelineMetadata) @@ -34,13 +34,13 @@ def getLatencyMillis(self) -> float: def getTimestampSeconds(self) -> float: """ - Returns the estimated time the frame was taken, in the recieved system's time base. This is - calculated as (NT recieve time (robot base) - (publish timestamp, coproc timebase - capture + 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)) """ - # TODO - we don't trust NT4 to correctly latency-compensate ntRecieveTimestampMicros + # TODO - we don't trust NT4 to correctly latency-compensate ntReceiveTimestampMicros return ( - self.ntRecieveTimestampMicros + self.ntReceiveTimestampMicros - ( self.metadata.publishTimestampMicros - self.metadata.captureTimestampMicros diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 62c90b326b..1130d20a0a 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -193,7 +193,7 @@ public List getAllUnreadResults() { // make time sync more reliable. for (var c : changes) { var result = c.value; - result.setRecieveTimestampMicros(c.timestamp); + result.setReceiveTimestampMicros(c.timestamp); ret.add(result); } @@ -201,7 +201,7 @@ public List getAllUnreadResults() { } /** - * Returns the latest pipeline result. This is simply the most recent result recieved via NT. + * Returns the latest pipeline result. This is simply the most recent result Received via NT. * Calling this multiple times will always return the most recent result. * *

Replaced by {@link #getAllUnreadResults()} over getLatestResult, as this function can miss @@ -221,7 +221,7 @@ public PhotonPipelineResult getLatestResult() { // contains a thing with time knowledge, set it here. // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds. // TODO: NT4 time sync is Not To Be Trusted, we should do something else? - result.setRecieveTimestampMicros(ret.timestamp); + result.setReceiveTimestampMicros(ret.timestamp); return result; } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index 4452368603..f5ab57e5c1 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -554,7 +554,7 @@ public PhotonPipelineResult process( now, detectableTgts, multitagResult); - ret.setRecieveTimestampMicros(now); + ret.setReceiveTimestampMicros(now); return ret; } diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 0b0f469555..34905a3820 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -133,7 +133,7 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { // Create the new result; PhotonPipelineResult result = packet.Unpack(); - result.SetRecieveTimestamp(now); + result.SetReceiveTimestamp(now); return result; } @@ -165,7 +165,7 @@ std::vector PhotonCamera::GetAllUnreadResults() { // 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.SetRecieveTimestamp(units::microsecond_t(value.time) - + result.SetReceiveTimestamp(units::microsecond_t(value.time) - result.GetLatency()); ret.push_back(result); diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index f62dd4126a..64c15129e2 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -100,6 +100,8 @@ std::optional PhotonPoseEstimator::Update( std::optional cameraDistCoeffs) { // Time in the past -- give up, since the following if expects times > 0 if (result.GetTimestamp() < 0_s) { + FRC_ReportError(frc::warn::Warning, + "Result timestamp was reported in the past!"); return std::nullopt; } @@ -164,7 +166,7 @@ std::optional PhotonPoseEstimator::Update( } if (ret) { - lastPose = ret.value().estimatedPose; + lastPose = ret->estimatedPose; } return ret; } @@ -197,8 +199,7 @@ std::optional PhotonPoseEstimator::LowestAmbiguityStrategy( } return EstimatedRobotPose{ - fiducialPose.value() - .TransformBy(bestTarget.GetBestCameraToTarget().Inverse()) + fiducialPose->TransformBy(bestTarget.GetBestCameraToTarget().Inverse()) .TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY}; } @@ -220,7 +221,7 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy( target.GetFiducialId()); continue; } - frc::Pose3d const targetPose = fiducialPose.value(); + frc::Pose3d const targetPose = *fiducialPose; units::meter_t const alternativeDifference = units::math::abs( m_robotToCamera.Z() - diff --git a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp index 567e6fed99..089ad80a88 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp @@ -314,7 +314,7 @@ PhotonPipelineResult PhotonCameraSim::Process( VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout); if (visibleLayoutTags.size() > 1) { std::vector usedIds{}; - usedIds.reserve(32); + usedIds.resize(visibleLayoutTags.size()); std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(), usedIds.begin(), [](const frc::AprilTag& tag) { return tag.ID; }); @@ -337,52 +337,52 @@ void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) { SubmitProcessedFrame(result, wpi::Now()); } void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result, - uint64_t recieveTimestamp) { + uint64_t ReceiveTimestamp) { ts.latencyMillisEntry.Set( result.GetLatency().convert().to(), - recieveTimestamp); + ReceiveTimestamp); Packet newPacket{}; newPacket.Pack(result); - ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp); + ts.rawBytesEntry.Set(newPacket.GetData(), ReceiveTimestamp); bool hasTargets = result.HasTargets(); - ts.hasTargetEntry.Set(hasTargets, recieveTimestamp); + ts.hasTargetEntry.Set(hasTargets, ReceiveTimestamp); if (!hasTargets) { - ts.targetPitchEntry.Set(0.0, recieveTimestamp); - ts.targetYawEntry.Set(0.0, recieveTimestamp); - ts.targetAreaEntry.Set(0.0, recieveTimestamp); + ts.targetPitchEntry.Set(0.0, ReceiveTimestamp); + ts.targetYawEntry.Set(0.0, ReceiveTimestamp); + ts.targetAreaEntry.Set(0.0, ReceiveTimestamp); std::array poseData{0.0, 0.0, 0.0}; - ts.targetPoseEntry.Set(poseData, recieveTimestamp); - ts.targetSkewEntry.Set(0.0, recieveTimestamp); + ts.targetPoseEntry.Set(poseData, ReceiveTimestamp); + ts.targetSkewEntry.Set(0.0, ReceiveTimestamp); } else { PhotonTrackedTarget bestTarget = result.GetBestTarget(); - ts.targetPitchEntry.Set(bestTarget.GetPitch(), recieveTimestamp); - ts.targetYawEntry.Set(bestTarget.GetYaw(), recieveTimestamp); - ts.targetAreaEntry.Set(bestTarget.GetArea(), recieveTimestamp); - ts.targetSkewEntry.Set(bestTarget.GetSkew(), recieveTimestamp); + ts.targetPitchEntry.Set(bestTarget.GetPitch(), ReceiveTimestamp); + ts.targetYawEntry.Set(bestTarget.GetYaw(), ReceiveTimestamp); + ts.targetAreaEntry.Set(bestTarget.GetArea(), ReceiveTimestamp); + ts.targetSkewEntry.Set(bestTarget.GetSkew(), ReceiveTimestamp); frc::Transform3d transform = bestTarget.GetBestCameraToTarget(); std::array poseData{ transform.X().to(), transform.Y().to(), transform.Rotation().ToRotation2d().Degrees().to()}; - ts.targetPoseEntry.Set(poseData, recieveTimestamp); + ts.targetPoseEntry.Set(poseData, ReceiveTimestamp); } Eigen::Matrix intrinsics = prop.GetIntrinsics(); std::span intrinsicsView{intrinsics.data(), intrinsics.data() + intrinsics.size()}; - ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp); + ts.cameraIntrinsicsPublisher.Set(intrinsicsView, ReceiveTimestamp); auto distortion = prop.GetDistCoeffs(); std::vector distortionView{distortion.data(), distortion.data() + distortion.size()}; - ts.cameraDistortionPublisher.Set(distortionView, recieveTimestamp); + ts.cameraDistortionPublisher.Set(distortionView, ReceiveTimestamp); - ts.heartbeatPublisher.Set(heartbeatCounter, recieveTimestamp); + ts.heartbeatPublisher.Set(heartbeatCounter, ReceiveTimestamp); ts.subTable->GetInstance().Flush(); } diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h index fe2082b180..ea6591c5c7 100644 --- a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h @@ -91,7 +91,7 @@ class PhotonCameraSim { void SubmitProcessedFrame(const PhotonPipelineResult& result); void SubmitProcessedFrame(const PhotonPipelineResult& result, - uint64_t recieveTimestamp); + uint64_t ReceiveTimestamp); SimCameraProperties prop; diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 28c6392cae..9ae688e7ed 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -130,7 +130,7 @@ void testLowestAmbiguityStrategy() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setRecieveTimestampMicros((long) (11 * 1e6)); + cameraOne.result.setReceiveTimestampMicros((long) (11 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d()); @@ -217,7 +217,7 @@ void testClosestToCameraHeightStrategy() { new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setRecieveTimestampMicros((long) (4 * 1e6)); + cameraOne.result.setReceiveTimestampMicros((long) (4 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -306,7 +306,7 @@ void closestToReferencePoseStrategy() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setRecieveTimestampMicros((long) (17 * 1e6)); + cameraOne.result.setReceiveTimestampMicros((long) (17 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -396,7 +396,7 @@ void closestToLastPose() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setRecieveTimestampMicros((long) (1 * 1e6)); + cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -478,7 +478,7 @@ void closestToLastPose() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setRecieveTimestampMicros((long) (7 * 1e6)); + cameraOne.result.setReceiveTimestampMicros((long) (7 * 1e6)); estimatedPose = estimator.update(cameraOne.result); pose = estimatedPose.get().estimatedPose; @@ -519,7 +519,7 @@ void cacheIsInvalidated() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - result.setRecieveTimestampMicros((long) (20 * 1e6)); + result.setReceiveTimestampMicros((long) (20 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -529,7 +529,7 @@ void cacheIsInvalidated() { // Empty result, expect empty result cameraOne.result = new PhotonPipelineResult(); - cameraOne.result.setRecieveTimestampMicros((long) (1 * 1e6)); + cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6)); Optional estimatedPose = estimator.update(cameraOne.result); assertFalse(estimatedPose.isPresent()); @@ -629,7 +629,7 @@ void averageBestPoses() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); // 3 3 3 ambig .4 - cameraOne.result.setRecieveTimestampMicros(20 * 1000000); + cameraOne.result.setReceiveTimestampMicros(20 * 1000000); PhotonPoseEstimator estimator = new PhotonPoseEstimator( diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index a1c901c15b..99ba5e3bcc 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -87,7 +87,7 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(11)); + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, frc::Transform3d{}); @@ -96,6 +96,7 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { for (const auto& result : cameraOne.GetAllUnreadResults()) { estimatedPose = estimator.Update(result); } + ASSERT_TRUE(estimatedPose); frc::Pose3d pose = estimatedPose.value().estimatedPose; EXPECT_NEAR(11, units::unit_cast(estimatedPose.value().timestamp), @@ -147,7 +148,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetRecieveTimestamp(17_s); + cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator( aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}}); @@ -156,6 +157,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { for (const auto& result : cameraOne.GetAllUnreadResults()) { estimatedPose = estimator.Update(result); } + ASSERT_TRUE(estimatedPose); frc::Pose3d pose = estimatedPose.value().estimatedPose; @@ -195,7 +197,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17)); + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_REFERENCE_POSE, {}); @@ -207,6 +209,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { estimatedPose = estimator.Update(result); } + ASSERT_TRUE(estimatedPose); frc::Pose3d pose = estimatedPose.value().estimatedPose; EXPECT_NEAR(17, units::unit_cast(estimatedPose.value().timestamp), @@ -245,7 +248,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17)); + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, {}); @@ -285,7 +288,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000}, targetsThree, std::nullopt}}; - cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(21)); + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); // std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -331,7 +334,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15)); + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, {}); @@ -341,6 +344,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { estimatedPose = estimator.Update(result); } + ASSERT_TRUE(estimatedPose); frc::Pose3d pose = estimatedPose.value().estimatedPose; EXPECT_NEAR(15.0, units::unit_cast(estimatedPose.value().timestamp), @@ -385,7 +389,7 @@ TEST(PhotonPoseEstimatorTest, PoseCache) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000}, std::vector{}, std::nullopt}}; - cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(1)); + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1)); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -397,13 +401,13 @@ TEST(PhotonPoseEstimatorTest, PoseCache) { // Set result, and update -- expect present and timestamp to be 15 cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 3000}, targets, std::nullopt}}; - cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15)); + cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); for (const auto& result : cameraOne.GetAllUnreadResults()) { estimatedPose = estimator.Update(result); } - EXPECT_TRUE(estimatedPose); + ASSERT_TRUE(estimatedPose); EXPECT_NEAR((15_s - 3_ms).to(), estimatedPose.value().timestamp.to(), 1e-6); @@ -414,3 +418,15 @@ TEST(PhotonPoseEstimatorTest, PoseCache) { EXPECT_FALSE(estimatedPose); } +TEST(PhotonPoseEstimatorTest, CopyResult) { + std::vector targets{}; + + auto testResult = photon::PhotonPipelineResult{ + photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}; + testResult.SetReceiveTimestamp(units::second_t(11)); + + auto test2 = testResult; + + EXPECT_NEAR(testResult.GetTimestamp().to(), + test2.GetTimestamp().to(), 0.001); +} 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 e84fb3883d..0a352cbfb4 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -41,7 +41,7 @@ public class PhotonPipelineResult public Optional multitagResult; // HACK: Since we don't trust NT time sync, keep track of when we got this packet into robot code - public long ntRecieveTimestampMicros = -1; + public long ntReceiveTimestampMicros = -1; /** Constructs an empty pipeline result. */ public PhotonPipelineResult() { @@ -160,26 +160,26 @@ public Optional getMultiTagResult() { } /** - * Returns the estimated time the frame was taken, in the recieved system's time base. This is - * calculated as (NT recieve time (robot base) - (publish timestamp, coproc timebase - capture + * 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)) * * @return The timestamp in seconds */ public double getTimestampSeconds() { - return (ntRecieveTimestampMicros + return (ntReceiveTimestampMicros - (metadata.publishTimestampMicros - metadata.captureTimestampMicros)) / 1e6; } - /** The time that the robot recieved this result, in the FPGA timebase. */ - public long getNtRecieveTimestampMicros() { - return ntRecieveTimestampMicros; + /** The time that the robot Received this result, in the FPGA timebase. */ + public long getNtReceiveTimestampMicros() { + return ntReceiveTimestampMicros; } - /** Sets the FPGA timestamp this result was recieved by robot code */ - public void setRecieveTimestampMicros(long timestampMicros) { - this.ntRecieveTimestampMicros = timestampMicros; + /** Sets the FPGA timestamp this result was Received by robot code */ + public void setReceiveTimestampMicros(long timestampMicros) { + this.ntReceiveTimestampMicros = timestampMicros; } @Override @@ -190,8 +190,8 @@ public String toString() { + targets + ", multitagResult=" + multitagResult - + ", ntRecieveTimestampMicros=" - + ntRecieveTimestampMicros + + ", ntReceiveTimestampMicros=" + + ntReceiveTimestampMicros + "]"; } @@ -202,7 +202,7 @@ public int hashCode() { result = prime * result + ((metadata == null) ? 0 : metadata.hashCode()); result = prime * result + ((targets == null) ? 0 : targets.hashCode()); result = prime * result + ((multitagResult == null) ? 0 : multitagResult.hashCode()); - result = prime * result + (int) (ntRecieveTimestampMicros ^ (ntRecieveTimestampMicros >>> 32)); + result = prime * result + (int) (ntReceiveTimestampMicros ^ (ntReceiveTimestampMicros >>> 32)); return result; } @@ -221,7 +221,7 @@ public boolean equals(Object obj) { if (multitagResult == null) { if (other.multitagResult != null) return false; } else if (!multitagResult.equals(other.multitagResult)) return false; - if (ntRecieveTimestampMicros != other.ntRecieveTimestampMicros) return false; + if (ntReceiveTimestampMicros != other.ntReceiveTimestampMicros) return false; return true; } diff --git a/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h b/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h index 631762bc89..b36a818159 100644 --- a/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h +++ b/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h @@ -17,7 +17,6 @@ #pragma once -#include #include #include #include @@ -28,6 +27,7 @@ #include #include +#include #include namespace photon { 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 108efa7417..a59c36a474 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -38,7 +38,37 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { using Base = PhotonPipelineResult_PhotonStruct; public: - explicit PhotonPipelineResult(Base&& data) : Base(data) {} + explicit PhotonPipelineResult(Base&& data) : Base(data) { + fmt::println("Hello, from a base-move ctor"); + } + + // Don't forget to deal with our ntRecieveTimestamp + PhotonPipelineResult(const PhotonPipelineResult& other) + : Base(other), ntReceiveTimestamp(other.ntReceiveTimestamp) { + fmt::println("Hello, from a const-copy ctor"); + } + PhotonPipelineResult(PhotonPipelineResult& other) + : Base(other), ntReceiveTimestamp(other.ntReceiveTimestamp) { + fmt::println("Hello, from a copy ctor"); + } + + PhotonPipelineResult(PhotonPipelineResult&& other) + : Base(std::move(other)), + ntReceiveTimestamp(std::move(other.ntReceiveTimestamp)) { + fmt::println("Hello, from a move ctor"); + } + auto operator=(PhotonPipelineResult& other) { + fmt::println("Hello from operator="); + Base::operator=(other); + ntReceiveTimestamp = other.ntReceiveTimestamp; + return *this; + } + auto operator=(PhotonPipelineResult other) { + fmt::println("Hello from operator="); + Base::operator=(other); + ntReceiveTimestamp = other.ntReceiveTimestamp; + return *this; + } template explicit PhotonPipelineResult(Args&&... args) @@ -80,7 +110,7 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { * with a timestamp. */ units::second_t GetTimestamp() const { - return ntRecieveTimestamp - GetLatency(); + return ntReceiveTimestamp - GetLatency(); } /** @@ -98,9 +128,9 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { */ int64_t SequenceID() const { return metadata.sequenceID; } - /** Sets the FPGA timestamp this result was recieved by robot code */ - void SetRecieveTimestamp(const units::second_t timestamp) { - this->ntRecieveTimestamp = timestamp; + /** Sets the FPGA timestamp this result was Received by robot code */ + void SetReceiveTimestamp(const units::second_t timestamp) { + this->ntReceiveTimestamp = timestamp; } /** @@ -122,7 +152,7 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { // Since we don't trust NT time sync, keep track of when we got this packet // into robot code - units::microsecond_t ntRecieveTimestamp = -1_s; + units::microsecond_t ntReceiveTimestamp = -1_s; inline static bool HAS_WARNED = false; }; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h index 0253f5c4cd..200bc43b59 100644 --- a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h @@ -79,10 +79,10 @@ class Vision { // In sim only, add our vision estimate to the sim debug field if (frc::RobotBase::IsSimulation()) { - if (visionEst.has_value()) { + if (visionEst) { GetSimDebugField() .GetObject("VisionEstimation") - ->SetPose(visionEst.value().estimatedPose.ToPose2d()); + ->SetPose(visionEst->estimatedPose.ToPose2d()); } else { GetSimDebugField().GetObject("VisionEstimation")->SetPoses({}); } @@ -101,9 +101,9 @@ class Vision { for (const auto& tgt : targets) { auto tagPose = photonEstimator.GetFieldLayout().GetTagPose(tgt.GetFiducialId()); - if (tagPose.has_value()) { + if (tagPose) { numTags++; - avgDist += tagPose.value().ToPose2d().Translation().Distance( + avgDist += tagPose->ToPose2d().Translation().Distance( estimatedPose.Translation()); } }