Skip to content
Open
12 changes: 4 additions & 8 deletions photon-lib/py/photonlibpy/targeting/photonPipelineResult.py
Original file line number Diff line number Diff line change
Expand Up @@ -42,15 +42,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
Expand Down
8 changes: 4 additions & 4 deletions photon-lib/py/test/photonPoseEstimator_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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()
Expand Down
8 changes: 4 additions & 4 deletions photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -239,10 +239,10 @@ 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());
// ntReceiveTimestamp records when the bytes arrived at robot code,
// independent of capture time. GetTimestamp() reads capture time
// directly from metadata.
result.SetReceiveTimestamp(wpi::units::microsecond_t(value.time));

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we still need this function?

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No we shouldn't. I'll go through and clean it up. Probably a few more things as well that can be cleaned up.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good catch — audited the whole path and you're right, the field was write-only across C++ and Python (and Java never had the analog at all). Folded a cleanup into d323b97: deletes the field, the setter, the bespoke copy/move/operator= members that existed solely to propagate it across the proto-generated Base, the 14 dead test calls in PhotonPoseEstimatorTest, and the Python sim's matching +10 no-op offset. Net +2/−87.


ret.push_back(result);
}
Expand Down
40 changes: 26 additions & 14 deletions photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {

cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
photon::PhotonPipelineMetadata{11'000'000, 11'000'000, 2000, 1000},
targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(11));

photon::PhotonPoseEstimator estimator(aprilTags, wpi::math::Transform3d{});
Expand Down Expand Up @@ -132,7 +133,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityIgnoresNonFiducialTargets) {

cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
photon::PhotonPipelineMetadata{11'000'000, 11'000'000, 2000, 1000},
targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(11));

photon::PhotonPoseEstimator estimator(aprilTags, wpi::math::Transform3d{});
Expand Down Expand Up @@ -191,7 +193,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {

cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
photon::PhotonPipelineMetadata{17'000'000, 17'000'000, 2000, 1000},
targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(17_s);

photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}});
Expand Down Expand Up @@ -242,7 +245,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {

cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
photon::PhotonPipelineMetadata{17'000'000, 17'000'000, 2000, 1000},
targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(17));

photon::PhotonPoseEstimator estimator(aprilTags, {});
Expand Down Expand Up @@ -295,7 +299,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {

cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
photon::PhotonPipelineMetadata{17'000'000, 17'000'000, 2000, 1000},
targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(17));

photon::PhotonPoseEstimator estimator(aprilTags, {});
Expand Down Expand Up @@ -334,8 +339,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
0.4, corners, detectedCorners}};

cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree,
std::nullopt}};
photon::PhotonPipelineMetadata{21'000'000, 21'000'000, 2000, 1000},
targetsThree, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(21));

for (const auto& result : cameraOne.GetAllUnreadResults()) {
Expand Down Expand Up @@ -383,6 +388,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
targets);
cameraOne.testResult = {result};
cameraOne.testResult[0].metadata.captureTimestampMicros = 17'000'000LL;
cameraOne.testResult[0].SetReceiveTimestamp(17_s);

estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
Expand Down Expand Up @@ -415,6 +421,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
targets);
cameraOne.testResult = {result};
cameraOne.testResult[0].metadata.captureTimestampMicros = 18'000'000LL;
cameraOne.testResult[0].SetReceiveTimestamp(18_s);

estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
Expand Down Expand Up @@ -471,7 +478,8 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {

cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
photon::PhotonPipelineMetadata{15'000'000, 15'000'000, 2000, 1000},
targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(15));

photon::PhotonPoseEstimator estimator(aprilTags, {});
Expand Down Expand Up @@ -509,7 +517,8 @@ TEST(PhotonPoseEstimatorTest,

cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
photon::PhotonPipelineMetadata{4'000'000, 4'000'000, 2000, 1000}, targets,
std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(4));

photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}});
Expand All @@ -531,7 +540,8 @@ TEST(PhotonPoseEstimatorTest,

cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
photon::PhotonPipelineMetadata{17'000'000, 17'000'000, 2000, 1000},
targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(17));

photon::PhotonPoseEstimator estimator(aprilTags, {});
Expand Down Expand Up @@ -565,7 +575,8 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) {

cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
photon::PhotonPipelineMetadata{11'000'000, 11'000'000, 2000, 1000},
targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(11));

photon::PhotonPoseEstimator estimator(aprilTags, wpi::math::Transform3d{});
Expand Down Expand Up @@ -596,7 +607,8 @@ TEST(PhotonPoseEstimatorTest, CopyResult) {
std::vector<photon::PhotonTrackedTarget> targets{};

auto testResult = photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt};
photon::PhotonPipelineMetadata{11'000'000, 11'000'000, 2000, 1000},
targets, std::nullopt};
testResult.SetReceiveTimestamp(wpi::units::second_t(11));

auto test2 = testResult;
Expand Down Expand Up @@ -651,8 +663,8 @@ 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};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -92,15 +92,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.
* time base (nt::Now). The robot shall run a server, so this is FPGA-relative
* on a real robot. Reads metadata.captureTimestampMicros directly — latency
* compensation is already baked in by the coprocessor.

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah I asked it to update comments. Been using it for comments and commit messages to try and speed things up. I'll try to be more careful.

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

To be clear -- our current posture on LLM use is that it's allowed, just that if you LLM write code to please call it out in the MR description so I know how/what to review for. LLMs make different mistakes from people and that changes how and what I review for, and it's helpful to know.

In this case my review comment is more about the comment talking about implementation details and not (IMO) adding value. Not that my old comment was very helpful :(

* @return The timestamp in seconds.
*/
wpi::units::second_t GetTimestamp() const {
Comment thread
JosephTLockwood marked this conversation as resolved.
return ntReceiveTimestamp - GetLatency();
return wpi::units::microsecond_t{
static_cast<double>(metadata.captureTimestampMicros)};
}

/**
Expand Down
Loading