From 3410918cbbbe50c8434ef803aa1f40994e3e2697 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 5 Mar 2026 01:48:49 -0500 Subject: [PATCH 01/62] began transferring camera transform to photoncamera --- .../java/org/photonvision/PhotonCamera.java | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index d548e17f09..6564490ddc 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -32,6 +32,7 @@ import edu.wpi.first.math.numbers.*; import edu.wpi.first.networktables.BooleanPublisher; import edu.wpi.first.networktables.BooleanSubscriber; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.DoubleArraySubscriber; import edu.wpi.first.networktables.IntegerEntry; import edu.wpi.first.networktables.IntegerPublisher; @@ -63,6 +64,7 @@ public class PhotonCamera implements AutoCloseable { private final NetworkTable cameraTable; PacketSubscriber resultSubscriber; + Optional robotToCamera; BooleanPublisher driverModePublisher; BooleanSubscriber driverModeSubscriber; IntegerPublisher fpsLimitPublisher; @@ -133,9 +135,37 @@ public static void setVersionCheckEnabled(boolean enabled) { * simulation, but should *usually* be the default NTInstance from * NetworkTableInstance::getDefault * @param cameraName The name of the camera, as seen in the UI. + * @param robotToCamera The transform from the robot to the camera. This is used for pose estimation + */ + public PhotonCamera(NetworkTableInstance instance, String cameraName, Transform3d robotToCamera) { + this(instance, cameraName, Optional.of(robotToCamera)); + } + + /** + * Constructs a PhotonCamera from a root table. + * + * @param instance The NetworkTableInstance to pull data from. This can be a custom instance in + * simulation, but should *usually* be the default NTInstance from + * NetworkTableInstance::getDefault + * @param cameraName The name of the camera, as seen in the UI. + * @param robotToCamera The transform from the robot to the camera. This is used for pose estimation */ public PhotonCamera(NetworkTableInstance instance, String cameraName) { + this(instance, cameraName, Optional.empty()); + } + + /** + * Internal implementation of the constructor + * + * @param instance The NetworkTableInstance to pull data from. This can be a custom instance in + * simulation, but should *usually* be the default NTInstance from + * NetworkTableInstance::getDefault + * @param cameraName The name of the camera, as seen in the UI. + * @param robotToCamera The transform from the robot to the camera. This is used for pose estimation + */ + private PhotonCamera(NetworkTableInstance instance, String cameraName, Optional robotToCamera) { name = cameraName; + this.robotToCamera = robotToCamera; disconnectAlert = new Alert( PHOTON_ALERT_GROUP, "PhotonCamera '" + name + "' is disconnected.", AlertType.kWarning); @@ -393,6 +423,12 @@ public int getPipelineIndex() { return (int) pipelineIndexState.get(0); } + /** + * Returns the robot-to-camera transform. + * + * @return The robot-to-camera transform, if it is set. Optional.empty() otherwise. + */ + /** * Allows the user to select the active pipeline index. * From 522d464b07e948b937da6d1334569368abd9079d Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 5 Mar 2026 11:04:12 -0500 Subject: [PATCH 02/62] edited photon-serde message for PipelineResult, added camera transform to PhotonPipelineResult --- .../java/org/photonvision/PhotonCamera.java | 37 ++++++++++++++++++- photon-serde/messages.yaml | 3 ++ .../targeting/PhotonPipelineResult.java | 27 +++++++++++--- .../targeting/PhotonPipelineResultTest.java | 16 ++++++-- 4 files changed, 72 insertions(+), 11 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 6564490ddc..a29b7e5f6a 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -34,6 +34,7 @@ import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.DoubleArraySubscriber; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.networktables.IntegerEntry; import edu.wpi.first.networktables.IntegerPublisher; import edu.wpi.first.networktables.IntegerSubscriber; @@ -53,6 +54,7 @@ import org.opencv.core.Core; import org.photonvision.common.hardware.VisionLEDMode; import org.photonvision.common.networktables.PacketSubscriber; +import org.photonvision.common.networktables.PacketPublisher; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.timesync.TimeSyncSingleton; @@ -65,6 +67,7 @@ public class PhotonCamera implements AutoCloseable { private final NetworkTable cameraTable; PacketSubscriber resultSubscriber; Optional robotToCamera; + StructPublisher robotToCameraPublisher; BooleanPublisher driverModePublisher; BooleanSubscriber driverModeSubscriber; IntegerPublisher fpsLimitPublisher; @@ -84,6 +87,7 @@ public void close() { resultSubscriber.close(); driverModePublisher.close(); driverModeSubscriber.close(); + robotToCameraPublisher.close(); fpsLimitPublisher.close(); fpsLimitSubscriber.close(); versionEntry.close(); @@ -173,7 +177,7 @@ private PhotonCamera(NetworkTableInstance instance, String cameraName, Optional< rootPhotonTable = instance.getTable(kTableName); this.cameraTable = rootPhotonTable.getSubTable(cameraName); path = cameraTable.getPath(); - var rawBytesEntry = + var rawBytesEntry_pipelineResult = cameraTable .getRawTopic("rawBytes") .subscribe( @@ -182,7 +186,8 @@ private PhotonCamera(NetworkTableInstance instance, String cameraName, Optional< PubSubOption.periodic(0.01), PubSubOption.sendAll(true), PubSubOption.pollStorage(20)); - resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.photonStruct); + resultSubscriber = new PacketSubscriber<>(rawBytesEntry_pipelineResult, PhotonPipelineResult.photonStruct); + robotToCameraPublisher = cameraTable.getStructTopic("robotToCamera", Transform3d.struct).publish(); driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish(); driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false); fpsLimitPublisher = cameraTable.getIntegerTopic("fpsLimitRequest").publish(); @@ -214,6 +219,15 @@ private PhotonCamera(NetworkTableInstance instance, String cameraName, Optional< // HACK - check if things are compatible verifyDependencies(); + + robotToCamera.ifPresent(this::publishRobotToCameraTransform); + } + + private void publishRobotToCameraTransform() { + robotToCamera.ifPresentOrElse( + (transform) -> robotToCameraPublisher.set(transform), + () -> robotToCameraPublisher.set(new Transform3d()) // TODO: See if this default value can cause any issues. + ); } static void verifyDependencies() { @@ -530,6 +544,25 @@ public final NetworkTable getCameraTable() { return cameraTable; } + /** + * Returns the camera's transform from the robot + * + * @return The camera's transform from the robot, if it is set. Empty otherwise. + */ + public Optional getCameraTransform() { + return robotToCamera; + } + + /** + * Sets the camera's transform from the robot. This is used for pose estimation. + * + * @param robotToCamera The transform from the robot to the camera. + */ + public void setCameraTransform(Transform3d robotToCamera) { + this.robotToCamera = Optional.of(robotToCamera); + publishRobotToCameraTransform(); + } + void verifyVersion() { if (!VERSION_CHECK_ENABLED) return; diff --git a/photon-serde/messages.yaml b/photon-serde/messages.yaml index 340e771807..b822c849b7 100644 --- a/photon-serde/messages.yaml +++ b/photon-serde/messages.yaml @@ -92,3 +92,6 @@ - name: multitagResult type: MultiTargetPNPResult optional: True + - name: robotToCamera + type: Transform3d + optional: True 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 a599c9a5c8..9f62225a58 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -18,6 +18,7 @@ package org.photonvision.targeting; import edu.wpi.first.util.protobuf.ProtobufSerializable; +import edu.wpi.first.math.geometry.Transform3d; import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -40,9 +41,12 @@ public class PhotonPipelineResult /** The multitag result, if using an AprilTag pipeline with Multi-Target Estimation enabled. */ public Optional multitagResult; + /** The transform from the robot to the camera. */ + public Optional robotToCamera; + /** Constructs an empty pipeline result. */ public PhotonPipelineResult() { - this(new PhotonPipelineMetadata(), List.of(), Optional.empty()); + this(new PhotonPipelineMetadata(), List.of(), Optional.empty(), Optional.empty()); } /** @@ -66,7 +70,9 @@ public PhotonPipelineResult( new PhotonPipelineMetadata( captureTimestampMicros, publishTimestampMicros, sequenceID, timeSinceLastPong), targets, - Optional.empty()); + Optional.empty(), + Optional.empty() + ); } /** @@ -80,6 +86,7 @@ public PhotonPipelineResult( * @param timeSinceLastPong The time since the last Time Sync Pong in uS. * @param targets The list of targets identified by the pipeline. * @param result Result from multi-target PNP. + * @param robotToCamera The transform from the robot to the camera. */ public PhotonPipelineResult( long sequenceID, @@ -87,21 +94,28 @@ public PhotonPipelineResult( long publishTimestamp, long timeSinceLastPong, List targets, - Optional result) { + Optional result, + Optional robotToCamera + ) { this( new PhotonPipelineMetadata( captureTimestamp, publishTimestamp, sequenceID, timeSinceLastPong), targets, - result); + result, + robotToCamera + ); } public PhotonPipelineResult( PhotonPipelineMetadata metadata, List targets, - Optional result) { + Optional result, + Optional robotToCamera + ) { this.metadata = metadata; this.targets.addAll(targets); this.multitagResult = result; + this.robotToCamera = robotToCamera; } /** @@ -214,6 +228,9 @@ public boolean equals(Object obj) { if (multitagResult == null) { if (other.multitagResult != null) return false; } else if (!multitagResult.equals(other.multitagResult)) return false; + if (robotToCamera == null) { + if (other.robotToCamera != null) return false; + } else if (!robotToCamera.equals(other.robotToCamera)) return false; return true; } diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java index 95d9506c3e..b967f261c3 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java @@ -187,7 +187,9 @@ public void equalityTest() { new MultiTargetPNPResult( new PnpResult( new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), - List.of((short) 1, (short) 2, (short) 3)))); + List.of((short) 1, (short) 2, (short) 3))), + Optional.of( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); b = new PhotonPipelineResult( 3, @@ -241,7 +243,9 @@ public void equalityTest() { new MultiTargetPNPResult( new PnpResult( new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), - List.of((short) 1, (short) 2, (short) 3)))); + List.of((short) 1, (short) 2, (short) 3))), + Optional.of( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); assertEquals(a, b); } @@ -400,7 +404,9 @@ public void inequalityTest() { new MultiTargetPNPResult( new PnpResult( new Transform3d(new Translation3d(1, 8, 3), new Rotation3d(1, 2, 3)), 0.1), - List.of((short) 3, (short) 4, (short) 7)))); + List.of((short) 3, (short) 4, (short) 7))), + Optional.of( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); b = new PhotonPipelineResult( 3, @@ -454,7 +460,9 @@ public void inequalityTest() { new MultiTargetPNPResult( new PnpResult( new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), - List.of((short) 1, (short) 2, (short) 3)))); + List.of((short) 1, (short) 2, (short) 3))), + Optional.of( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); assertNotEquals(a, b); } } From 984521686e2d4b2b7ac5d9b9fe9d85061558b6f5 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 5 Mar 2026 11:25:39 -0500 Subject: [PATCH 03/62] updated PipelineResult protobuf --- .../common/dataflow/networktables/NTDataPublisher.java | 5 ++++- .../targeting/proto/PhotonPipelineResultProto.java | 6 +++++- photon-targeting/src/main/proto/photon.proto | 2 ++ .../src/test/java/org/photonvision/PacketTest.java | 4 +++- .../targeting/proto/PhotonPipelineResultProtoTest.java | 4 +++- 5 files changed, 17 insertions(+), 4 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 7eff9733a0..2a21514b43 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -22,6 +22,7 @@ import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTablesJNI; import java.util.List; +import java.util.Optional; import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.Supplier; @@ -186,7 +187,9 @@ public void accept(CVPipelineResult result) { now + offset, NetworkTablesManager.getInstance().getTimeSinceLastPong(), TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets), - acceptedResult.multiTagResult); + acceptedResult.multiTagResult, + Optional.empty() // TODO: robotToCamera -- should pull this from the NT table, temporary solution for testing + ); // random guess at size of the array ts.resultPublisher.set(simplified, 1024); diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java index 97bf80e6cc..62ecd4a451 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java @@ -17,6 +17,7 @@ package org.photonvision.targeting.proto; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.util.protobuf.Protobuf; import java.util.Optional; import org.photonvision.proto.Photon.ProtobufPhotonPipelineResult; @@ -52,7 +53,10 @@ public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) { PhotonTrackedTarget.proto.unpack(msg.getTargets()), msg.hasMultiTargetResult() ? Optional.of(MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult())) - : Optional.empty()); + : Optional.empty(), + msg.hasRobotToCamera() + ? Optional.of(Transform3d.proto.unpack(msg.getRobotToCamera())) + : Optional.empty()); } @Override diff --git a/photon-targeting/src/main/proto/photon.proto b/photon-targeting/src/main/proto/photon.proto index dea75aab07..539c03042b 100644 --- a/photon-targeting/src/main/proto/photon.proto +++ b/photon-targeting/src/main/proto/photon.proto @@ -67,6 +67,8 @@ message ProtobufPhotonPipelineResult { int64 capture_timestamp_micros = 5; int64 nt_publish_timestamp_micros = 6; int64 time_since_last_pong_micros = 7; + + optional wpi.proto.ProtobufTransform3d robotToCamera = 8; } message ProtobufDeviceMetrics { diff --git a/photon-targeting/src/test/java/org/photonvision/PacketTest.java b/photon-targeting/src/test/java/org/photonvision/PacketTest.java index 4b1b140d16..9fc89b59ff 100644 --- a/photon-targeting/src/test/java/org/photonvision/PacketTest.java +++ b/photon-targeting/src/test/java/org/photonvision/PacketTest.java @@ -155,7 +155,9 @@ void pipelineResultSerde() { new MultiTargetPNPResult( new PnpResult( new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), - List.of((short) 1, (short) 2, (short) 3)))); + List.of((short) 1, (short) 2, (short) 3))), + Optional.of( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); var p3 = new Packet(10); PhotonPipelineResult.photonStruct.pack(p3, ret3); var unpackedRet3 = PhotonPipelineResult.photonStruct.unpack(p3); diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java index a60dddbed8..05b2d77456 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java @@ -146,7 +146,9 @@ public void protobufTest() { new MultiTargetPNPResult( new PnpResult( new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), - List.of((short) 1, (short) 2, (short) 3)))); + List.of((short) 1, (short) 2, (short) 3))), + Optional.of( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); serializedResult = PhotonPipelineResult.proto.createMessage(); PhotonPipelineResult.proto.pack(serializedResult, result); unpackedResult = PhotonPipelineResult.proto.unpack(serializedResult); From 955d597b5ab87abe903f9038459ac78645dfa261 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 5 Mar 2026 17:12:30 -0500 Subject: [PATCH 04/62] fixed photoncamera constructor --- photon-lib/src/main/java/org/photonvision/PhotonCamera.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index a29b7e5f6a..d215763021 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -220,7 +220,9 @@ private PhotonCamera(NetworkTableInstance instance, String cameraName, Optional< // HACK - check if things are compatible verifyDependencies(); - robotToCamera.ifPresent(this::publishRobotToCameraTransform); + if (robotToCamera.isPresent()) { + publishRobotToCameraTransform(); + } } private void publishRobotToCameraTransform() { From b27d8afc25a81b1c041affb1ff07008ee26c5554 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 5 Mar 2026 17:30:18 -0500 Subject: [PATCH 05/62] fixed photoncamerasim --- .../java/org/photonvision/simulation/PhotonCameraSim.java | 4 +++- .../java/org/photonvision/LegacyPhotonPoseEstimatorTest.java | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) 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 c4331ba8cd..bcb0946102 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -649,7 +649,9 @@ public PhotonPipelineResult process( // Pretend like we heard a pong recently 1000L + (long) ((Math.random() - 0.5) * 50), detectableTgts, - multitagResult); + multitagResult, + cam.getCameraTransform() + ); return ret; } diff --git a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java index 9e8f337462..dfb882bf6f 100644 --- a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java @@ -880,7 +880,8 @@ public void testConstrainedPnpOneTag() { -0.08413452932300695, 0.9130568172784148))), 0.1), - new ArrayList(8)))); + new ArrayList(8))), + Optional.of(new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(4, 5, 6)))); final double camPitch = Units.degreesToRadians(30.0); final Transform3d kRobotToCam = From a9cb620cc92285c3cc46450fe4ccb8f050d46703 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 5 Mar 2026 18:09:04 -0500 Subject: [PATCH 06/62] added back some PhotonPipelineResult constructors for backwards compatibility --- .../targeting/PhotonPipelineResult.java | 39 +++++++++++++++++++ 1 file changed, 39 insertions(+) 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 9f62225a58..ffd1b296a7 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -75,6 +75,45 @@ public PhotonPipelineResult( ); } + + /** + * Constructs a pipeline result. + * + * @param sequenceID The number of frames processed by this camera since boot + * @param captureTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor + * captured the image this result contains the targeting info of + * @param publishTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor + * published targeting info + * @param timeSinceLastPong The time since the last Time Sync Pong in uS. + * @param targets The list of targets identified by the pipeline. + * @param result Result from multi-target PNP. + * @param robotToCamera The transform from the robot to the camera. + */ + public PhotonPipelineResult( + long sequenceID, + long captureTimestamp, + long publishTimestamp, + long timeSinceLastPong, + List targets, + Optional result + ) { + this( + new PhotonPipelineMetadata( + captureTimestamp, publishTimestamp, sequenceID, timeSinceLastPong), + targets, + result, + Optional.empty() + ); + } + + public PhotonPipelineResult( + PhotonPipelineMetadata metadata, + List targets, + Optional result + ) { + this(metadata, targets, result, Optional.empty()); + } + /** * Constructs a pipeline result. * From 47272a2595f5e6adf5529a9bb7afb0202cc763b3 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 5 Mar 2026 20:11:01 -0500 Subject: [PATCH 07/62] worked on java serde for the new PipelineResult --- .../configuration/CameraConfiguration.java | 13 +++ .../networktables/NTDataPublisher.java | 18 +++- .../vision/processes/VisionModule.java | 18 +++- .../processes/VisionSourceSettables.java | 7 ++ .../generated/PhotonPipelineResultSerde.py | 11 ++- .../generated/RobotToCameraTransformSerde.py | 61 +++++++++++++ photon-serde/messages.yaml | 6 +- .../struct/PhotonPipelineResultSerde.java | 12 ++- .../struct/RobotToCameraTransformSerde.java | 85 +++++++++++++++++++ .../serde/PhotonPipelineResultSerde.cpp | 2 + .../serde/RobotToCameraTransformSerde.cpp | 43 ++++++++++ .../photon/serde/PhotonPipelineResultSerde.h | 5 +- .../serde/RobotToCameraTransformSerde.h | 58 +++++++++++++ .../struct/PhotonPipelineResultStruct.h | 2 + .../struct/RobotToCameraTransformStruct.h | 42 +++++++++ .../common/networktables/NTTopicSet.java | 11 +++ .../targeting/PhotonPipelineResult.java | 16 +++- .../targeting/RobotToCameraTransform.java | 43 ++++++++++ .../proto/RobotToCameraTransformProto.java | 37 ++++++++ photon-targeting/src/main/proto/photon.proto | 4 + 20 files changed, 481 insertions(+), 13 deletions(-) create mode 100644 photon-lib/py/photonlibpy/generated/RobotToCameraTransformSerde.py create mode 100644 photon-targeting/src/generated/main/java/org/photonvision/struct/RobotToCameraTransformSerde.java create mode 100644 photon-targeting/src/generated/main/native/cpp/photon/serde/RobotToCameraTransformSerde.cpp create mode 100644 photon-targeting/src/generated/main/native/include/photon/serde/RobotToCameraTransformSerde.h create mode 100644 photon-targeting/src/generated/main/native/include/photon/struct/RobotToCameraTransformStruct.h create mode 100644 photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java create mode 100644 photon-targeting/src/main/java/org/photonvision/targeting/proto/RobotToCameraTransformProto.java diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java index e51175f36c..981d409826 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java @@ -21,6 +21,8 @@ import com.fasterxml.jackson.annotation.JsonIgnore; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.cscore.UsbCameraInfo; +import edu.wpi.first.math.geometry.Transform3d; + import java.util.ArrayList; import java.util.List; import java.util.UUID; @@ -56,6 +58,8 @@ public class CameraConfiguration { public QuirkyCamera cameraQuirks; + public Transform3d robotToCamera = null; + public double FOV = 70; public List calibrations = new ArrayList<>(); public int currentPipelineIndex = 0; @@ -208,6 +212,15 @@ public void removeCalibration(Size unrotatedImageSize) { }); } + /** + * Set the robot to camera transform for this camera configuration. + * + * @param robotToCamera the transform from the robot's origin to the camera's origin, in the robot's coordinate system. + */ + public void setRobotToCamera(Transform3d robotToCamera) { + this.robotToCamera = robotToCamera; + } + /** * cscore will auto-reconnect to the camera path we give it. v4l does not guarantee that if i swap * cameras around, the same /dev/videoN ID will be assigned to that camera. So instead default to diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 2a21514b43..136790056b 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -56,6 +56,9 @@ public class NTDataPublisher implements CVPipelineResultConsumer { private final Consumer fpsLimitConsumer; private final Supplier fpsLimitSupplier; + NTDataChangeListener robotToCameraListener; + private final Consumer robotToCameraConsumer; + public NTDataPublisher( String cameraNickname, Supplier pipelineIndexSupplier, @@ -63,13 +66,16 @@ public NTDataPublisher( BooleanSupplier driverModeSupplier, Consumer driverModeConsumer, Supplier fpsLimitSupplier, - Consumer fpsLimitConsumer) { + Consumer fpsLimitConsumer, + Consumer robotToCameraConsumer + ) { this.pipelineIndexSupplier = pipelineIndexSupplier; this.pipelineIndexConsumer = pipelineIndexConsumer; this.driverModeSupplier = driverModeSupplier; this.driverModeConsumer = driverModeConsumer; this.fpsLimitSupplier = fpsLimitSupplier; this.fpsLimitConsumer = fpsLimitConsumer; + this.robotToCameraConsumer = robotToCameraConsumer; updateCameraNickname(cameraNickname); updateEntries(); @@ -99,6 +105,12 @@ private void onPipelineIndexChange(NetworkTableEvent entryNotification) { logger.debug("Set pipeline index to " + newIndex); } + private void onRobotToCameraChange(NetworkTableEvent entryNotification) { + var newRobotToCamera = (Transform3d) entryNotification.valueData.value.getValue(); + robotToCameraConsumer.accept(newRobotToCamera); + logger.debug("Updated robot to camera transform to " + newRobotToCamera); + } + private void onDriverModeChange(NetworkTableEvent entryNotification) { var newDriverMode = entryNotification.valueData.value.getBoolean(); var originalDriverMode = driverModeSupplier.getAsBoolean(); @@ -149,6 +161,10 @@ private void updateEntries() { fpsLimitListener = new NTDataChangeListener( ts.subTable.getInstance(), ts.fpsLimitSubscriber, this::onFPSLimitChange); + + robotToCameraListener = + new NTDataChangeListener( + ts.subTable.getInstance(), ts.robotToCameraSubscriber, this::onRobotToCameraChange); } public void updateCameraNickname(String newCameraNickname) { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index b57b01d969..744b2bfaf0 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -19,6 +19,7 @@ import edu.wpi.first.cscore.CameraServerJNI; import edu.wpi.first.cscore.VideoException; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.util.Units; import io.javalin.websocket.WsContext; import java.util.ArrayList; @@ -89,6 +90,8 @@ public class VisionModule { private int fpsLimit = -1; + private Transform3d robotToCameraTransform = null; + FileSaveFrameConsumer inputFrameSaver; FileSaveFrameConsumer outputFrameSaver; @@ -153,7 +156,9 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource) pipelineManager::getDriverMode, this::setDriverMode, this::getFPSLimit, - this::setFPSLimit); + this::setFPSLimit, + this::setRobotToCameraTransform + ); uiDataConsumer = new UIDataPublisher(visionSource.getSettables().getConfiguration().uniqueName); statusLEDsConsumer = new StatusLEDConsumer(visionSource.getSettables().getConfiguration().uniqueName); @@ -635,6 +640,17 @@ public void setFPSLimit(int fps) { saveAndBroadcastAll(); } + /** + * Set camera transform for this vision module. As of now this doesn't affect vision processing in any way + * + * @param robotToCameraTransform the transform from the robot's origin to the camera's origin, in the robot's coordinate system. This should be provided in meters. + */ + + public void setRobotToCameraTransform(Transform3d robotToCameraTransform) { + this.robotToCameraTransform = robotToCameraTransform; + saveAndBroadcastAll(); + } + /** * Get the current FPS limit for this vision module. This limit cannot be exceeded, but may be * lower depending on processing time. diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index 331ea10b8b..f4cb7c884c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java @@ -18,6 +18,8 @@ package org.photonvision.vision.processes; import edu.wpi.first.cscore.VideoMode; +import edu.wpi.first.math.geometry.Transform3d; + import java.util.HashMap; import org.opencv.core.Size; import org.photonvision.common.configuration.CameraConfiguration; @@ -126,6 +128,11 @@ public void removeCalibration(Size unrotatedImageSize) { calculateFrameStaticProps(); } + public void setRobotToCamera(Transform3d robotToCamera) { + configuration.setRobotToCamera(robotToCamera); + // calculateFrameStaticProps(); // This shouldn't be necessary + } + protected void calculateFrameStaticProps() { var videoMode = getCurrentVideoMode(); this.frameStaticProperties = diff --git a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py index 885ce2c963..8b8b94f520 100644 --- a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py +++ b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py @@ -37,12 +37,13 @@ from ..targeting import PhotonPipelineMetadata # noqa from ..targeting import PhotonPipelineResult # noqa from ..targeting import PhotonTrackedTarget # noqa + from ..targeting import RobotToCameraTransform # noqa class PhotonPipelineResultSerde: # Message definition md5sum. See photon_packet.adoc for details - MESSAGE_VERSION = "4b2ff16a964b5e2bf04be0c1454d91c4" - MESSAGE_FORMAT = "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;" + MESSAGE_VERSION = "b0040327f63abf872824e05641cbdede" + MESSAGE_FORMAT = "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;optional RobotToCameraTransform:575b4e398df72967da55b383dfe7784d robotToCamera;" @staticmethod def pack(value: "PhotonPipelineResult") -> "Packet": @@ -56,6 +57,9 @@ def pack(value: "PhotonPipelineResult") -> "Packet": # multitagResult is optional! it better not be a VLA too ret.encodeOptional(value.multitagResult, MultiTargetPNPResult.photonStruct) + + # robotToCamera is optional! it better not be a VLA too + ret.encodeOptional(value.robotToCamera, RobotToCameraTransform.photonStruct) return ret @staticmethod @@ -71,6 +75,9 @@ def unpack(packet: "Packet") -> "PhotonPipelineResult": # multitagResult is optional! it better not be a VLA too ret.multitagResult = packet.decodeOptional(MultiTargetPNPResult.photonStruct) + # robotToCamera is optional! it better not be a VLA too + ret.robotToCamera = packet.decodeOptional(RobotToCameraTransform.photonStruct) + return ret diff --git a/photon-lib/py/photonlibpy/generated/RobotToCameraTransformSerde.py b/photon-lib/py/photonlibpy/generated/RobotToCameraTransformSerde.py new file mode 100644 index 0000000000..5790a3370a --- /dev/null +++ b/photon-lib/py/photonlibpy/generated/RobotToCameraTransformSerde.py @@ -0,0 +1,61 @@ +# +# MIT License +# +# Copyright (c) PhotonVision +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +# + +############################################################################### +## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. +## --> DO NOT MODIFY <-- +############################################################################### + +from typing import TYPE_CHECKING + +from ..packet import Packet +from ..targeting import * # noqa + +if TYPE_CHECKING: + from ..targeting import RobotToCameraTransform # noqa + + +class RobotToCameraTransformSerde: + # Message definition md5sum. See photon_packet.adoc for details + MESSAGE_VERSION = "575b4e398df72967da55b383dfe7784d" + MESSAGE_FORMAT = "Transform3d robotToCamera;" + + @staticmethod + def pack(value: "RobotToCameraTransform") -> "Packet": + ret = Packet() + + ret.encodeTransform(value.robotToCamera) + return ret + + @staticmethod + def unpack(packet: "Packet") -> "RobotToCameraTransform": + ret = RobotToCameraTransform() + + ret.robotToCamera = packet.decodeTransform() + + return ret + + +# Hack ourselves into the base class +RobotToCameraTransform.photonStruct = RobotToCameraTransformSerde() diff --git a/photon-serde/messages.yaml b/photon-serde/messages.yaml index b822c849b7..7877577345 100644 --- a/photon-serde/messages.yaml +++ b/photon-serde/messages.yaml @@ -81,6 +81,10 @@ type: int16 vla: True +- name: RobotToCameraTransform + fields: + - name: robotToCamera + type: Transform3d - name: PhotonPipelineResult fields: @@ -93,5 +97,5 @@ type: MultiTargetPNPResult optional: True - name: robotToCamera - type: Transform3d + type: RobotToCameraTransform optional: True diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java index 5f9134fda8..673b2a98ae 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java @@ -43,9 +43,9 @@ public class PhotonPipelineResultSerde implements PacketSerde { @Override - public final String getInterfaceUUID() { return "4b2ff16a964b5e2bf04be0c1454d91c4"; } + public final String getInterfaceUUID() { return "b0040327f63abf872824e05641cbdede"; } @Override - public final String getSchema() { return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"; } + public final String getSchema() { return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;optional RobotToCameraTransform:575b4e398df72967da55b383dfe7784d robotToCamera;"; } @Override public final String getTypeName() { return "PhotonPipelineResult"; } @@ -65,6 +65,9 @@ public void pack(Packet packet, PhotonPipelineResult value) { // multitagResult is optional! it better not be a VLA too packet.encodeOptional(value.multitagResult); + + // robotToCamera is optional! it better not be a VLA too + packet.encodeOptional(value.robotToCamera); } @Override @@ -80,13 +83,16 @@ public PhotonPipelineResult unpack(Packet packet) { // multitagResult is optional! it better not be a VLA too ret.multitagResult = packet.decodeOptional(MultiTargetPNPResult.photonStruct); + // robotToCamera is optional! it better not be a VLA too + ret.robotToCamera = packet.decodeOptional(RobotToCameraTransform.photonStruct); + return ret; } @Override public PacketSerde[] getNestedPhotonMessages() { return new PacketSerde[] { - MultiTargetPNPResult.photonStruct,PhotonPipelineMetadata.photonStruct,PhotonTrackedTarget.photonStruct + MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct,RobotToCameraTransform.photonStruct,PhotonPipelineMetadata.photonStruct }; } diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/RobotToCameraTransformSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/RobotToCameraTransformSerde.java new file mode 100644 index 0000000000..4c0afa99b0 --- /dev/null +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/RobotToCameraTransformSerde.java @@ -0,0 +1,85 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY + +package org.photonvision.struct; + +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.dataflow.structures.PacketSerde; +import org.photonvision.utils.PacketUtils; + +// Assume that the base class lives here and we can import it +import org.photonvision.targeting.*; + +// WPILib imports (if any) +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.math.geometry.Transform3d; + +/** + * Auto-generated serialization/deserialization helper for RobotToCameraTransform + */ +public class RobotToCameraTransformSerde implements PacketSerde { + + @Override + public final String getInterfaceUUID() { return "575b4e398df72967da55b383dfe7784d"; } + @Override + public final String getSchema() { return "Transform3d robotToCamera;"; } + @Override + public final String getTypeName() { return "RobotToCameraTransform"; } + + @Override + public int getMaxByteSize() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'"); + } + + @Override + public void pack(Packet packet, RobotToCameraTransform value) { + PacketUtils.packTransform3d(packet, value.robotToCamera); + } + + @Override + public RobotToCameraTransform unpack(Packet packet) { + var ret = new RobotToCameraTransform(); + + ret.robotToCamera = PacketUtils.unpackTransform3d(packet); + + return ret; + } + + @Override + public PacketSerde[] getNestedPhotonMessages() { + return new PacketSerde[] { + + }; + } + + @Override + public Struct[] getNestedWpilibMessages() { + return new Struct[] { + Transform3d.struct + }; + } +} diff --git a/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineResultSerde.cpp b/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineResultSerde.cpp index eebb222826..951cbe15a0 100644 --- a/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineResultSerde.cpp +++ b/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineResultSerde.cpp @@ -34,6 +34,7 @@ void StructType::Pack(Packet& packet, const PhotonPipelineResult& value) { packet.Pack(value.metadata); packet.Pack>(value.targets); packet.Pack>(value.multitagResult); + packet.Pack>(value.robotToCamera); } PhotonPipelineResult StructType::Unpack(Packet& packet) { @@ -41,6 +42,7 @@ PhotonPipelineResult StructType::Unpack(Packet& packet) { .metadata = packet.Unpack(), .targets = packet.Unpack>(), .multitagResult = packet.Unpack>(), + .robotToCamera = packet.Unpack>(), }}; } diff --git a/photon-targeting/src/generated/main/native/cpp/photon/serde/RobotToCameraTransformSerde.cpp b/photon-targeting/src/generated/main/native/cpp/photon/serde/RobotToCameraTransformSerde.cpp new file mode 100644 index 0000000000..4c2215add6 --- /dev/null +++ b/photon-targeting/src/generated/main/native/cpp/photon/serde/RobotToCameraTransformSerde.cpp @@ -0,0 +1,43 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY + +#include "photon/serde/RobotToCameraTransformSerde.h" + +namespace photon { + +using StructType = SerdeType; + +void StructType::Pack(Packet& packet, const RobotToCameraTransform& value) { + packet.Pack(value.robotToCamera); +} + +RobotToCameraTransform StructType::Unpack(Packet& packet) { + return RobotToCameraTransform{ RobotToCameraTransform_PhotonStruct{ + .robotToCamera = packet.Unpack(), + }}; +} + +} // namespace photon diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h index 836e347ca1..16e347c607 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h @@ -36,6 +36,7 @@ #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PhotonPipelineMetadata.h" #include "photon/targeting/PhotonTrackedTarget.h" +#include "photon/targeting/RobotToCameraTransform.h" #include #include #include @@ -46,11 +47,11 @@ namespace photon { template <> struct WPILIB_DLLEXPORT SerdeType { static constexpr std::string_view GetSchemaHash() { - return "4b2ff16a964b5e2bf04be0c1454d91c4"; + return "b0040327f63abf872824e05641cbdede"; } static constexpr std::string_view GetSchema() { - return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"; + return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;optional RobotToCameraTransform:575b4e398df72967da55b383dfe7784d robotToCamera;"; } static photon::PhotonPipelineResult Unpack(photon::Packet& packet); diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/RobotToCameraTransformSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/RobotToCameraTransformSerde.h new file mode 100644 index 0000000000..4f45f3d619 --- /dev/null +++ b/photon-targeting/src/generated/main/native/include/photon/serde/RobotToCameraTransformSerde.h @@ -0,0 +1,58 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY + +#include + +// Include myself +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/RobotToCameraTransform.h" + +// Includes for dependant types +#include +#include + + +namespace photon { + +template <> +struct WPILIB_DLLEXPORT SerdeType { + static constexpr std::string_view GetSchemaHash() { + return "575b4e398df72967da55b383dfe7784d"; + } + + static constexpr std::string_view GetSchema() { + return "Transform3d robotToCamera;"; + } + + static photon::RobotToCameraTransform Unpack(photon::Packet& packet); + static void Pack(photon::Packet& packet, const photon::RobotToCameraTransform& value); +}; + +static_assert(photon::PhotonStructSerializable); + +} // namespace photon diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h index 0d4fa4ac37..764c403d16 100644 --- a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h +++ b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h @@ -30,6 +30,7 @@ #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PhotonPipelineMetadata.h" #include "photon/targeting/PhotonTrackedTarget.h" +#include "photon/targeting/RobotToCameraTransform.h" #include #include #include @@ -41,6 +42,7 @@ struct PhotonPipelineResult_PhotonStruct { photon::PhotonPipelineMetadata metadata; std::vector targets; std::optional multitagResult; + std::optional robotToCamera; friend bool operator==(PhotonPipelineResult_PhotonStruct const&, PhotonPipelineResult_PhotonStruct const&) = default; }; diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/RobotToCameraTransformStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/RobotToCameraTransformStruct.h new file mode 100644 index 0000000000..575f2efce7 --- /dev/null +++ b/photon-targeting/src/generated/main/native/include/photon/struct/RobotToCameraTransformStruct.h @@ -0,0 +1,42 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY + +// Includes for dependant types +#include +#include + + +namespace photon { + +struct RobotToCameraTransform_PhotonStruct { + frc::Transform3d robotToCamera; + + friend bool operator==(RobotToCameraTransform_PhotonStruct const&, RobotToCameraTransform_PhotonStruct const&) = default; +}; + +} // namespace photon diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index 5b3d50d0fc..3577f33d3e 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -30,6 +30,8 @@ import edu.wpi.first.networktables.ProtobufPublisher; import edu.wpi.first.networktables.PubSubOption; import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.networktables.StructSubscriber; + import org.photonvision.targeting.PhotonPipelineResult; /** @@ -78,6 +80,10 @@ public class NTTopicSet { public DoubleArrayPublisher cameraIntrinsicsPublisher; public DoubleArrayPublisher cameraDistortionPublisher; + // Camera Intrinsics + public StructSubscriber robotToCameraSubscriber; + public StructPublisher robotToCameraPublisher; + public void updateEntries() { var rawBytesEntry = subTable @@ -127,6 +133,10 @@ public void updateEntries() { cameraIntrinsicsPublisher = subTable.getDoubleArrayTopic("cameraIntrinsics").publish(); cameraDistortionPublisher = subTable.getDoubleArrayTopic("cameraDistortion").publish(); + robotToCameraSubscriber = + subTable + .getStructTopic("robotToCamera", Transform3d.struct) + .subscribe(null); } @SuppressWarnings("DuplicatedCode") @@ -156,5 +166,6 @@ public void removeEntries() { if (cameraIntrinsicsPublisher != null) cameraIntrinsicsPublisher.close(); if (cameraDistortionPublisher != null) cameraDistortionPublisher.close(); + if (robotToCameraSubscriber != null) robotToCameraSubscriber.close(); } } 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 ffd1b296a7..9856389a15 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -41,8 +41,8 @@ public class PhotonPipelineResult /** The multitag result, if using an AprilTag pipeline with Multi-Target Estimation enabled. */ public Optional multitagResult; - /** The transform from the robot to the camera. */ - public Optional robotToCamera; + /** The transform from the robot to the camera. This is an optional value, but photonserde doesn't seem to support optional Transform3ds*/ + public Optional robotToCamera; /** Constructs an empty pipeline result. */ public PhotonPipelineResult() { @@ -154,7 +154,7 @@ public PhotonPipelineResult( this.metadata = metadata; this.targets.addAll(targets); this.multitagResult = result; - this.robotToCamera = robotToCamera; + this.robotToCamera = robotToCamera.map(RobotToCameraTransform::new); } /** @@ -220,6 +220,16 @@ public Optional getMultiTagResult() { return multitagResult; } + /** + * Return the robot to camera transform. Be sure to check {@code getRobotToCamera().isPresent()} + * before using the transform! + * + * @return The robot to camera transform. Empty if no transform is set. + */ + public Optional getRobotToCamera() { + return robotToCamera.map(RobotToCameraTransform::unwrap); + } + /** * 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 diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java b/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java new file mode 100644 index 0000000000..fec1ae174c --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java @@ -0,0 +1,43 @@ +package org.photonvision.targeting; + +import org.photonvision.common.dataflow.structures.PacketSerde; +import org.photonvision.struct.MultiTargetPNPResultSerde; +import org.photonvision.struct.RobotToCameraTransformSerde; +import org.photonvision.targeting.serde.PhotonStructSerializable; +import org.photonvision.targeting.proto.RobotToCameraTransformProto; + +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.util.protobuf.ProtobufSerializable; + +// We need a wrapper class for photonserde, as this needs to be an optional value +public class RobotToCameraTransform + implements ProtobufSerializable, PhotonStructSerializable { + public Transform3d robotToCamera; + + public RobotToCameraTransform() { + this.robotToCamera = new Transform3d(); + } + + public RobotToCameraTransform(Transform3d robotToCamera) { + this.robotToCamera = robotToCamera; + } + + public Transform3d getTransform3d() { + return robotToCamera; + } + + /** RobotToCameraTransform protobuf for serialization. */ + public static final RobotToCameraTransformProto proto = new RobotToCameraTransformProto(); + + /** RobotToCameraTransform PhotonStruct for serialization. */ + public static final RobotToCameraTransformSerde photonStruct = new RobotToCameraTransformSerde(); + + @Override + public PacketSerde getSerde() { + return photonStruct; + } + + public static Transform3d unwrap(RobotToCameraTransform transform) { + return transform.getTransform3d(); + } +} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/RobotToCameraTransformProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/RobotToCameraTransformProto.java new file mode 100644 index 0000000000..ff1b4b407b --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/RobotToCameraTransformProto.java @@ -0,0 +1,37 @@ +package org.photonvision.targeting.proto; + +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +import org.photonvision.proto.Photon.ProtobufRobotToCameraTransform; +import org.photonvision.targeting.RobotToCameraTransform; + + +public class RobotToCameraTransformProto implements + Protobuf { + @Override + public Class getTypeClass() { + return RobotToCameraTransform.class; + } + + @Override + public ProtobufRobotToCameraTransform createMessage() { + return ProtobufRobotToCameraTransform.newInstance(); + } + + @Override + public RobotToCameraTransform unpack(ProtobufRobotToCameraTransform msg) { + return new RobotToCameraTransform(Transform3d.proto.unpack(msg.getRobotToCamera())); + } + + @Override + public void pack(ProtobufRobotToCameraTransform msg, RobotToCameraTransform value) { + Transform3d.proto.pack(msg.getMutableRobotToCamera(), value.robotToCamera); + } + + @Override + public Descriptor getDescriptor() { + return ProtobufRobotToCameraTransform.getDescriptor(); + } +} diff --git a/photon-targeting/src/main/proto/photon.proto b/photon-targeting/src/main/proto/photon.proto index 539c03042b..4eb123f967 100644 --- a/photon-targeting/src/main/proto/photon.proto +++ b/photon-targeting/src/main/proto/photon.proto @@ -57,6 +57,10 @@ message ProtobufPhotonTrackedTarget { float obj_detection_conf = 12; } +message ProtobufRobotToCameraTransform { + wpi.proto.ProtobufTransform3d robotToCamera = 1; +} + message ProtobufPhotonPipelineResult { double latency_ms = 1 [deprecated = true]; From 9aa9c33358be5ac903b9405752d99a7455378f6d Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 5 Mar 2026 21:48:58 -0500 Subject: [PATCH 08/62] Hopefully compiles? Added serde code for C++ --- .../targeting/RobotToCameraTransform.java | 17 ++++++++ .../photon/targeting/RobotToCameraTransform.h | 39 +++++++++++++++++++ 2 files changed, 56 insertions(+) create mode 100644 photon-targeting/src/main/native/include/photon/targeting/RobotToCameraTransform.h diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java b/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java index fec1ae174c..047addd509 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java @@ -1,3 +1,20 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + package org.photonvision.targeting; import org.photonvision.common.dataflow.structures.PacketSerde; diff --git a/photon-targeting/src/main/native/include/photon/targeting/RobotToCameraTransform.h b/photon-targeting/src/main/native/include/photon/targeting/RobotToCameraTransform.h new file mode 100644 index 0000000000..96eefb8231 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/targeting/RobotToCameraTransform.h @@ -0,0 +1,39 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include +#include "photon/dataflow/structures/Packet.h" +#include "photon/struct/RobotToCameraTransformStruct.h" + +namespace photon { + +struct RobotToCameraTransform : public RobotToCameraTransform_PhotonStruct { + using Base = RobotToCameraTransform_PhotonStruct; + + explicit RobotToCameraTransform(Base&& data) : Base(data) {} + + template + explicit RobotToCameraTransform(Args&&... args) : Base{std::forward(args)...} {} + + friend bool operator==(RobotToCameraTransform const&, RobotToCameraTransform const&) = default; +}; + +} // namespace photon + +#include "photon/serde/RobotToCameraTransformSerde.h" \ No newline at end of file From b4180de4f03c543f3fec56efa47685b63c4ffd7d Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 5 Mar 2026 21:56:36 -0500 Subject: [PATCH 09/62] fixed photonpipelineresult constructor description --- .../java/org/photonvision/targeting/PhotonPipelineResult.java | 1 - 1 file changed, 1 deletion(-) 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 9856389a15..c93eebe72e 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -87,7 +87,6 @@ public PhotonPipelineResult( * @param timeSinceLastPong The time since the last Time Sync Pong in uS. * @param targets The list of targets identified by the pipeline. * @param result Result from multi-target PNP. - * @param robotToCamera The transform from the robot to the camera. */ public PhotonPipelineResult( long sequenceID, From 8e5ef169a84ffb6777ee74e0f39ad45170e13743 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 5 Mar 2026 22:13:10 -0500 Subject: [PATCH 10/62] added old constructor for PhotonPipelineResult --- .../include/photon/targeting/PhotonPipelineResult.h | 9 +++++++++ 1 file changed, 9 insertions(+) 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 b7cf660357..edf85d98b3 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -63,6 +63,15 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { template explicit PhotonPipelineResult(Args&&... args) : Base{std::forward(args)...} {} + + // Old constructor for backwards compatibility. + explicit PhotonPipelineResult( + long sequenceID, long captureTimestamp, long publishTimestamp, + long timeSinceLastPong, std::vector targets, + std::optional result) + : Base(PhotonPipelineMetadata{captureTimestamp, publishTimestamp, + sequenceID, timeSinceLastPong}, + targets, result, std::nullopt) {} /** * Returns the best target in this pipeline result. If there are no targets, From cd707bcbefc0447f203677959c491f6f4d2f6541 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 5 Mar 2026 22:48:08 -0500 Subject: [PATCH 11/62] Hopefully fixed LegacyPhotonPoseEstimatorTest --- .../java/org/photonvision/PhotonCamera.java | 1 - .../cpp/LegacyPhotonPoseEstimatorTest.cpp | 22 +++++++++---------- .../photon/targeting/PhotonPipelineResult.h | 9 -------- 3 files changed, 11 insertions(+), 21 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index d215763021..6965c895fb 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -152,7 +152,6 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName, Transform3 * simulation, but should *usually* be the default NTInstance from * NetworkTableInstance::getDefault * @param cameraName The name of the camera, as seen in the UI. - * @param robotToCamera The transform from the robot to the camera. This is used for pose estimation */ public PhotonCamera(NetworkTableInstance instance, String cameraName) { this(instance, cameraName, Optional.empty()); diff --git a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp index 0445be6b1e..cdcd5567ac 100644 --- a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp @@ -91,7 +91,7 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -152,7 +152,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator( @@ -201,7 +201,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, @@ -252,7 +252,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, @@ -293,7 +293,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, - std::nullopt}}; + std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); // std::optional estimatedPose; @@ -417,7 +417,7 @@ TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, @@ -472,7 +472,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { // empty input, expect empty out cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, - std::vector{}, std::nullopt}}; + std::vector{}, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1)); std::optional estimatedPose; @@ -484,7 +484,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { // Set result, and update -- expect present and timestamp to be 15 cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -553,7 +553,7 @@ TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -578,7 +578,7 @@ TEST(LegacyPhotonPoseEstimatorTest, CopyResult) { std::vector targets{}; auto testResult = photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; testResult.SetReceiveTimestamp(units::second_t(11)); auto test2 = testResult; @@ -628,7 +628,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, - multiTagResult}; + multiTagResult, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; cameraOne.test = true; cameraOne.testResult = {result}; 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 edf85d98b3..04ae33dba1 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -64,15 +64,6 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { explicit PhotonPipelineResult(Args&&... args) : Base{std::forward(args)...} {} - // Old constructor for backwards compatibility. - explicit PhotonPipelineResult( - long sequenceID, long captureTimestamp, long publishTimestamp, - long timeSinceLastPong, std::vector targets, - std::optional result) - : Base(PhotonPipelineMetadata{captureTimestamp, publishTimestamp, - sequenceID, timeSinceLastPong}, - targets, result, std::nullopt) {} - /** * Returns the best target in this pipeline result. If there are no targets, * this method will return null. The best target is determined by the target From 6e3759e49580cc6c27a7a03a5391818969b86706 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Fri, 6 Mar 2026 17:22:19 -0500 Subject: [PATCH 12/62] continued work on the c++ version of photonlib, still doesn't compile --- .../configuration/CameraConfiguration.java | 6 ++-- .../networktables/NTDataPublisher.java | 6 ++-- .../vision/processes/VisionModule.java | 12 +++---- .../processes/VisionSourceSettables.java | 1 - .../java/org/photonvision/PhotonCamera.java | 34 +++++++++++-------- .../simulation/PhotonCameraSim.java | 3 +- .../main/native/cpp/photon/PhotonCamera.cpp | 8 ++++- .../cpp/photon/simulation/PhotonCameraSim.cpp | 4 ++- .../main/native/include/photon/PhotonCamera.h | 18 ++++++++++ .../src/test/native/cpp/PhotonCameraTest.cpp | 2 +- .../native/cpp/PhotonPoseEstimatorTest.cpp | 20 +++++------ .../common/networktables/NTTopicSet.java | 5 +-- .../targeting/PhotonPipelineResult.java | 34 ++++++++----------- .../targeting/RobotToCameraTransform.java | 14 ++++---- .../proto/PhotonPipelineResultProto.java | 6 ++-- .../proto/RobotToCameraTransformProto.java | 8 ++--- .../java/org/photonvision/PacketTest.java | 3 +- .../targeting/PhotonPipelineResultTest.java | 12 +++---- .../proto/PhotonPipelineResultProtoTest.java | 3 +- 19 files changed, 106 insertions(+), 93 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java index 981d409826..d19171e5f0 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java @@ -22,7 +22,6 @@ import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.cscore.UsbCameraInfo; import edu.wpi.first.math.geometry.Transform3d; - import java.util.ArrayList; import java.util.List; import java.util.UUID; @@ -214,8 +213,9 @@ public void removeCalibration(Size unrotatedImageSize) { /** * Set the robot to camera transform for this camera configuration. - * - * @param robotToCamera the transform from the robot's origin to the camera's origin, in the robot's coordinate system. + * + * @param robotToCamera the transform from the robot's origin to the camera's origin, in the + * robot's coordinate system. */ public void setRobotToCamera(Transform3d robotToCamera) { this.robotToCamera = robotToCamera; diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 136790056b..ba83ae911a 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -67,8 +67,7 @@ public NTDataPublisher( Consumer driverModeConsumer, Supplier fpsLimitSupplier, Consumer fpsLimitConsumer, - Consumer robotToCameraConsumer - ) { + Consumer robotToCameraConsumer) { this.pipelineIndexSupplier = pipelineIndexSupplier; this.pipelineIndexConsumer = pipelineIndexConsumer; this.driverModeSupplier = driverModeSupplier; @@ -204,7 +203,8 @@ public void accept(CVPipelineResult result) { NetworkTablesManager.getInstance().getTimeSinceLastPong(), TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets), acceptedResult.multiTagResult, - Optional.empty() // TODO: robotToCamera -- should pull this from the NT table, temporary solution for testing + Optional.empty() // TODO: robotToCamera -- should pull this from the NT table, temporary + // solution for testing ); // random guess at size of the array diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index 744b2bfaf0..f83b76cd44 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -157,8 +157,7 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource) this::setDriverMode, this::getFPSLimit, this::setFPSLimit, - this::setRobotToCameraTransform - ); + this::setRobotToCameraTransform); uiDataConsumer = new UIDataPublisher(visionSource.getSettables().getConfiguration().uniqueName); statusLEDsConsumer = new StatusLEDConsumer(visionSource.getSettables().getConfiguration().uniqueName); @@ -641,11 +640,12 @@ public void setFPSLimit(int fps) { } /** - * Set camera transform for this vision module. As of now this doesn't affect vision processing in any way - * - * @param robotToCameraTransform the transform from the robot's origin to the camera's origin, in the robot's coordinate system. This should be provided in meters. + * Set camera transform for this vision module. As of now this doesn't affect vision processing in + * any way + * + * @param robotToCameraTransform the transform from the robot's origin to the camera's origin, in + * the robot's coordinate system. This should be provided in meters. */ - public void setRobotToCameraTransform(Transform3d robotToCameraTransform) { this.robotToCameraTransform = robotToCameraTransform; saveAndBroadcastAll(); diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index f4cb7c884c..2a94dd6617 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java @@ -19,7 +19,6 @@ import edu.wpi.first.cscore.VideoMode; import edu.wpi.first.math.geometry.Transform3d; - import java.util.HashMap; import org.opencv.core.Size; import org.photonvision.common.configuration.CameraConfiguration; diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 6965c895fb..0a980161b6 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -29,12 +29,11 @@ import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.*; import edu.wpi.first.networktables.BooleanPublisher; import edu.wpi.first.networktables.BooleanSubscriber; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.DoubleArraySubscriber; -import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.networktables.IntegerEntry; import edu.wpi.first.networktables.IntegerPublisher; import edu.wpi.first.networktables.IntegerSubscriber; @@ -43,6 +42,7 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.PubSubOption; import edu.wpi.first.networktables.StringSubscriber; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; @@ -54,7 +54,6 @@ import org.opencv.core.Core; import org.photonvision.common.hardware.VisionLEDMode; import org.photonvision.common.networktables.PacketSubscriber; -import org.photonvision.common.networktables.PacketPublisher; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.timesync.TimeSyncSingleton; @@ -139,7 +138,8 @@ public static void setVersionCheckEnabled(boolean enabled) { * simulation, but should *usually* be the default NTInstance from * NetworkTableInstance::getDefault * @param cameraName The name of the camera, as seen in the UI. - * @param robotToCamera The transform from the robot to the camera. This is used for pose estimation + * @param robotToCamera The transform from the robot to the camera. This is used for pose + * estimation */ public PhotonCamera(NetworkTableInstance instance, String cameraName, Transform3d robotToCamera) { this(instance, cameraName, Optional.of(robotToCamera)); @@ -159,14 +159,16 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { /** * Internal implementation of the constructor - * + * * @param instance The NetworkTableInstance to pull data from. This can be a custom instance in * simulation, but should *usually* be the default NTInstance from * NetworkTableInstance::getDefault * @param cameraName The name of the camera, as seen in the UI. - * @param robotToCamera The transform from the robot to the camera. This is used for pose estimation + * @param robotToCamera The transform from the robot to the camera. This is used for pose + * estimation */ - private PhotonCamera(NetworkTableInstance instance, String cameraName, Optional robotToCamera) { + private PhotonCamera( + NetworkTableInstance instance, String cameraName, Optional robotToCamera) { name = cameraName; this.robotToCamera = robotToCamera; disconnectAlert = @@ -185,8 +187,10 @@ private PhotonCamera(NetworkTableInstance instance, String cameraName, Optional< PubSubOption.periodic(0.01), PubSubOption.sendAll(true), PubSubOption.pollStorage(20)); - resultSubscriber = new PacketSubscriber<>(rawBytesEntry_pipelineResult, PhotonPipelineResult.photonStruct); - robotToCameraPublisher = cameraTable.getStructTopic("robotToCamera", Transform3d.struct).publish(); + resultSubscriber = + new PacketSubscriber<>(rawBytesEntry_pipelineResult, PhotonPipelineResult.photonStruct); + robotToCameraPublisher = + cameraTable.getStructTopic("robotToCamera", Transform3d.struct).publish(); driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish(); driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false); fpsLimitPublisher = cameraTable.getIntegerTopic("fpsLimitRequest").publish(); @@ -226,9 +230,11 @@ private PhotonCamera(NetworkTableInstance instance, String cameraName, Optional< private void publishRobotToCameraTransform() { robotToCamera.ifPresentOrElse( - (transform) -> robotToCameraPublisher.set(transform), - () -> robotToCameraPublisher.set(new Transform3d()) // TODO: See if this default value can cause any issues. - ); + (transform) -> robotToCameraPublisher.set(transform), + () -> + robotToCameraPublisher.set( + new Transform3d()) // TODO: See if this default value can cause any issues. + ); } static void verifyDependencies() { @@ -440,7 +446,7 @@ public int getPipelineIndex() { /** * Returns the robot-to-camera transform. - * + * * @return The robot-to-camera transform, if it is set. Optional.empty() otherwise. */ @@ -547,7 +553,7 @@ public final NetworkTable getCameraTable() { /** * Returns the camera's transform from the robot - * + * * @return The camera's transform from the robot, if it is set. Empty otherwise. */ public Optional getCameraTransform() { 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 bcb0946102..44c2af4132 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -650,8 +650,7 @@ public PhotonPipelineResult process( 1000L + (long) ((Math.random() - 0.5) * 50), detectableTgts, multitagResult, - cam.getCameraTransform() - ); + cam.getCameraTransform()); 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 adf87b8ee6..d6a70d5834 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -122,7 +122,8 @@ static const std::string TYPE_STRING = std::string{SerdeType::GetSchemaHash()}; PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, - const std::string_view cameraName) + const std::string_view cameraName, + const std::optional& robotToCamera) : mainTable(instance.GetTable("photonvision")), rootTable(mainTable->GetSubTable(cameraName)), rawBytesEntry( @@ -165,6 +166,7 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, std::string{"PhotonCamera '"} + std::string{cameraName} + "' is disconnected.", frc::Alert::AlertType::kWarning), + robotToCamera(robotToCamera), timesyncAlert(PHOTON_ALERT_GROUP, "", frc::Alert::AlertType::kWarning) { verifyDependencies(); HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount); @@ -176,6 +178,10 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, InitTspServer(); } +PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, + const std::string_view cameraName) + : PhotonCamera(instance, cameraName, std::nullopt) {} + PhotonCamera::PhotonCamera(const std::string_view cameraName) : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {} 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 4914436ece..0ca5d075f6 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp @@ -343,7 +343,9 @@ PhotonPipelineResult PhotonCameraSim::Process( PhotonPipelineMetadata{heartbeatCounter, 0, units::microsecond_t{latency}.to(), 1000000}, - detectableTgts, multiTagResults}; + detectableTgts, multiTagResults, + cam->GetRobotToCamera() + ? std::make_optional(photon::RobotToCameraTransform(camera->GetRobotToCamera())) : std::nullopt}; } void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) { SubmitProcessedFrame(result, wpi::Now()); diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 06a409577f..a0d6f4d7ab 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -29,6 +29,7 @@ #include #include +#include #include #include #include @@ -74,6 +75,20 @@ class PhotonCamera { */ explicit PhotonCamera(const std::string_view cameraName); + /** + * Constructs a PhotonCamera from a root table. + * + * @param instance The NetworkTableInstance to pull data from. This can be a + * custom instance in simulation, but should *usually* be the default + * NTInstance from {@link NetworkTableInstance::getDefault} + * @param cameraName The name of the camera, as seen in the UI. + * over. + * @param robotToCamera The transform from the robot's center to the camera. This is used for pose estimation + */ + explicit PhotonCamera(nt::NetworkTableInstance instance, + const std::string_view cameraName, + const std::optional& robotToCamera); + PhotonCamera(PhotonCamera&&) = default; ~PhotonCamera() = default; @@ -244,6 +259,8 @@ class PhotonCamera { frc::Alert disconnectAlert; frc::Alert timesyncAlert; + const std::optional& GetRobotToCamera() const { return robotToCamera; } + private: units::second_t lastVersionCheckTime = 0_s; static bool VERSION_CHECK_ENABLED; @@ -260,6 +277,7 @@ class PhotonCamera { void CheckTimeSyncOrWarn(photon::PhotonPipelineResult& result); std::vector tablesThatLookLikePhotonCameras(); + std::optional robotToCamera; }; } // namespace photon diff --git a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp index e021299206..d778baf4c3 100644 --- a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp @@ -101,7 +101,7 @@ TEST(PhotonCameraTest, Alerts) { // AND a result with a timeSinceLastPong in the past photon::PhotonPipelineMetadata metadata{3, 1, 2, 10 * 1000000}; photon::PhotonPipelineResult noPongResult{ - metadata, std::vector{}, std::nullopt}; + metadata, std::vector{}, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}; // Loop to hit cases past first iteration for (int i = 0; i < 10; i++) { diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 5d5caeade6..fdd0180d7e 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -90,7 +90,7 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); @@ -150,7 +150,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}}); @@ -198,7 +198,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -248,7 +248,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -288,7 +288,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, - std::nullopt}}; + std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -330,7 +330,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { photon::PhotonPipelineResult result = cameraOneSim.Process( 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), - targets); + targets, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})); cameraOne.testResult = {result}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); @@ -410,7 +410,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -451,7 +451,7 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); @@ -479,7 +479,7 @@ TEST(PhotonPoseEstimatorTest, CopyResult) { std::vector targets{}; auto testResult = photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}; testResult.SetReceiveTimestamp(units::second_t(11)); auto test2 = testResult; @@ -519,7 +519,7 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, - multiTagResult}; + multiTagResult, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}; cameraOne.test = true; cameraOne.testResult = {result}; diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index 3577f33d3e..7f9c68c38e 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -31,7 +31,6 @@ import edu.wpi.first.networktables.PubSubOption; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.networktables.StructSubscriber; - import org.photonvision.targeting.PhotonPipelineResult; /** @@ -134,9 +133,7 @@ public void updateEntries() { cameraIntrinsicsPublisher = subTable.getDoubleArrayTopic("cameraIntrinsics").publish(); cameraDistortionPublisher = subTable.getDoubleArrayTopic("cameraDistortion").publish(); robotToCameraSubscriber = - subTable - .getStructTopic("robotToCamera", Transform3d.struct) - .subscribe(null); + subTable.getStructTopic("robotToCamera", Transform3d.struct).subscribe(null); } @SuppressWarnings("DuplicatedCode") 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 c93eebe72e..a612fee46d 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -17,8 +17,8 @@ package org.photonvision.targeting; -import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.util.protobuf.ProtobufSerializable; import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -41,8 +41,12 @@ public class PhotonPipelineResult /** The multitag result, if using an AprilTag pipeline with Multi-Target Estimation enabled. */ public Optional multitagResult; - /** The transform from the robot to the camera. This is an optional value, but photonserde doesn't seem to support optional Transform3ds*/ - public Optional robotToCamera; + /** + * The transform from the robot to the camera. This is an optional value, but photonserde doesn't + * seem to support optional Transform3ds + */ + public Optional + robotToCamera; // TODO: Rename RobotToCameraTransform to RobotToCameraWrapper or something /** Constructs an empty pipeline result. */ public PhotonPipelineResult() { @@ -71,11 +75,9 @@ public PhotonPipelineResult( captureTimestampMicros, publishTimestampMicros, sequenceID, timeSinceLastPong), targets, Optional.empty(), - Optional.empty() - ); + Optional.empty()); } - /** * Constructs a pipeline result. * @@ -94,22 +96,19 @@ public PhotonPipelineResult( long publishTimestamp, long timeSinceLastPong, List targets, - Optional result - ) { + Optional result) { this( new PhotonPipelineMetadata( captureTimestamp, publishTimestamp, sequenceID, timeSinceLastPong), targets, result, - Optional.empty() - ); + Optional.empty()); } public PhotonPipelineResult( PhotonPipelineMetadata metadata, List targets, - Optional result - ) { + Optional result) { this(metadata, targets, result, Optional.empty()); } @@ -133,23 +132,20 @@ public PhotonPipelineResult( long timeSinceLastPong, List targets, Optional result, - Optional robotToCamera - ) { + Optional robotToCamera) { this( new PhotonPipelineMetadata( captureTimestamp, publishTimestamp, sequenceID, timeSinceLastPong), targets, result, - robotToCamera - ); + robotToCamera); } public PhotonPipelineResult( PhotonPipelineMetadata metadata, List targets, Optional result, - Optional robotToCamera - ) { + Optional robotToCamera) { this.metadata = metadata; this.targets.addAll(targets); this.multitagResult = result; @@ -222,7 +218,7 @@ public Optional getMultiTagResult() { /** * Return the robot to camera transform. Be sure to check {@code getRobotToCamera().isPresent()} * before using the transform! - * + * * @return The robot to camera transform. Empty if no transform is set. */ public Optional getRobotToCamera() { diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java b/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java index 047addd509..5e7e68f0c0 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java @@ -17,18 +17,16 @@ package org.photonvision.targeting; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.util.protobuf.ProtobufSerializable; import org.photonvision.common.dataflow.structures.PacketSerde; -import org.photonvision.struct.MultiTargetPNPResultSerde; import org.photonvision.struct.RobotToCameraTransformSerde; -import org.photonvision.targeting.serde.PhotonStructSerializable; import org.photonvision.targeting.proto.RobotToCameraTransformProto; - -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.util.protobuf.ProtobufSerializable; +import org.photonvision.targeting.serde.PhotonStructSerializable; // We need a wrapper class for photonserde, as this needs to be an optional value -public class RobotToCameraTransform - implements ProtobufSerializable, PhotonStructSerializable { +public class RobotToCameraTransform + implements ProtobufSerializable, PhotonStructSerializable { public Transform3d robotToCamera; public RobotToCameraTransform() { @@ -56,5 +54,5 @@ public PacketSerde getSerde() { public static Transform3d unwrap(RobotToCameraTransform transform) { return transform.getTransform3d(); - } + } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java index 62ecd4a451..867b040235 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java @@ -54,9 +54,9 @@ public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) { msg.hasMultiTargetResult() ? Optional.of(MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult())) : Optional.empty(), - msg.hasRobotToCamera() - ? Optional.of(Transform3d.proto.unpack(msg.getRobotToCamera())) - : Optional.empty()); + msg.hasRobotToCamera() + ? Optional.of(Transform3d.proto.unpack(msg.getRobotToCamera())) + : Optional.empty()); } @Override diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/RobotToCameraTransformProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/RobotToCameraTransformProto.java index ff1b4b407b..3682195d0f 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/RobotToCameraTransformProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/RobotToCameraTransformProto.java @@ -2,14 +2,12 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.util.protobuf.Protobuf; -import us.hebi.quickbuf.Descriptors.Descriptor; - import org.photonvision.proto.Photon.ProtobufRobotToCameraTransform; import org.photonvision.targeting.RobotToCameraTransform; +import us.hebi.quickbuf.Descriptors.Descriptor; - -public class RobotToCameraTransformProto implements - Protobuf { +public class RobotToCameraTransformProto + implements Protobuf { @Override public Class getTypeClass() { return RobotToCameraTransform.class; diff --git a/photon-targeting/src/test/java/org/photonvision/PacketTest.java b/photon-targeting/src/test/java/org/photonvision/PacketTest.java index 9fc89b59ff..cc799908f1 100644 --- a/photon-targeting/src/test/java/org/photonvision/PacketTest.java +++ b/photon-targeting/src/test/java/org/photonvision/PacketTest.java @@ -156,8 +156,7 @@ void pipelineResultSerde() { new PnpResult( new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), List.of((short) 1, (short) 2, (short) 3))), - Optional.of( - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); + Optional.of(new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); var p3 = new Packet(10); PhotonPipelineResult.photonStruct.pack(p3, ret3); var unpackedRet3 = PhotonPipelineResult.photonStruct.unpack(p3); diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java index b967f261c3..374e12b7b4 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java @@ -188,8 +188,7 @@ public void equalityTest() { new PnpResult( new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), List.of((short) 1, (short) 2, (short) 3))), - Optional.of( - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); + Optional.of(new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); b = new PhotonPipelineResult( 3, @@ -244,8 +243,7 @@ public void equalityTest() { new PnpResult( new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), List.of((short) 1, (short) 2, (short) 3))), - Optional.of( - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); + Optional.of(new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); assertEquals(a, b); } @@ -405,8 +403,7 @@ public void inequalityTest() { new PnpResult( new Transform3d(new Translation3d(1, 8, 3), new Rotation3d(1, 2, 3)), 0.1), List.of((short) 3, (short) 4, (short) 7))), - Optional.of( - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); + Optional.of(new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); b = new PhotonPipelineResult( 3, @@ -461,8 +458,7 @@ public void inequalityTest() { new PnpResult( new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), List.of((short) 1, (short) 2, (short) 3))), - Optional.of( - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); + Optional.of(new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); assertNotEquals(a, b); } } diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java index 05b2d77456..89eee363c4 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java @@ -147,8 +147,7 @@ public void protobufTest() { new PnpResult( new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), List.of((short) 1, (short) 2, (short) 3))), - Optional.of( - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); + Optional.of(new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)))); serializedResult = PhotonPipelineResult.proto.createMessage(); PhotonPipelineResult.proto.pack(serializedResult, result); unpackedResult = PhotonPipelineResult.proto.unpack(serializedResult); From a780d943f59993f054a983d36a4e5f6a85e44d64 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Fri, 6 Mar 2026 20:51:45 -0500 Subject: [PATCH 13/62] CPP tests compile now i think --- .../main/native/cpp/photon/PhotonCamera.cpp | 4 ++-- .../cpp/photon/simulation/PhotonCameraSim.cpp | 2 +- .../main/native/include/photon/PhotonCamera.h | 9 ++++++-- .../cpp/LegacyPhotonPoseEstimatorTest.cpp | 22 +++++++++---------- .../native/cpp/PhotonPoseEstimatorTest.cpp | 2 +- 5 files changed, 22 insertions(+), 17 deletions(-) diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index d6a70d5834..bca4e47521 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -166,8 +166,8 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, std::string{"PhotonCamera '"} + std::string{cameraName} + "' is disconnected.", frc::Alert::AlertType::kWarning), - robotToCamera(robotToCamera), - timesyncAlert(PHOTON_ALERT_GROUP, "", frc::Alert::AlertType::kWarning) { + timesyncAlert(PHOTON_ALERT_GROUP, "", frc::Alert::AlertType::kWarning), + robotToCamera(robotToCamera) { verifyDependencies(); HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount); InstanceCount++; 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 0ca5d075f6..802e914a05 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp @@ -345,7 +345,7 @@ PhotonPipelineResult PhotonCameraSim::Process( 1000000}, detectableTgts, multiTagResults, cam->GetRobotToCamera() - ? std::make_optional(photon::RobotToCameraTransform(camera->GetRobotToCamera())) : std::nullopt}; + ? std::make_optional(photon::RobotToCameraTransform(cam->GetRobotToCamera().value())) : std::nullopt}; } void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) { SubmitProcessedFrame(result, wpi::Now()); diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index a0d6f4d7ab..f5a76bdbb7 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -92,6 +92,13 @@ class PhotonCamera { PhotonCamera(PhotonCamera&&) = default; ~PhotonCamera() = default; + + /** + * Returns the robot to camera transform + * + * @return The transform from the robot's center to the camera, if it was set. Empty otherwise. + */ + std::optional GetRobotToCamera() { return robotToCamera; } /** * The list of pipeline results sent by PhotonVision since the last call to @@ -259,8 +266,6 @@ class PhotonCamera { frc::Alert disconnectAlert; frc::Alert timesyncAlert; - const std::optional& GetRobotToCamera() const { return robotToCamera; } - private: units::second_t lastVersionCheckTime = 0_s; static bool VERSION_CHECK_ENABLED; diff --git a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp index cdcd5567ac..2c1573fc4e 100644 --- a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp @@ -91,7 +91,7 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -152,7 +152,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator( @@ -201,7 +201,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, @@ -252,7 +252,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, @@ -293,7 +293,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, - std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); // std::optional estimatedPose; @@ -417,7 +417,7 @@ TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, @@ -472,7 +472,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { // empty input, expect empty out cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, - std::vector{}, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::vector{}, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1)); std::optional estimatedPose; @@ -484,7 +484,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { // Set result, and update -- expect present and timestamp to be 15 cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -553,7 +553,7 @@ TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -578,7 +578,7 @@ TEST(LegacyPhotonPoseEstimatorTest, CopyResult) { std::vector targets{}; auto testResult = photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}; testResult.SetReceiveTimestamp(units::second_t(11)); auto test2 = testResult; @@ -628,7 +628,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, - multiTagResult, std::make_optional(photon::RobotToCameraTransform{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + multiTagResult, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}; cameraOne.test = true; cameraOne.testResult = {result}; diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index fdd0180d7e..1dafa270f3 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -330,7 +330,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { photon::PhotonPipelineResult result = cameraOneSim.Process( 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), - targets, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})); + targets); cameraOne.testResult = {result}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); From 4b0fd04310aba2bdad1a924bbf488ae30dd72a01 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Fri, 6 Mar 2026 22:29:38 -0500 Subject: [PATCH 14/62] fixed cpp packet test --- photon-targeting/src/test/native/cpp/PacketTest.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp index 94cb707e13..34c4d38713 100644 --- a/photon-targeting/src/test/native/cpp/PacketTest.cpp +++ b/photon-targeting/src/test/native/cpp/PacketTest.cpp @@ -88,7 +88,7 @@ TEST(PacketTest, PnpResult) { TEST(PacketTest, PhotonPipelineResult) { PhotonPipelineResult result(PhotonPipelineMetadata(0, 0, 1, 2), - std::vector{}, std::nullopt); + std::vector{}, std::nullopt, std::make_optional(RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})); Packet p; p.Pack(result); @@ -133,7 +133,7 @@ TEST(PacketTest, PhotonPipelineResult) { std::vector{8, 7, 11, 22, 59, 40}}; PhotonPipelineResult result2(PhotonPipelineMetadata{0, 0, 1, 1}, targets, - mtResult); + mtResult, std::make_optional(RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})); Packet p2; auto t1 = std::chrono::steady_clock::now(); From 0baf428345f0bc8c9294b673eac460de9ab8db5d Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sat, 7 Mar 2026 13:31:33 -0500 Subject: [PATCH 15/62] misc changes, fixed PhotonPipelineResult equality --- .../src/main/native/cpp/photon/PhotonPoseEstimator.cpp | 2 ++ .../photonvision/targeting/PhotonPipelineResult.java | 6 +++--- .../photonvision/targeting/RobotToCameraTransform.java | 10 ++++++++++ .../targeting/proto/PhotonPipelineResultProto.java | 4 ++-- photon-targeting/src/main/proto/photon.proto | 2 +- 5 files changed, 18 insertions(+), 6 deletions(-) diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 2e0b34a9bd..13ff84d128 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -53,6 +53,8 @@ #define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT #include WPI_IGNORE_DEPRECATED + +//TODO: Update to use the new PhotonPipelineResult namespace photon { namespace detail { 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 a612fee46d..12186d32d0 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -138,18 +138,18 @@ public PhotonPipelineResult( captureTimestamp, publishTimestamp, sequenceID, timeSinceLastPong), targets, result, - robotToCamera); + robotToCamera.map(RobotToCameraTransform::new)); } public PhotonPipelineResult( PhotonPipelineMetadata metadata, List targets, Optional result, - Optional robotToCamera) { + Optional robotToCamera) { this.metadata = metadata; this.targets.addAll(targets); this.multitagResult = result; - this.robotToCamera = robotToCamera.map(RobotToCameraTransform::new); + this.robotToCamera = robotToCamera; } /** diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java b/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java index 5e7e68f0c0..5a50145675 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java @@ -55,4 +55,14 @@ public PacketSerde getSerde() { public static Transform3d unwrap(RobotToCameraTransform transform) { return transform.getTransform3d(); } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (o == null || getClass() != o.getClass()) return false; + + RobotToCameraTransform that = (RobotToCameraTransform) o; + + return robotToCamera.equals(that.robotToCamera); + } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java index 867b040235..352d6afe7b 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java @@ -17,13 +17,13 @@ package org.photonvision.targeting.proto; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.util.protobuf.Protobuf; import java.util.Optional; import org.photonvision.proto.Photon.ProtobufPhotonPipelineResult; import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.RobotToCameraTransform; import us.hebi.quickbuf.Descriptors.Descriptor; public class PhotonPipelineResultProto @@ -55,7 +55,7 @@ public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) { ? Optional.of(MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult())) : Optional.empty(), msg.hasRobotToCamera() - ? Optional.of(Transform3d.proto.unpack(msg.getRobotToCamera())) + ? Optional.of(RobotToCameraTransform.proto.unpack(msg.getRobotToCamera())) : Optional.empty()); } diff --git a/photon-targeting/src/main/proto/photon.proto b/photon-targeting/src/main/proto/photon.proto index 4eb123f967..fcac350ed9 100644 --- a/photon-targeting/src/main/proto/photon.proto +++ b/photon-targeting/src/main/proto/photon.proto @@ -72,7 +72,7 @@ message ProtobufPhotonPipelineResult { int64 nt_publish_timestamp_micros = 6; int64 time_since_last_pong_micros = 7; - optional wpi.proto.ProtobufTransform3d robotToCamera = 8; + optional ProtobufRobotToCameraTransform robotToCamera = 8; } message ProtobufDeviceMetrics { From ddf4e1d0f072c3e0ea6ee44725b07030deea1aa8 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sat, 7 Mar 2026 14:22:53 -0500 Subject: [PATCH 16/62] updated photon serde to support optional shimmed types --- .../generated/PhotonPipelineResultSerde.py | 11 +++----- photon-serde/messages.yaml | 2 +- photon-serde/templates/Message.java.jinja | 21 +++++++++++++++- .../struct/MultiTargetPNPResultSerde.java | 4 +++ .../struct/PhotonPipelineMetadataSerde.java | 4 +++ .../struct/PhotonPipelineResultSerde.java | 25 ++++++++++++------- .../struct/PhotonTrackedTargetSerde.java | 4 +++ .../photonvision/struct/PnpResultSerde.java | 4 +++ .../struct/RobotToCameraTransformSerde.java | 4 +++ .../struct/TargetCornerSerde.java | 4 +++ .../serde/PhotonPipelineResultSerde.cpp | 4 +-- .../photon/serde/PhotonPipelineResultSerde.h | 6 ++--- .../struct/PhotonPipelineResultStruct.h | 4 +-- .../org/photonvision/utils/PacketUtils.java | 23 +++++++++++++++++ 14 files changed, 95 insertions(+), 25 deletions(-) diff --git a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py index 8b8b94f520..97c295ce09 100644 --- a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py +++ b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py @@ -37,13 +37,12 @@ from ..targeting import PhotonPipelineMetadata # noqa from ..targeting import PhotonPipelineResult # noqa from ..targeting import PhotonTrackedTarget # noqa - from ..targeting import RobotToCameraTransform # noqa class PhotonPipelineResultSerde: # Message definition md5sum. See photon_packet.adoc for details - MESSAGE_VERSION = "b0040327f63abf872824e05641cbdede" - MESSAGE_FORMAT = "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;optional RobotToCameraTransform:575b4e398df72967da55b383dfe7784d robotToCamera;" + MESSAGE_VERSION = "c3e6b96bad05f102560d0abcad50debc" + MESSAGE_FORMAT = "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;optional Transform3d robotToCamera;" @staticmethod def pack(value: "PhotonPipelineResult") -> "Packet": @@ -58,8 +57,7 @@ def pack(value: "PhotonPipelineResult") -> "Packet": # multitagResult is optional! it better not be a VLA too ret.encodeOptional(value.multitagResult, MultiTargetPNPResult.photonStruct) - # robotToCamera is optional! it better not be a VLA too - ret.encodeOptional(value.robotToCamera, RobotToCameraTransform.photonStruct) + ret.encodeTransform(value.robotToCamera) return ret @staticmethod @@ -75,8 +73,7 @@ def unpack(packet: "Packet") -> "PhotonPipelineResult": # multitagResult is optional! it better not be a VLA too ret.multitagResult = packet.decodeOptional(MultiTargetPNPResult.photonStruct) - # robotToCamera is optional! it better not be a VLA too - ret.robotToCamera = packet.decodeOptional(RobotToCameraTransform.photonStruct) + ret.robotToCamera = packet.decodeTransform() return ret diff --git a/photon-serde/messages.yaml b/photon-serde/messages.yaml index 7877577345..e5fc298101 100644 --- a/photon-serde/messages.yaml +++ b/photon-serde/messages.yaml @@ -97,5 +97,5 @@ type: MultiTargetPNPResult optional: True - name: robotToCamera - type: RobotToCameraTransform + type: Transform3d optional: True diff --git a/photon-serde/templates/Message.java.jinja b/photon-serde/templates/Message.java.jinja index b1e27b68e9..30edb978b3 100644 --- a/photon-serde/templates/Message.java.jinja +++ b/photon-serde/templates/Message.java.jinja @@ -33,6 +33,10 @@ import org.photonvision.utils.PacketUtils; // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// Needed for optional shims +import java.util.function.BiConsumer; +import java.util.function.Function; + // WPILib imports (if any) import edu.wpi.first.util.struct.Struct; {% for type in nested_wpilib_types -%} @@ -57,12 +61,22 @@ public class {{ name }}Serde implements PacketSerde<{{name}}> { // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'"); } - +{% for field in fields -%} +{%- if field.type | is_shimmed and field.optional == True %} + private static BiConsumer {{ field.name }}_PSINTERNALencode_shim_callable = (packet, value) -> {{ get_message_by_name(field.type).java_encode_shim }}(packet, value); + private static Function {{ field.name }}_PSINTERNALdecode_shim_callable = (packet) -> {{ get_message_by_name(field.type).java_decode_shim }}(packet); +{% endif -%} +{% endfor%} @Override public void pack(Packet packet, {{ name }} value) { {%- for field in fields -%} {%- if field.type | is_shimmed %} + {%- if field.optional == True %} + // {{ field.name }} is optional and shimmed! + PacketUtils.packOptional(packet, value.{{ field.name }}, {{ field.name }}_PSINTERNALencode_shim_callable); + {%- else %} {{ get_message_by_name(field.type).java_encode_shim }}(packet, value.{{ field.name }}); + {%- endif %} {%- elif field.optional == True %} // {{ field.name }} is optional! it better not be a VLA too packet.encodeOptional(value.{{ field.name }}); @@ -89,7 +103,12 @@ public class {{ name }}Serde implements PacketSerde<{{name}}> { var ret = new {{ name }}(); {% for field in fields -%} {%- if field.type | is_shimmed %} + {%- if field.optional == True %} + // {{ field.name }} is optional and shimmed! + ret.{{ field.name }} = PacketUtils.unpackOptional(packet, {{ field.name }}_PSINTERNALdecode_shim_callable); + {%- else %} ret.{{ field.name }} = {{ get_message_by_name(field.type).java_decode_shim }}(packet); + {%- endif %} {%- elif field.optional == True %} // {{ field.name }} is optional! it better not be a VLA too ret.{{ field.name }} = packet.decodeOptional({{ field.type }}.photonStruct); diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java index 38269c428b..bafc13a54e 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java @@ -33,6 +33,10 @@ // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// Needed for optional shims +import java.util.function.BiConsumer; +import java.util.function.Function; + // WPILib imports (if any) import edu.wpi.first.util.struct.Struct; diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java index 8d26d29818..32d2f85f65 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java @@ -33,6 +33,10 @@ // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// Needed for optional shims +import java.util.function.BiConsumer; +import java.util.function.Function; + // WPILib imports (if any) import edu.wpi.first.util.struct.Struct; diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java index 673b2a98ae..27b56a2521 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java @@ -33,9 +33,13 @@ // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// Needed for optional shims +import java.util.function.BiConsumer; +import java.util.function.Function; + // WPILib imports (if any) import edu.wpi.first.util.struct.Struct; - +import edu.wpi.first.math.geometry.Transform3d; /** * Auto-generated serialization/deserialization helper for PhotonPipelineResult @@ -43,9 +47,9 @@ public class PhotonPipelineResultSerde implements PacketSerde { @Override - public final String getInterfaceUUID() { return "b0040327f63abf872824e05641cbdede"; } + public final String getInterfaceUUID() { return "c3e6b96bad05f102560d0abcad50debc"; } @Override - public final String getSchema() { return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;optional RobotToCameraTransform:575b4e398df72967da55b383dfe7784d robotToCamera;"; } + public final String getSchema() { return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;optional Transform3d robotToCamera;"; } @Override public final String getTypeName() { return "PhotonPipelineResult"; } @@ -55,6 +59,9 @@ public int getMaxByteSize() { throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'"); } + private static BiConsumer robotToCamera_PSINTERNALencode_shim_callable = (packet, value) -> PacketUtils.packTransform3d(packet, value); + private static Function robotToCamera_PSINTERNALdecode_shim_callable = (packet) -> PacketUtils.unpackTransform3d(packet); + @Override public void pack(Packet packet, PhotonPipelineResult value) { // field metadata is of non-intrinsic type PhotonPipelineMetadata @@ -66,8 +73,8 @@ public void pack(Packet packet, PhotonPipelineResult value) { // multitagResult is optional! it better not be a VLA too packet.encodeOptional(value.multitagResult); - // robotToCamera is optional! it better not be a VLA too - packet.encodeOptional(value.robotToCamera); + // robotToCamera is optional and shimmed! + PacketUtils.packOptional(packet, value.robotToCamera, robotToCamera_PSINTERNALencode_shim_callable); } @Override @@ -83,8 +90,8 @@ public PhotonPipelineResult unpack(Packet packet) { // multitagResult is optional! it better not be a VLA too ret.multitagResult = packet.decodeOptional(MultiTargetPNPResult.photonStruct); - // robotToCamera is optional! it better not be a VLA too - ret.robotToCamera = packet.decodeOptional(RobotToCameraTransform.photonStruct); + // robotToCamera is optional and shimmed! + ret.robotToCamera = PacketUtils.unpackOptional(packet, robotToCamera_PSINTERNALdecode_shim_callable); return ret; } @@ -92,14 +99,14 @@ public PhotonPipelineResult unpack(Packet packet) { @Override public PacketSerde[] getNestedPhotonMessages() { return new PacketSerde[] { - MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct,RobotToCameraTransform.photonStruct,PhotonPipelineMetadata.photonStruct + PhotonTrackedTarget.photonStruct,MultiTargetPNPResult.photonStruct,PhotonPipelineMetadata.photonStruct }; } @Override public Struct[] getNestedWpilibMessages() { return new Struct[] { - + Transform3d.struct }; } } diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java index 7a1e5363ab..8edd9acecf 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java @@ -33,6 +33,10 @@ // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// Needed for optional shims +import java.util.function.BiConsumer; +import java.util.function.Function; + // WPILib imports (if any) import edu.wpi.first.util.struct.Struct; import edu.wpi.first.math.geometry.Transform3d; diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java index 4b61e5cbd4..f46ef734ad 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java @@ -33,6 +33,10 @@ // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// Needed for optional shims +import java.util.function.BiConsumer; +import java.util.function.Function; + // WPILib imports (if any) import edu.wpi.first.util.struct.Struct; import edu.wpi.first.math.geometry.Transform3d; diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/RobotToCameraTransformSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/RobotToCameraTransformSerde.java index 4c0afa99b0..fd45041cb0 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/RobotToCameraTransformSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/RobotToCameraTransformSerde.java @@ -33,6 +33,10 @@ // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// Needed for optional shims +import java.util.function.BiConsumer; +import java.util.function.Function; + // WPILib imports (if any) import edu.wpi.first.util.struct.Struct; import edu.wpi.first.math.geometry.Transform3d; diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java index c276b1e211..d9231d55ed 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java @@ -33,6 +33,10 @@ // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// Needed for optional shims +import java.util.function.BiConsumer; +import java.util.function.Function; + // WPILib imports (if any) import edu.wpi.first.util.struct.Struct; diff --git a/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineResultSerde.cpp b/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineResultSerde.cpp index 951cbe15a0..14540b6e18 100644 --- a/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineResultSerde.cpp +++ b/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineResultSerde.cpp @@ -34,7 +34,7 @@ void StructType::Pack(Packet& packet, const PhotonPipelineResult& value) { packet.Pack(value.metadata); packet.Pack>(value.targets); packet.Pack>(value.multitagResult); - packet.Pack>(value.robotToCamera); + packet.Pack>(value.robotToCamera); } PhotonPipelineResult StructType::Unpack(Packet& packet) { @@ -42,7 +42,7 @@ PhotonPipelineResult StructType::Unpack(Packet& packet) { .metadata = packet.Unpack(), .targets = packet.Unpack>(), .multitagResult = packet.Unpack>(), - .robotToCamera = packet.Unpack>(), + .robotToCamera = packet.Unpack>(), }}; } diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h index 16e347c607..7f875796ee 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h @@ -36,7 +36,7 @@ #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PhotonPipelineMetadata.h" #include "photon/targeting/PhotonTrackedTarget.h" -#include "photon/targeting/RobotToCameraTransform.h" +#include #include #include #include @@ -47,11 +47,11 @@ namespace photon { template <> struct WPILIB_DLLEXPORT SerdeType { static constexpr std::string_view GetSchemaHash() { - return "b0040327f63abf872824e05641cbdede"; + return "c3e6b96bad05f102560d0abcad50debc"; } static constexpr std::string_view GetSchema() { - return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;optional RobotToCameraTransform:575b4e398df72967da55b383dfe7784d robotToCamera;"; + return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;optional Transform3d robotToCamera;"; } static photon::PhotonPipelineResult Unpack(photon::Packet& packet); diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h index 764c403d16..65f913b6ee 100644 --- a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h +++ b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h @@ -30,7 +30,7 @@ #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PhotonPipelineMetadata.h" #include "photon/targeting/PhotonTrackedTarget.h" -#include "photon/targeting/RobotToCameraTransform.h" +#include #include #include #include @@ -42,7 +42,7 @@ struct PhotonPipelineResult_PhotonStruct { photon::PhotonPipelineMetadata metadata; std::vector targets; std::optional multitagResult; - std::optional robotToCamera; + std::optional robotToCamera; friend bool operator==(PhotonPipelineResult_PhotonStruct const&, PhotonPipelineResult_PhotonStruct const&) = default; }; diff --git a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java index 187cfc0ef1..82cebced65 100644 --- a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java +++ b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java @@ -18,6 +18,11 @@ package org.photonvision.utils; import edu.wpi.first.math.geometry.*; + +import java.util.Optional; +import java.util.function.BiConsumer; +import java.util.function.Function; + import org.photonvision.common.dataflow.structures.Packet; @SuppressWarnings("doclint") @@ -114,4 +119,22 @@ public static void packPose3d(Packet packet, Pose3d pose) { public static Pose3d unpackPose3d(Packet packet) { return new Pose3d(unpackTranslation3d(packet), unpackRotation3d(packet)); } + + public static void packOptional(Packet packet, Optional optional, BiConsumer packer) { + if (optional.isPresent()) { + packet.encode(true); + packer.accept(packet, optional.get()); + } else { + packet.encode(false); + } + } + + public static Optional unpackOptional(Packet packet, Function unpacker) { + boolean isPresent = packet.decodeBoolean(); + if (isPresent) { + return Optional.of(unpacker.apply(packet)); + } else { + return Optional.empty(); + } + } } From b7efcd36b3f80050c8b812707017f872be042821 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sat, 7 Mar 2026 16:19:17 -0500 Subject: [PATCH 17/62] hopefully added optional WPI structs to photonserde --- .../LegacyPhotonPoseEstimatorTest.java | 8 +-- .../cpp/LegacyPhotonPoseEstimatorTest.cpp | 22 ++++---- .../native/cpp/PhotonPoseEstimatorTest.cpp | 18 +++---- photon-serde/messages.yaml | 5 -- .../targeting/PhotonPipelineResult.java | 14 ++--- .../proto/PhotonPipelineResultProto.java | 4 +- .../org/photonvision/utils/PacketUtils.java | 5 +- .../photon/dataflow/structures/Packet.h | 51 +++++++++++++++++++ photon-targeting/src/main/proto/photon.proto | 2 +- .../src/test/native/cpp/PacketTest.cpp | 2 +- 10 files changed, 86 insertions(+), 45 deletions(-) diff --git a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java index dfb882bf6f..3f573abdbc 100644 --- a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java @@ -861,6 +861,10 @@ public void testConstrainedPnpOneTag() { new TargetCorner(127.17118732489361, 313.81406314178633), new TargetCorner(104.28543773760417, 309.6516557438994)); + final double camPitch = Units.degreesToRadians(30.0); + final Transform3d kRobotToCam = + new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0)); + var result = new PhotonPipelineResult( new PhotonPipelineMetadata(10000, 2000, 1, 100), @@ -883,10 +887,6 @@ public void testConstrainedPnpOneTag() { new ArrayList(8))), Optional.of(new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(4, 5, 6)))); - final double camPitch = Units.degreesToRadians(30.0); - final Transform3d kRobotToCam = - new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0)); - PhotonPoseEstimator estimator = new PhotonPoseEstimator( AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), diff --git a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp index 2c1573fc4e..53c43d1745 100644 --- a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp @@ -91,7 +91,7 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -152,7 +152,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator( @@ -201,7 +201,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, @@ -252,7 +252,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, @@ -293,7 +293,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, - std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); // std::optional estimatedPose; @@ -417,7 +417,7 @@ TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, @@ -472,7 +472,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { // empty input, expect empty out cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, - std::vector{}, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + std::vector{}, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1)); std::optional estimatedPose; @@ -484,7 +484,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { // Set result, and update -- expect present and timestamp to be 15 cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -553,7 +553,7 @@ TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -578,7 +578,7 @@ TEST(LegacyPhotonPoseEstimatorTest, CopyResult) { std::vector targets{}; auto testResult = photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; testResult.SetReceiveTimestamp(units::second_t(11)); auto test2 = testResult; @@ -628,7 +628,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, - multiTagResult, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}; + multiTagResult, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; cameraOne.test = true; cameraOne.testResult = {result}; diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 1dafa270f3..b187a844a6 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -90,7 +90,7 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); @@ -150,7 +150,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}}); @@ -198,7 +198,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -248,7 +248,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -288,7 +288,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, - std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -410,7 +410,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -451,7 +451,7 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); @@ -479,7 +479,7 @@ TEST(PhotonPoseEstimatorTest, CopyResult) { std::vector targets{}; auto testResult = photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; testResult.SetReceiveTimestamp(units::second_t(11)); auto test2 = testResult; @@ -519,7 +519,7 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, - multiTagResult, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}; + multiTagResult, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; cameraOne.test = true; cameraOne.testResult = {result}; diff --git a/photon-serde/messages.yaml b/photon-serde/messages.yaml index e5fc298101..a5728a0f5d 100644 --- a/photon-serde/messages.yaml +++ b/photon-serde/messages.yaml @@ -81,11 +81,6 @@ type: int16 vla: True -- name: RobotToCameraTransform - fields: - - name: robotToCamera - type: Transform3d - - name: PhotonPipelineResult fields: - name: metadata 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 12186d32d0..4c55356a97 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -41,12 +41,8 @@ public class PhotonPipelineResult /** The multitag result, if using an AprilTag pipeline with Multi-Target Estimation enabled. */ public Optional multitagResult; - /** - * The transform from the robot to the camera. This is an optional value, but photonserde doesn't - * seem to support optional Transform3ds - */ - public Optional - robotToCamera; // TODO: Rename RobotToCameraTransform to RobotToCameraWrapper or something + /** The transform from the robot to the camera. This is an optional value */ + public Optional robotToCamera; /** Constructs an empty pipeline result. */ public PhotonPipelineResult() { @@ -138,14 +134,14 @@ public PhotonPipelineResult( captureTimestamp, publishTimestamp, sequenceID, timeSinceLastPong), targets, result, - robotToCamera.map(RobotToCameraTransform::new)); + robotToCamera); } public PhotonPipelineResult( PhotonPipelineMetadata metadata, List targets, Optional result, - Optional robotToCamera) { + Optional robotToCamera) { this.metadata = metadata; this.targets.addAll(targets); this.multitagResult = result; @@ -222,7 +218,7 @@ public Optional getMultiTagResult() { * @return The robot to camera transform. Empty if no transform is set. */ public Optional getRobotToCamera() { - return robotToCamera.map(RobotToCameraTransform::unwrap); + return robotToCamera; } /** diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java index 352d6afe7b..867b040235 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java @@ -17,13 +17,13 @@ package org.photonvision.targeting.proto; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.util.protobuf.Protobuf; import java.util.Optional; import org.photonvision.proto.Photon.ProtobufPhotonPipelineResult; import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.RobotToCameraTransform; import us.hebi.quickbuf.Descriptors.Descriptor; public class PhotonPipelineResultProto @@ -55,7 +55,7 @@ public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) { ? Optional.of(MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult())) : Optional.empty(), msg.hasRobotToCamera() - ? Optional.of(RobotToCameraTransform.proto.unpack(msg.getRobotToCamera())) + ? Optional.of(Transform3d.proto.unpack(msg.getRobotToCamera())) : Optional.empty()); } diff --git a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java index 82cebced65..5f155a2c74 100644 --- a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java +++ b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java @@ -18,11 +18,9 @@ package org.photonvision.utils; import edu.wpi.first.math.geometry.*; - import java.util.Optional; import java.util.function.BiConsumer; import java.util.function.Function; - import org.photonvision.common.dataflow.structures.Packet; @SuppressWarnings("doclint") @@ -120,7 +118,8 @@ public static Pose3d unpackPose3d(Packet packet) { return new Pose3d(unpackTranslation3d(packet), unpackRotation3d(packet)); } - public static void packOptional(Packet packet, Optional optional, BiConsumer packer) { + public static void packOptional( + Packet packet, Optional optional, BiConsumer packer) { if (optional.isPresent()) { packet.encode(true); packer.accept(packet, optional.get()); 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 b36a818159..a2ea1555ba 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 @@ -34,6 +34,32 @@ namespace photon { class Packet; +template +struct optional_inner; + +template +struct optional_inner> { + using type = T; +}; + +template +using optional_inner_t = + typename optional_inner>::type; + +template +struct is_optional : std::false_type {}; + +template +struct is_optional> : std::true_type {}; + +template +concept Optional = is_optional>::value; + +template +concept OptionalWPIStructSerializable = + Optional && + wpi::StructSerializable, I...>; + // Struct is where all our actual ser/de methods are implemented template struct SerdeType {}; @@ -108,6 +134,19 @@ class Packet { writePos = newWritePos; } + // Support encoding optional wpi structs + template + requires OptionalWPIStructSerializable + inline void Pack(const std::optional>& value) { + using T = optional_inner_t; + if (value) { + this->Pack(1u); + this->Pack(*value); + } else { + this->Pack(0u); + } + } + template requires(PhotonStructSerializable) inline void Pack(const T& value) { @@ -124,6 +163,18 @@ class Packet { return ret; } + // Support decoding optional wpi structs + template + requires OptionalWPIStructSerializable + inline std::optional> Unpack() { + using T = optional_inner_t; + if (this->Unpack() == 0u) { + return std::nullopt; + } else { + return std::make_optional(this->Unpack()); + } + } + template requires(PhotonStructSerializable) inline T Unpack() { diff --git a/photon-targeting/src/main/proto/photon.proto b/photon-targeting/src/main/proto/photon.proto index fcac350ed9..4eb123f967 100644 --- a/photon-targeting/src/main/proto/photon.proto +++ b/photon-targeting/src/main/proto/photon.proto @@ -72,7 +72,7 @@ message ProtobufPhotonPipelineResult { int64 nt_publish_timestamp_micros = 6; int64 time_since_last_pong_micros = 7; - optional ProtobufRobotToCameraTransform robotToCamera = 8; + optional wpi.proto.ProtobufTransform3d robotToCamera = 8; } message ProtobufDeviceMetrics { diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp index 34c4d38713..01731da6a2 100644 --- a/photon-targeting/src/test/native/cpp/PacketTest.cpp +++ b/photon-targeting/src/test/native/cpp/PacketTest.cpp @@ -88,7 +88,7 @@ TEST(PacketTest, PnpResult) { TEST(PacketTest, PhotonPipelineResult) { PhotonPipelineResult result(PhotonPipelineMetadata(0, 0, 1, 2), - std::vector{}, std::nullopt, std::make_optional(RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})); + std::vector{}, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})); Packet p; p.Pack(result); From 4a2c9f0f87d078e5c68575be8f9b4642fb54fc47 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sat, 7 Mar 2026 17:00:01 -0500 Subject: [PATCH 18/62] added optional WPI structs to cpp photon serde --- .../cpp/photon/simulation/PhotonCameraSim.cpp | 5 ++--- .../cpp/LegacyPhotonPoseEstimatorTest.cpp | 22 +++++++++---------- .../src/test/native/cpp/PhotonCameraTest.cpp | 2 +- .../native/cpp/PhotonPoseEstimatorTest.cpp | 18 +++++++-------- .../src/test/native/cpp/PacketTest.cpp | 4 ++-- 5 files changed, 25 insertions(+), 26 deletions(-) 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 802e914a05..f8f2f3f205 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp @@ -343,9 +343,8 @@ PhotonPipelineResult PhotonCameraSim::Process( PhotonPipelineMetadata{heartbeatCounter, 0, units::microsecond_t{latency}.to(), 1000000}, - detectableTgts, multiTagResults, - cam->GetRobotToCamera() - ? std::make_optional(photon::RobotToCameraTransform(cam->GetRobotToCamera().value())) : std::nullopt}; + detectableTgts, multiTagResults, cam->GetRobotToCamera() + }; } void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) { SubmitProcessedFrame(result, wpi::Now()); diff --git a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp index 53c43d1745..98c96c9493 100644 --- a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp @@ -91,7 +91,7 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -152,7 +152,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator( @@ -201,7 +201,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, @@ -252,7 +252,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, @@ -293,7 +293,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, - std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); // std::optional estimatedPose; @@ -417,7 +417,7 @@ TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, @@ -472,7 +472,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { // empty input, expect empty out cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, - std::vector{}, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::vector{}, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1)); std::optional estimatedPose; @@ -484,7 +484,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { // Set result, and update -- expect present and timestamp to be 15 cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -553,7 +553,7 @@ TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -578,7 +578,7 @@ TEST(LegacyPhotonPoseEstimatorTest, CopyResult) { std::vector targets{}; auto testResult = photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; testResult.SetReceiveTimestamp(units::second_t(11)); auto test2 = testResult; @@ -628,7 +628,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, - multiTagResult, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + multiTagResult, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; cameraOne.test = true; cameraOne.testResult = {result}; diff --git a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp index d778baf4c3..de2539e345 100644 --- a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp @@ -101,7 +101,7 @@ TEST(PhotonCameraTest, Alerts) { // AND a result with a timeSinceLastPong in the past photon::PhotonPipelineMetadata metadata{3, 1, 2, 10 * 1000000}; photon::PhotonPipelineResult noPongResult{ - metadata, std::vector{}, std::nullopt, std::make_optional(photon::RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})}; + metadata, std::vector{}, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; // Loop to hit cases past first iteration for (int i = 0; i < 10; i++) { diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index b187a844a6..6a5657b23b 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -90,7 +90,7 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); @@ -150,7 +150,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}}); @@ -198,7 +198,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -248,7 +248,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -288,7 +288,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, - std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -410,7 +410,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -451,7 +451,7 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); @@ -479,7 +479,7 @@ TEST(PhotonPoseEstimatorTest, CopyResult) { std::vector targets{}; auto testResult = photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; testResult.SetReceiveTimestamp(units::second_t(11)); auto test2 = testResult; @@ -519,7 +519,7 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, - multiTagResult, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + multiTagResult, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; cameraOne.test = true; cameraOne.testResult = {result}; diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp index 01731da6a2..9ed0015ab4 100644 --- a/photon-targeting/src/test/native/cpp/PacketTest.cpp +++ b/photon-targeting/src/test/native/cpp/PacketTest.cpp @@ -88,7 +88,7 @@ TEST(PacketTest, PnpResult) { TEST(PacketTest, PhotonPipelineResult) { PhotonPipelineResult result(PhotonPipelineMetadata(0, 0, 1, 2), - std::vector{}, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})); + std::vector{}, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})); Packet p; p.Pack(result); @@ -133,7 +133,7 @@ TEST(PacketTest, PhotonPipelineResult) { std::vector{8, 7, 11, 22, 59, 40}}; PhotonPipelineResult result2(PhotonPipelineMetadata{0, 0, 1, 1}, targets, - mtResult, std::make_optional(RobotToCameraTransform{frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()}})); + mtResult, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})); Packet p2; auto t1 = std::chrono::steady_clock::now(); From 6855851b4c7953d6069d92e73577804ee1afd677 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sat, 7 Mar 2026 19:09:40 -0500 Subject: [PATCH 19/62] removed robottocameratransform --- .../struct/PhotonPipelineResultSerde.java | 2 +- .../struct/RobotToCameraTransformSerde.java | 89 ------------------- .../serde/RobotToCameraTransformSerde.cpp | 43 --------- .../serde/RobotToCameraTransformSerde.h | 58 ------------ .../targeting/PhotonPipelineResult.java | 2 + .../targeting/RobotToCameraTransform.java | 68 -------------- .../proto/PhotonPipelineResultProto.java | 6 ++ .../proto/RobotToCameraTransformProto.java | 35 -------- .../photon/targeting/RobotToCameraTransform.h | 39 -------- photon-targeting/src/main/proto/photon.proto | 4 - 10 files changed, 9 insertions(+), 337 deletions(-) delete mode 100644 photon-targeting/src/generated/main/java/org/photonvision/struct/RobotToCameraTransformSerde.java delete mode 100644 photon-targeting/src/generated/main/native/cpp/photon/serde/RobotToCameraTransformSerde.cpp delete mode 100644 photon-targeting/src/generated/main/native/include/photon/serde/RobotToCameraTransformSerde.h delete mode 100644 photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java delete mode 100644 photon-targeting/src/main/java/org/photonvision/targeting/proto/RobotToCameraTransformProto.java delete mode 100644 photon-targeting/src/main/native/include/photon/targeting/RobotToCameraTransform.h diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java index 27b56a2521..03c8d127eb 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java @@ -99,7 +99,7 @@ public PhotonPipelineResult unpack(Packet packet) { @Override public PacketSerde[] getNestedPhotonMessages() { return new PacketSerde[] { - PhotonTrackedTarget.photonStruct,MultiTargetPNPResult.photonStruct,PhotonPipelineMetadata.photonStruct + MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct,PhotonPipelineMetadata.photonStruct }; } diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/RobotToCameraTransformSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/RobotToCameraTransformSerde.java deleted file mode 100644 index fd45041cb0..0000000000 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/RobotToCameraTransformSerde.java +++ /dev/null @@ -1,89 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY - -package org.photonvision.struct; - -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.common.dataflow.structures.PacketSerde; -import org.photonvision.utils.PacketUtils; - -// Assume that the base class lives here and we can import it -import org.photonvision.targeting.*; - -// Needed for optional shims -import java.util.function.BiConsumer; -import java.util.function.Function; - -// WPILib imports (if any) -import edu.wpi.first.util.struct.Struct; -import edu.wpi.first.math.geometry.Transform3d; - -/** - * Auto-generated serialization/deserialization helper for RobotToCameraTransform - */ -public class RobotToCameraTransformSerde implements PacketSerde { - - @Override - public final String getInterfaceUUID() { return "575b4e398df72967da55b383dfe7784d"; } - @Override - public final String getSchema() { return "Transform3d robotToCamera;"; } - @Override - public final String getTypeName() { return "RobotToCameraTransform"; } - - @Override - public int getMaxByteSize() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'"); - } - - @Override - public void pack(Packet packet, RobotToCameraTransform value) { - PacketUtils.packTransform3d(packet, value.robotToCamera); - } - - @Override - public RobotToCameraTransform unpack(Packet packet) { - var ret = new RobotToCameraTransform(); - - ret.robotToCamera = PacketUtils.unpackTransform3d(packet); - - return ret; - } - - @Override - public PacketSerde[] getNestedPhotonMessages() { - return new PacketSerde[] { - - }; - } - - @Override - public Struct[] getNestedWpilibMessages() { - return new Struct[] { - Transform3d.struct - }; - } -} diff --git a/photon-targeting/src/generated/main/native/cpp/photon/serde/RobotToCameraTransformSerde.cpp b/photon-targeting/src/generated/main/native/cpp/photon/serde/RobotToCameraTransformSerde.cpp deleted file mode 100644 index 4c2215add6..0000000000 --- a/photon-targeting/src/generated/main/native/cpp/photon/serde/RobotToCameraTransformSerde.cpp +++ /dev/null @@ -1,43 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY - -#include "photon/serde/RobotToCameraTransformSerde.h" - -namespace photon { - -using StructType = SerdeType; - -void StructType::Pack(Packet& packet, const RobotToCameraTransform& value) { - packet.Pack(value.robotToCamera); -} - -RobotToCameraTransform StructType::Unpack(Packet& packet) { - return RobotToCameraTransform{ RobotToCameraTransform_PhotonStruct{ - .robotToCamera = packet.Unpack(), - }}; -} - -} // namespace photon diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/RobotToCameraTransformSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/RobotToCameraTransformSerde.h deleted file mode 100644 index 4f45f3d619..0000000000 --- a/photon-targeting/src/generated/main/native/include/photon/serde/RobotToCameraTransformSerde.h +++ /dev/null @@ -1,58 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#pragma once - -// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY - -#include - -// Include myself -#include "photon/dataflow/structures/Packet.h" -#include "photon/targeting/RobotToCameraTransform.h" - -// Includes for dependant types -#include -#include - - -namespace photon { - -template <> -struct WPILIB_DLLEXPORT SerdeType { - static constexpr std::string_view GetSchemaHash() { - return "575b4e398df72967da55b383dfe7784d"; - } - - static constexpr std::string_view GetSchema() { - return "Transform3d robotToCamera;"; - } - - static photon::RobotToCameraTransform Unpack(photon::Packet& packet); - static void Pack(photon::Packet& packet, const photon::RobotToCameraTransform& value); -}; - -static_assert(photon::PhotonStructSerializable); - -} // namespace photon 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 4c55356a97..0560a0e6bc 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -240,6 +240,8 @@ public String toString() { + targets + ", multitagResult=" + multitagResult + + ", robotToCamera=" + + robotToCamera + "]"; } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java b/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java deleted file mode 100644 index 5a50145675..0000000000 --- a/photon-targeting/src/main/java/org/photonvision/targeting/RobotToCameraTransform.java +++ /dev/null @@ -1,68 +0,0 @@ -/* - * Copyright (C) Photon Vision. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -package org.photonvision.targeting; - -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.util.protobuf.ProtobufSerializable; -import org.photonvision.common.dataflow.structures.PacketSerde; -import org.photonvision.struct.RobotToCameraTransformSerde; -import org.photonvision.targeting.proto.RobotToCameraTransformProto; -import org.photonvision.targeting.serde.PhotonStructSerializable; - -// We need a wrapper class for photonserde, as this needs to be an optional value -public class RobotToCameraTransform - implements ProtobufSerializable, PhotonStructSerializable { - public Transform3d robotToCamera; - - public RobotToCameraTransform() { - this.robotToCamera = new Transform3d(); - } - - public RobotToCameraTransform(Transform3d robotToCamera) { - this.robotToCamera = robotToCamera; - } - - public Transform3d getTransform3d() { - return robotToCamera; - } - - /** RobotToCameraTransform protobuf for serialization. */ - public static final RobotToCameraTransformProto proto = new RobotToCameraTransformProto(); - - /** RobotToCameraTransform PhotonStruct for serialization. */ - public static final RobotToCameraTransformSerde photonStruct = new RobotToCameraTransformSerde(); - - @Override - public PacketSerde getSerde() { - return photonStruct; - } - - public static Transform3d unwrap(RobotToCameraTransform transform) { - return transform.getTransform3d(); - } - - @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - - RobotToCameraTransform that = (RobotToCameraTransform) o; - - return robotToCamera.equals(that.robotToCamera); - } -} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java index 867b040235..fe5ec4d10b 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java @@ -70,6 +70,12 @@ public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) { msg.clearMultiTargetResult(); } + if (value.getRobotToCamera().isPresent()) { + Transform3d.proto.pack(msg.getMutableRobotToCamera(), value.getRobotToCamera().get()); + } else { + msg.clearRobotToCamera(); + } + msg.setSequenceId(value.metadata.getSequenceID()); msg.setCaptureTimestampMicros(value.metadata.getCaptureTimestampMicros()); msg.setNtPublishTimestampMicros(value.metadata.getPublishTimestampMicros()); diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/RobotToCameraTransformProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/RobotToCameraTransformProto.java deleted file mode 100644 index 3682195d0f..0000000000 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/RobotToCameraTransformProto.java +++ /dev/null @@ -1,35 +0,0 @@ -package org.photonvision.targeting.proto; - -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.util.protobuf.Protobuf; -import org.photonvision.proto.Photon.ProtobufRobotToCameraTransform; -import org.photonvision.targeting.RobotToCameraTransform; -import us.hebi.quickbuf.Descriptors.Descriptor; - -public class RobotToCameraTransformProto - implements Protobuf { - @Override - public Class getTypeClass() { - return RobotToCameraTransform.class; - } - - @Override - public ProtobufRobotToCameraTransform createMessage() { - return ProtobufRobotToCameraTransform.newInstance(); - } - - @Override - public RobotToCameraTransform unpack(ProtobufRobotToCameraTransform msg) { - return new RobotToCameraTransform(Transform3d.proto.unpack(msg.getRobotToCamera())); - } - - @Override - public void pack(ProtobufRobotToCameraTransform msg, RobotToCameraTransform value) { - Transform3d.proto.pack(msg.getMutableRobotToCamera(), value.robotToCamera); - } - - @Override - public Descriptor getDescriptor() { - return ProtobufRobotToCameraTransform.getDescriptor(); - } -} diff --git a/photon-targeting/src/main/native/include/photon/targeting/RobotToCameraTransform.h b/photon-targeting/src/main/native/include/photon/targeting/RobotToCameraTransform.h deleted file mode 100644 index 96eefb8231..0000000000 --- a/photon-targeting/src/main/native/include/photon/targeting/RobotToCameraTransform.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - * Copyright (C) Photon Vision. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#pragma once - -#include -#include "photon/dataflow/structures/Packet.h" -#include "photon/struct/RobotToCameraTransformStruct.h" - -namespace photon { - -struct RobotToCameraTransform : public RobotToCameraTransform_PhotonStruct { - using Base = RobotToCameraTransform_PhotonStruct; - - explicit RobotToCameraTransform(Base&& data) : Base(data) {} - - template - explicit RobotToCameraTransform(Args&&... args) : Base{std::forward(args)...} {} - - friend bool operator==(RobotToCameraTransform const&, RobotToCameraTransform const&) = default; -}; - -} // namespace photon - -#include "photon/serde/RobotToCameraTransformSerde.h" \ No newline at end of file diff --git a/photon-targeting/src/main/proto/photon.proto b/photon-targeting/src/main/proto/photon.proto index 4eb123f967..539c03042b 100644 --- a/photon-targeting/src/main/proto/photon.proto +++ b/photon-targeting/src/main/proto/photon.proto @@ -57,10 +57,6 @@ message ProtobufPhotonTrackedTarget { float obj_detection_conf = 12; } -message ProtobufRobotToCameraTransform { - wpi.proto.ProtobufTransform3d robotToCamera = 1; -} - message ProtobufPhotonPipelineResult { double latency_ms = 1 [deprecated = true]; From 656fde1479516afdaac445c1813b0bd08c9b2dd8 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sat, 7 Mar 2026 19:24:20 -0500 Subject: [PATCH 20/62] istg if this fixes the CI build --- .../targeting/proto/PhotonPipelineResultProto.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java index fe5ec4d10b..37c10c2acb 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java @@ -55,8 +55,8 @@ public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) { ? Optional.of(MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult())) : Optional.empty(), msg.hasRobotToCamera() - ? Optional.of(Transform3d.proto.unpack(msg.getRobotToCamera())) - : Optional.empty()); + ? Optional.of(Transform3d.proto.unpack(msg.getRobotToCamera())) + : Optional.empty()); } @Override From ac2b0835814f889065cade2404575b4cf40e82ec Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sat, 7 Mar 2026 19:54:49 -0500 Subject: [PATCH 21/62] added python photonpipelineresult --- .../generated/PhotonPipelineResultSerde.py | 5 +- .../generated/RobotToCameraTransformSerde.py | 61 ------------------- photon-lib/py/photonlibpy/packet.py | 18 +++++- .../targeting/photonPipelineResult.py | 2 + photon-serde/templates/ThingSerde.py.jinja | 9 +++ .../struct/PhotonPipelineResultSerde.java | 2 +- 6 files changed, 32 insertions(+), 65 deletions(-) delete mode 100644 photon-lib/py/photonlibpy/generated/RobotToCameraTransformSerde.py diff --git a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py index 97c295ce09..4e1cfed47b 100644 --- a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py +++ b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py @@ -57,7 +57,7 @@ def pack(value: "PhotonPipelineResult") -> "Packet": # multitagResult is optional! it better not be a VLA too ret.encodeOptional(value.multitagResult, MultiTargetPNPResult.photonStruct) - ret.encodeTransform(value.robotToCamera) + ret.encodeOptionalShimmed(value.robotToCamera, ret.encodeTransform) return ret @staticmethod @@ -73,7 +73,8 @@ def unpack(packet: "Packet") -> "PhotonPipelineResult": # multitagResult is optional! it better not be a VLA too ret.multitagResult = packet.decodeOptional(MultiTargetPNPResult.photonStruct) - ret.robotToCamera = packet.decodeTransform() + # robotToCamera is optional and shimmed! + ret.robotToCamera = packet.decodeOptionalShimmed(packet.decodeTransform) return ret diff --git a/photon-lib/py/photonlibpy/generated/RobotToCameraTransformSerde.py b/photon-lib/py/photonlibpy/generated/RobotToCameraTransformSerde.py deleted file mode 100644 index 5790a3370a..0000000000 --- a/photon-lib/py/photonlibpy/generated/RobotToCameraTransformSerde.py +++ /dev/null @@ -1,61 +0,0 @@ -# -# MIT License -# -# Copyright (c) PhotonVision -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. -# - -############################################################################### -## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. -## --> DO NOT MODIFY <-- -############################################################################### - -from typing import TYPE_CHECKING - -from ..packet import Packet -from ..targeting import * # noqa - -if TYPE_CHECKING: - from ..targeting import RobotToCameraTransform # noqa - - -class RobotToCameraTransformSerde: - # Message definition md5sum. See photon_packet.adoc for details - MESSAGE_VERSION = "575b4e398df72967da55b383dfe7784d" - MESSAGE_FORMAT = "Transform3d robotToCamera;" - - @staticmethod - def pack(value: "RobotToCameraTransform") -> "Packet": - ret = Packet() - - ret.encodeTransform(value.robotToCamera) - return ret - - @staticmethod - def unpack(packet: "Packet") -> "RobotToCameraTransform": - ret = RobotToCameraTransform() - - ret.robotToCamera = packet.decodeTransform() - - return ret - - -# Hack ourselves into the base class -RobotToCameraTransform.photonStruct = RobotToCameraTransformSerde() diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py index 8aae95062e..36d1452d81 100644 --- a/photon-lib/py/photonlibpy/packet.py +++ b/photon-lib/py/photonlibpy/packet.py @@ -16,7 +16,7 @@ ############################################################################### import struct -from typing import Generic, Optional, Protocol, TypeVar +from typing import Generic, Optional, Protocol, TypeVar, Callable import wpilib from wpimath.geometry import Quaternion, Rotation3d, Transform3d, Translation3d @@ -206,6 +206,12 @@ def decodeOptional(self, serde: Serde[T]) -> Optional[T]: return serde.unpack(self) else: return None + + def decodeOptionalShimmed(self, shim: Callable[[], T]) -> Optional[T]: + if self.decodeBoolean(): + return shim() + else: + return None def _encodeGeneric(self, packFormat, value): """ @@ -310,6 +316,16 @@ def encodeOptional(self, value: Optional[T], serde: Serde[T]): self.packetData = self.packetData + packed.getData() self.size = len(self.packetData) + def encodeOptionalShimmed(self, value: Optional[T], shim: Callable[[T], None]): + """ + Encodes an optional value using a specific shimmed serializer. + """ + if value is None: + self.encodeBoolean(False) + else: + self.encodeBoolean(True) + shim(value) + def encodeBytes(self, value: bytes): self.packetData = self.packetData + value self.size = len(self.packetData) diff --git a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py b/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py index 24d5d709c0..33075b7148 100644 --- a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py +++ b/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py @@ -1,5 +1,6 @@ from dataclasses import dataclass, field from typing import TYPE_CHECKING, ClassVar, Optional +from wpimath.geometry import Transform3d from .multiTargetPNPResult import MultiTargetPNPResult from .photonTrackedTarget import PhotonTrackedTarget @@ -34,6 +35,7 @@ class PhotonPipelineResult: # an arbitrary timebase. This is not true in C++ or Java. metadata: PhotonPipelineMetadata = field(default_factory=PhotonPipelineMetadata) multitagResult: Optional[MultiTargetPNPResult] = None + robotToCamera: Optional[Transform3d] = None def getLatencyMillis(self) -> float: return ( diff --git a/photon-serde/templates/ThingSerde.py.jinja b/photon-serde/templates/ThingSerde.py.jinja index 324757a520..fb6c4b4d3e 100644 --- a/photon-serde/templates/ThingSerde.py.jinja +++ b/photon-serde/templates/ThingSerde.py.jinja @@ -55,7 +55,11 @@ class {{ name }}Serde: ret = Packet() {% for field in fields -%} {%- if field.type | is_shimmed %} + {%- if field.optional == True %} + ret.encodeOptionalShimmed(value.{{ field.name }}, ret.{{ get_message_by_name(field.type).python_encode_shim }}) + {%- else %} ret.{{ get_message_by_name(field.type).python_encode_shim}}(value.{{ field.name }}) + {%- endif %} {%- elif field.optional == True %} # {{ field.name }} is optional! it better not be a VLA too ret.encodeOptional(value.{{ field.name }}, {{ field.type }}.photonStruct) @@ -82,7 +86,12 @@ class {{ name }}Serde: ret = {{ name }}() {% for field in fields -%} {%- if field.type | is_shimmed %} +{%- if field.optional == True %} + # {{ field.name }} is optional and shimmed! + ret.{{ field.name }} = packet.decodeOptionalShimmed(packet.{{ get_message_by_name(field.type).python_decode_shim }}) +{%- else %} ret.{{ field.name }} = packet.{{ get_message_by_name(field.type).python_decode_shim }}() +{%- endif %} {%- elif field.optional == True %} # {{ field.name }} is optional! it better not be a VLA too ret.{{ field.name }} = packet.decodeOptional({{ field.type }}.photonStruct) diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java index 03c8d127eb..1a21bf578b 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java @@ -99,7 +99,7 @@ public PhotonPipelineResult unpack(Packet packet) { @Override public PacketSerde[] getNestedPhotonMessages() { return new PacketSerde[] { - MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct,PhotonPipelineMetadata.photonStruct + PhotonPipelineMetadata.photonStruct,MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct }; } From cbfdb4563bdddce908ebace07efac7a0e51bf7d8 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sat, 7 Mar 2026 20:04:52 -0500 Subject: [PATCH 22/62] ran wpiformat --- photon-lib/py/photonlibpy/packet.py | 4 +- .../targeting/photonPipelineResult.py | 1 + .../native/cpp/photon/PhotonPoseEstimator.cpp | 2 +- .../cpp/photon/simulation/PhotonCameraSim.cpp | 3 +- .../main/native/include/photon/PhotonCamera.h | 10 +++-- .../cpp/LegacyPhotonPoseEstimatorTest.cpp | 44 ++++++++++++++----- .../src/test/native/cpp/PhotonCameraTest.cpp | 4 +- .../native/cpp/PhotonPoseEstimatorTest.cpp | 36 +++++++++++---- .../photon/dataflow/structures/Packet.h | 24 +++++----- .../photon/targeting/PhotonPipelineResult.h | 2 +- .../src/test/native/cpp/PacketTest.cpp | 13 ++++-- 11 files changed, 95 insertions(+), 48 deletions(-) diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py index 36d1452d81..5569d88c5f 100644 --- a/photon-lib/py/photonlibpy/packet.py +++ b/photon-lib/py/photonlibpy/packet.py @@ -16,7 +16,7 @@ ############################################################################### import struct -from typing import Generic, Optional, Protocol, TypeVar, Callable +from typing import Callable, Generic, Optional, Protocol, TypeVar import wpilib from wpimath.geometry import Quaternion, Rotation3d, Transform3d, Translation3d @@ -206,7 +206,7 @@ def decodeOptional(self, serde: Serde[T]) -> Optional[T]: return serde.unpack(self) else: return None - + def decodeOptionalShimmed(self, shim: Callable[[], T]) -> Optional[T]: if self.decodeBoolean(): return shim() diff --git a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py b/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py index 33075b7148..d6171741df 100644 --- a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py +++ b/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py @@ -1,5 +1,6 @@ from dataclasses import dataclass, field from typing import TYPE_CHECKING, ClassVar, Optional + from wpimath.geometry import Transform3d from .multiTargetPNPResult import MultiTargetPNPResult diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 13ff84d128..b51efcf634 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -54,7 +54,7 @@ #include WPI_IGNORE_DEPRECATED -//TODO: Update to use the new PhotonPipelineResult +// TODO: Update to use the new PhotonPipelineResult namespace photon { namespace detail { 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 f8f2f3f205..e5fa457860 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp @@ -343,8 +343,7 @@ PhotonPipelineResult PhotonCameraSim::Process( PhotonPipelineMetadata{heartbeatCounter, 0, units::microsecond_t{latency}.to(), 1000000}, - detectableTgts, multiTagResults, cam->GetRobotToCamera() - }; + detectableTgts, multiTagResults, cam->GetRobotToCamera()}; } void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) { SubmitProcessedFrame(result, wpi::Now()); diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index f5a76bdbb7..1454e94239 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -83,7 +83,8 @@ class PhotonCamera { * NTInstance from {@link NetworkTableInstance::getDefault} * @param cameraName The name of the camera, as seen in the UI. * over. - * @param robotToCamera The transform from the robot's center to the camera. This is used for pose estimation + * @param robotToCamera The transform from the robot's center to the camera. + * This is used for pose estimation */ explicit PhotonCamera(nt::NetworkTableInstance instance, const std::string_view cameraName, @@ -92,11 +93,12 @@ class PhotonCamera { PhotonCamera(PhotonCamera&&) = default; ~PhotonCamera() = default; - + /** * Returns the robot to camera transform - * - * @return The transform from the robot's center to the camera, if it was set. Empty otherwise. + * + * @return The transform from the robot's center to the camera, if it was set. + * Empty otherwise. */ std::optional GetRobotToCamera() { return robotToCamera; } diff --git a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp index 98c96c9493..f568880ebc 100644 --- a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp @@ -91,7 +91,9 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -152,7 +154,9 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator( @@ -201,7 +205,9 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, @@ -252,7 +258,9 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, @@ -293,7 +301,9 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, - std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); // std::optional estimatedPose; @@ -417,7 +427,9 @@ TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, @@ -472,7 +484,9 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { // empty input, expect empty out cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, - std::vector{}, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::vector{}, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1)); std::optional estimatedPose; @@ -484,7 +498,9 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { // Set result, and update -- expect present and timestamp to be 15 cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -553,7 +569,9 @@ TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -578,7 +596,9 @@ TEST(LegacyPhotonPoseEstimatorTest, CopyResult) { std::vector targets{}; auto testResult = photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; testResult.SetReceiveTimestamp(units::second_t(11)); auto test2 = testResult; @@ -628,7 +648,9 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, - multiTagResult, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + multiTagResult, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; cameraOne.test = true; cameraOne.testResult = {result}; diff --git a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp index de2539e345..503a7fe865 100644 --- a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp @@ -101,7 +101,9 @@ TEST(PhotonCameraTest, Alerts) { // AND a result with a timeSinceLastPong in the past photon::PhotonPipelineMetadata metadata{3, 1, 2, 10 * 1000000}; photon::PhotonPipelineResult noPongResult{ - metadata, std::vector{}, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + metadata, std::vector{}, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; // Loop to hit cases past first iteration for (int i = 0; i < 10; i++) { diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 6a5657b23b..123e80e6b0 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -90,7 +90,9 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); @@ -150,7 +152,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}}); @@ -198,7 +202,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -248,7 +254,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -288,7 +296,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, - std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -410,7 +420,9 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -451,7 +463,9 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); @@ -479,7 +493,9 @@ TEST(PhotonPoseEstimatorTest, CopyResult) { std::vector targets{}; auto testResult = photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; testResult.SetReceiveTimestamp(units::second_t(11)); auto test2 = testResult; @@ -519,7 +535,9 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, - multiTagResult, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + multiTagResult, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; cameraOne.test = true; cameraOne.testResult = {result}; 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 a2ea1555ba..9e34c1230a 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 @@ -34,31 +34,29 @@ namespace photon { class Packet; -template +template struct optional_inner; -template +template struct optional_inner> { - using type = T; + using type = T; }; -template -using optional_inner_t = - typename optional_inner>::type; +template +using optional_inner_t = typename optional_inner>::type; -template +template struct is_optional : std::false_type {}; -template +template struct is_optional> : std::true_type {}; -template +template concept Optional = is_optional>::value; -template -concept OptionalWPIStructSerializable = - Optional && - wpi::StructSerializable, I...>; +template +concept OptionalWPIStructSerializable = + Optional && wpi::StructSerializable, I...>; // Struct is where all our actual ser/de methods are implemented template 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 04ae33dba1..b7cf660357 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -63,7 +63,7 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { template explicit PhotonPipelineResult(Args&&... args) : Base{std::forward(args)...} {} - + /** * Returns the best target in this pipeline result. If there are no targets, * this method will return null. The best target is determined by the target diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp index 9ed0015ab4..007aa0128c 100644 --- a/photon-targeting/src/test/native/cpp/PacketTest.cpp +++ b/photon-targeting/src/test/native/cpp/PacketTest.cpp @@ -87,8 +87,11 @@ TEST(PacketTest, PnpResult) { // } TEST(PacketTest, PhotonPipelineResult) { - PhotonPipelineResult result(PhotonPipelineMetadata(0, 0, 1, 2), - std::vector{}, std::nullopt, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})); + PhotonPipelineResult result( + PhotonPipelineMetadata(0, 0, 1, 2), std::vector{}, + std::nullopt, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})); Packet p; p.Pack(result); @@ -132,8 +135,10 @@ TEST(PacketTest, PhotonPipelineResult) { 17.0, 22.33, 2.54}, std::vector{8, 7, 11, 22, 59, 40}}; - PhotonPipelineResult result2(PhotonPipelineMetadata{0, 0, 1, 1}, targets, - mtResult, std::make_optional(frc::Transform3d{frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})); + PhotonPipelineResult result2( + PhotonPipelineMetadata{0, 0, 1, 1}, targets, mtResult, + std::make_optional(frc::Transform3d{ + frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})); Packet p2; auto t1 = std::chrono::steady_clock::now(); From d6090a1e2def3c5a77ecb1030de71c660b4d41a5 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sun, 8 Mar 2026 12:40:39 -0400 Subject: [PATCH 23/62] Backend plumbing, added Camera position to frames --- .../networktables/NTDataPublisher.java | 6 +++++- .../vision/camera/FileVisionSource.java | 8 +++++++- .../org/photonvision/vision/frame/Frame.java | 19 ++++++++++++++++--- .../frame/provider/CpuImageProcessor.java | 16 ++++++++++++++-- .../frame/provider/FileFrameProvider.java | 17 +++++++++++++---- .../provider/LibcameraGpuFrameProvider.java | 3 ++- .../frame/provider/USBFrameProvider.java | 6 +++++- 7 files changed, 62 insertions(+), 13 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index ba83ae911a..52f2d886e2 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -203,7 +203,11 @@ public void accept(CVPipelineResult result) { NetworkTablesManager.getInstance().getTimeSinceLastPong(), TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets), acceptedResult.multiTagResult, - Optional.empty() // TODO: robotToCamera -- should pull this from the NT table, temporary + Optional.ofNullable( + acceptedResult + .inputAndOutputFrame + .robotToCamera) // TODO: robotToCamera -- should pull this from the NT table, + // temporary // solution for testing ); diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java index 76737fbf20..9127d6b7c9 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java @@ -19,6 +19,7 @@ import edu.wpi.first.cscore.UsbCameraInfo; import edu.wpi.first.cscore.VideoMode; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.util.PixelFormat; import java.nio.file.Path; import java.util.HashMap; @@ -45,7 +46,8 @@ public FileVisionSource(CameraConfiguration cameraConfiguration) { Path.of(cameraConfiguration.getDevicePath()), cameraConfiguration.FOV, FileFrameProvider.MAX_FPS, - calibration); + calibration, + this::getRobotToCamera); if (getCameraConfiguration().cameraQuirks == null) getCameraConfiguration().cameraQuirks = QuirkyCamera.DefaultCamera; @@ -54,6 +56,10 @@ public FileVisionSource(CameraConfiguration cameraConfiguration) { new FileSourceSettables(cameraConfiguration, frameProvider.get().frameStaticProperties); } + public Transform3d getRobotToCamera() { + return settables.getConfiguration().robotToCamera; + } + public FileVisionSource(String name, String imagePath, double fov) { // TODO - create new File/replay camera info type super( diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/Frame.java b/photon-core/src/main/java/org/photonvision/vision/frame/Frame.java index b37c03a8a2..9d99c6acb5 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/Frame.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/Frame.java @@ -17,6 +17,7 @@ package org.photonvision.vision.frame; +import edu.wpi.first.math.geometry.Transform3d; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.math.MathUtils; @@ -36,19 +37,23 @@ public class Frame implements Releasable { public final FrameStaticProperties frameStaticProperties; + public final Transform3d robotToCamera; + public Frame( long sequenceID, CVMat color, CVMat processed, FrameThresholdType type, long timestampNanos, - FrameStaticProperties frameStaticProperties) { + FrameStaticProperties frameStaticProperties, + Transform3d robotToCamera) { this.sequenceID = sequenceID; this.colorImage = color; this.processedImage = processed; this.type = type; this.timestampNanos = timestampNanos; this.frameStaticProperties = frameStaticProperties; + this.robotToCamera = robotToCamera; logger.trace( () -> @@ -66,7 +71,14 @@ public Frame( CVMat processed, FrameThresholdType processType, FrameStaticProperties frameStaticProperties) { - this(sequenceID, color, processed, processType, MathUtils.wpiNanoTime(), frameStaticProperties); + this( + sequenceID, + color, + processed, + processType, + MathUtils.wpiNanoTime(), + frameStaticProperties, + null); } public Frame() { @@ -76,7 +88,8 @@ public Frame() { new CVMat(), FrameThresholdType.NONE, MathUtils.wpiNanoTime(), - new FrameStaticProperties(0, 0, 0, null)); + new FrameStaticProperties(0, 0, 0, null), + null); } public void copyTo(Frame destFrame) { diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java index 173aae518a..453441156a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java @@ -17,6 +17,7 @@ package org.photonvision.vision.frame.provider; +import edu.wpi.first.math.geometry.Transform3d; import org.photonvision.common.util.numbers.IntegerCouple; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameProvider; @@ -33,12 +34,22 @@ protected static class CapturedFrame { CVMat colorImage; FrameStaticProperties staticProps; long captureTimestamp; + Transform3d robotToCamera; public CapturedFrame( - CVMat colorImage, FrameStaticProperties staticProps, long captureTimestampNanos) { + CVMat colorImage, + FrameStaticProperties staticProps, + long captureTimestampNanos, + Transform3d robotToCamera) { this.colorImage = colorImage; this.staticProps = staticProps; this.captureTimestamp = captureTimestampNanos; + this.robotToCamera = robotToCamera; + } + + public CapturedFrame( + CVMat colorImage, FrameStaticProperties staticProps, long captureTimestampNanos) { + this(colorImage, staticProps, captureTimestampNanos, null); } } @@ -93,7 +104,8 @@ public final Frame get() { input.captureTimestamp, input.staticProps != null ? input.staticProps.rotate(m_rImagePipe.getParams().rotation()) - : input.staticProps); + : input.staticProps, + input.robotToCamera); } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java index 2b281b517f..6ceab6e3da 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java @@ -17,9 +17,11 @@ package org.photonvision.vision.frame.provider; +import edu.wpi.first.math.geometry.Transform3d; import java.nio.file.Files; import java.nio.file.Path; import java.nio.file.Paths; +import java.util.function.Supplier; import org.opencv.core.Mat; import org.opencv.imgcodecs.Imgcodecs; import org.photonvision.common.util.math.MathUtils; @@ -41,6 +43,7 @@ public class FileFrameProvider extends CpuImageProcessor implements Releasable { private final Path path; private final int millisDelay; private final CVMat originalFrame; + private final Supplier robotToCameraSupplier; private final FrameStaticProperties properties; @@ -54,15 +57,20 @@ public class FileFrameProvider extends CpuImageProcessor implements Releasable { * @param maxFPS The max framerate to provide the image at. */ public FileFrameProvider(Path path, double fov, int maxFPS) { - this(path, fov, maxFPS, null); + this(path, fov, maxFPS, null, () -> null); } public FileFrameProvider(Path path, double fov, CameraCalibrationCoefficients calibration) { - this(path, fov, MAX_FPS, calibration); + this(path, fov, MAX_FPS, calibration, () -> null); } public FileFrameProvider( - Path path, double fov, int maxFPS, CameraCalibrationCoefficients calibration) { + Path path, + double fov, + int maxFPS, + CameraCalibrationCoefficients calibration, + Supplier robotToCameraSupplier) { + this.robotToCameraSupplier = robotToCameraSupplier; if (!Files.exists(path)) throw new RuntimeException("Invalid path for image: " + path.toAbsolutePath()); this.path = path; @@ -114,7 +122,8 @@ public CapturedFrame getInputMat() { } lastGetMillis = System.currentTimeMillis(); - return new CapturedFrame(out, properties, MathUtils.wpiNanoTime()); + return new CapturedFrame( + out, properties, MathUtils.wpiNanoTime(), this.robotToCameraSupplier.get()); } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java index e805602fe0..fa5ecffec2 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java @@ -103,7 +103,8 @@ public Frame get() { processedMat, type, MathUtils.wpiNanoTime() - latency, - settables.getFrameStaticProperties().rotate(settables.getRotation())); + settables.getFrameStaticProperties().rotate(settables.getRotation()), + settables.getConfiguration().robotToCamera); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java index 85d461ca18..3cd11990bf 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java @@ -127,7 +127,11 @@ public CapturedFrame getInputMat() { ret = new CVMat(mat, frame); } - return new CapturedFrame(ret, settables.getFrameStaticProperties(), captureTimeUs * 1000); + return new CapturedFrame( + ret, + settables.getFrameStaticProperties(), + captureTimeUs * 1000, + settables.getConfiguration().robotToCamera); } } From 87edd941bf951f996688e6df70302deb6246f05b Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sun, 8 Mar 2026 12:59:57 -0400 Subject: [PATCH 24/62] fixed null settables case in FileVisionSource --- .../java/org/photonvision/vision/camera/FileVisionSource.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java index 9127d6b7c9..e5629a493e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java @@ -57,6 +57,9 @@ public FileVisionSource(CameraConfiguration cameraConfiguration) { } public Transform3d getRobotToCamera() { + if (settables == null) { + return null; + } return settables.getConfiguration().robotToCamera; } From c31b8f10ff53d8875cdcf3911015cb24e6fedaa2 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Mon, 9 Mar 2026 11:02:37 -0400 Subject: [PATCH 25/62] updated PhotonPoseEstimator --- .../networktables/NTDataPublisher.java | 8 +- .../org/photonvision/PhotonPoseEstimator.java | 74 ++++++++++--------- .../LegacyPhotonPoseEstimatorTest.java | 9 +-- .../photonvision/PhotonPoseEstimatorTest.java | 74 ++++++++++--------- 4 files changed, 82 insertions(+), 83 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 52f2d886e2..31dbb2648b 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -203,13 +203,7 @@ public void accept(CVPipelineResult result) { NetworkTablesManager.getInstance().getTimeSinceLastPong(), TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets), acceptedResult.multiTagResult, - Optional.ofNullable( - acceptedResult - .inputAndOutputFrame - .robotToCamera) // TODO: robotToCamera -- should pull this from the NT table, - // temporary - // solution for testing - ); + Optional.ofNullable(acceptedResult.inputAndOutputFrame.robotToCamera)); // random guess at size of the array ts.resultPublisher.set(simplified, 1024); diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 2b560abe75..4b42b31e0b 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -126,7 +126,6 @@ public static final record ConstrainedSolvepnpParams( private TargetModel tagModel = TargetModel.kAprilTag36h11; private PoseStrategy primaryStrategy; private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY; - private Transform3d robotToCamera; private Pose3d lastPose; private Pose3d referencePose; @@ -148,10 +147,23 @@ public static final record ConstrainedSolvepnpParams( * robot âž” camera) in the Robot * Coordinate System. + * @deprecated PhotonPoseEstimator robotToCamera is now pulled from the Frame's robotToCamera, so + * this constructor is no longer necessary. Use the 1 argument constructor instead. */ public PhotonPoseEstimator(AprilTagFieldLayout fieldTags, Transform3d robotToCamera) { this.fieldTags = fieldTags; - this.robotToCamera = robotToCamera; + + HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); + InstanceCount++; + } + + /** + * Create a new PhotonPoseEstimator. + * + * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects + */ + public PhotonPoseEstimator(AprilTagFieldLayout fieldTags) { + this.fieldTags = fieldTags; HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); InstanceCount++; @@ -177,7 +189,6 @@ public PhotonPoseEstimator( AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) { this.fieldTags = fieldTags; this.primaryStrategy = strategy; - this.robotToCamera = robotToCamera; if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO || strategy == PoseStrategy.CONSTRAINED_SOLVEPNP) { @@ -397,23 +408,6 @@ public void resetHeadingData(double timestampSeconds, Rotation2d heading) { addHeadingData(timestampSeconds, heading); } - /** - * @return The current transform from the center of the robot to the camera mount position - */ - public Transform3d getRobotToCameraTransform() { - return robotToCamera; - } - - /** - * Useful for pan and tilt mechanisms and such. - * - * @param robotToCamera The current transform from the center of the robot to the camera mount - * position - */ - public void setRobotToCameraTransform(Transform3d robotToCamera) { - this.robotToCamera = robotToCamera; - } - /** * Updates the estimated position of the robot, assuming no camera calibration is required for the * selected strategy. Returns empty if: @@ -607,7 +601,7 @@ yield update( .get() .estimatedPose .best - .plus(robotToCamera.inverse())); + .plus(cameraResult.robotToCamera.get().inverse())); } else { // HACK - use fallback strategy to gimme a seed pose // TODO - make sure nested update doesn't break state @@ -671,6 +665,10 @@ private boolean shouldEstimate(PhotonPipelineResult cameraResult) { return false; } + if (cameraResult.getRobotToCamera().isEmpty()) { + return false; + } + // If no targets seen, trivial case -- can't do estimation return cameraResult.hasTargets(); } @@ -710,7 +708,7 @@ public Optional estimatePnpDistanceTrigSolvePose( 0, -Math.toRadians(bestTarget.getPitch()), -Math.toRadians(bestTarget.getYaw()))) - .rotateBy(robotToCamera.getRotation()) + .rotateBy(cameraResult.robotToCamera.get().getRotation()) .toTranslation2d() .rotateBy(headingSample); @@ -724,7 +722,13 @@ public Optional estimatePnpDistanceTrigSolvePose( tagPose2d.getTranslation().plus(camToTagTranslation.unaryMinus()); Translation2d camToRobotTranslation = - robotToCamera.getTranslation().toTranslation2d().unaryMinus().rotateBy(headingSample); + cameraResult + .robotToCamera + .get() + .getTranslation() + .toTranslation2d() + .unaryMinus() + .rotateBy(headingSample); Pose2d robotPose = new Pose2d(fieldToCameraTranslation.plus(camToRobotTranslation), headingSample); @@ -788,7 +792,7 @@ public Optional estimateConstrainedSolvepnpPose( cameraMatrix, distCoeffs, cameraResult.getTargets(), - robotToCamera, + cameraResult.robotToCamera.get(), seedPose, fieldTags, tagModel, @@ -826,7 +830,7 @@ public Optional estimateCoprocMultiTagPose( Pose3d.kZero .plus(best_tf) // field-to-camera .relativeTo(fieldTags.getOrigin()) - .plus(robotToCamera.inverse()); // field-to-robot + .plus(cameraResult.robotToCamera.get().inverse()); // field-to-robot return Optional.of( new EstimatedRobotPose( best, @@ -860,7 +864,7 @@ public Optional estimateRioMultiTagPose( var best = Pose3d.kZero .plus(pnpResult.get().best) // field-to-camera - .plus(robotToCamera.inverse()); // field-to-robot + .plus(cameraResult.robotToCamera.get().inverse()); // field-to-robot return Optional.of( new EstimatedRobotPose( @@ -914,7 +918,7 @@ public Optional estimateLowestAmbiguityPose( targetPosition .get() .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), + .transformBy(cameraResult.robotToCamera.get().inverse()), cameraResult.getTimestampSeconds(), cameraResult.getTargets(), PoseStrategy.LOWEST_AMBIGUITY)); @@ -953,14 +957,14 @@ public Optional estimateClosestToCameraHeightPose( double alternateTransformDelta = Math.abs( - robotToCamera.getZ() + cameraResult.robotToCamera.get().getZ() - targetPosition .get() .transformBy(target.getAlternateCameraToTarget().inverse()) .getZ()); double bestTransformDelta = Math.abs( - robotToCamera.getZ() + cameraResult.robotToCamera.get().getZ() - targetPosition .get() .transformBy(target.getBestCameraToTarget().inverse()) @@ -973,7 +977,7 @@ public Optional estimateClosestToCameraHeightPose( targetPosition .get() .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), + .transformBy(cameraResult.robotToCamera.get().inverse()), cameraResult.getTimestampSeconds(), cameraResult.getTargets(), PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); @@ -986,7 +990,7 @@ public Optional estimateClosestToCameraHeightPose( targetPosition .get() .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), + .transformBy(cameraResult.robotToCamera.get().inverse()), cameraResult.getTimestampSeconds(), cameraResult.getTargets(), PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); @@ -1040,12 +1044,12 @@ public Optional estimateClosestToReferencePose( targetPosition .get() .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); + .transformBy(cameraResult.robotToCamera.get().inverse()); Pose3d bestTransformPosition = targetPosition .get() .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); + .transformBy(cameraResult.robotToCamera.get().inverse()); double altDifference = Math.abs(calculateDifference(referencePose, altTransformPosition)); double bestDifference = Math.abs(calculateDifference(referencePose, bestTransformPosition)); @@ -1111,7 +1115,7 @@ public Optional estimateAverageBestTargetsPose( targetPosition .get() .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), + .transformBy(cameraResult.robotToCamera.get().inverse()), cameraResult.getTimestampSeconds(), cameraResult.getTargets(), PoseStrategy.AVERAGE_BEST_TARGETS)); @@ -1125,7 +1129,7 @@ public Optional estimateAverageBestTargetsPose( targetPosition .get() .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()))); + .transformBy(cameraResult.robotToCamera.get().inverse()))); } // Take the average diff --git a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java index 3f573abdbc..ad5ed6940c 100644 --- a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java @@ -549,8 +549,7 @@ void pnpDistanceTrigSolve() { /* this is the real pose of the robot base we test against */ var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197)); PhotonPipelineResult result = - cameraOneSim.process( - 1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets); + cameraOneSim.process(1, realPose.transformBy(compoundTestTransform), simTargets); var bestTarget = result.getBestTarget(); assertNotNull(bestTarget); assertEquals(0, bestTarget.fiducialId); @@ -566,13 +565,11 @@ void pnpDistanceTrigSolve() { /* Straight on */ Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero); - estimator.setRobotToCameraTransform(straightOnTestTransform); + // estimator.setRobotToCameraTransform(straightOnTestTransform); /* Pose to compare with */ realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818)); - result = - cameraOneSim.process( - 1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets); + result = cameraOneSim.process(1, realPose.transformBy(straightOnTestTransform), simTargets); estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d()); estimatedPose = estimator.update(result); diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index bfc169aa89..727a2f1ed3 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -161,9 +161,11 @@ void testLowestAmbiguityStrategy() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of(new Transform3d())); - PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, new Transform3d()); + PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags); Optional estimatedPose = estimator.estimateLowestAmbiguityPose(cameraOne.result); @@ -246,11 +248,11 @@ void testClosestToCameraHeightStrategy() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of(new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()))); - PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, new Transform3d(new Translation3d(0, 0, 4), new Rotation3d())); + PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags); Optional estimatedPose = estimator.estimateClosestToCameraHeightPose(cameraOne.result); @@ -333,11 +335,11 @@ void closestToReferencePoseStrategy() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of(new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()))); - PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())); + PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags); Optional estimatedPose = estimator.estimateClosestToReferencePose( @@ -421,11 +423,11 @@ void closestToLastPose() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of(new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()))); - PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())); + PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags); Optional estimatedPose = estimator.estimateClosestToReferencePose( @@ -501,7 +503,9 @@ void closestToLastPose() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of(new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()))); estimatedPose = estimator.estimateClosestToReferencePose(cameraOne.result, pose); pose = estimatedPose.get().estimatedPose; @@ -531,13 +535,12 @@ void pnpDistanceTrigSolve() { Units.degreesToRadians(6), Units.degreesToRadians(60))); - var estimator = new PhotonPoseEstimator(aprilTags, compoundTestTransform); + var estimator = new PhotonPoseEstimator(aprilTags); /* this is the real pose of the robot base we test against */ var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197)); PhotonPipelineResult result = - cameraOneSim.process( - 1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets); + cameraOneSim.process(1, realPose.transformBy(compoundTestTransform), simTargets); var bestTarget = result.getBestTarget(); assertNotNull(bestTarget); assertEquals(0, bestTarget.fiducialId); @@ -553,13 +556,11 @@ void pnpDistanceTrigSolve() { /* Straight on */ Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero); - estimator.setRobotToCameraTransform(straightOnTestTransform); + // estimator.setRobotToCameraTransform(straightOnTestTransform); /* Pose to compare with */ realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818)); - result = - cameraOneSim.process( - 1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets); + result = cameraOneSim.process(1, realPose.transformBy(straightOnTestTransform), simTargets); estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d()); estimatedPose = estimator.estimatePnpDistanceTrigSolvePose(result); @@ -642,11 +643,12 @@ void averageBestPoses() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); // 3 3 3 ambig .4 + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of( + new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()))); // 3 3 3 ambig .4 - PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())); + PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags); Optional estimatedPose = estimator.estimateAverageBestTargetsPose(cameraOne.result); @@ -709,8 +711,10 @@ void testMultiTagOnCoprocFallback() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); - PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, Transform3d.kZero); + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of(Transform3d.kZero)); + PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags); Optional estimatedPose = estimator.estimateCoprocMultiTagPose(camera.result); @@ -775,6 +779,10 @@ public void testConstrainedPnpOneTag() { new TargetCorner(127.17118732489361, 313.81406314178633), new TargetCorner(104.28543773760417, 309.6516557438994)); + final double camPitch = Units.degreesToRadians(30.0); + final Transform3d kRobotToCam = + new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0)); + var result = new PhotonPipelineResult( new PhotonPipelineMetadata(10000, 2000, 1, 100), @@ -794,15 +802,11 @@ public void testConstrainedPnpOneTag() { -0.08413452932300695, 0.9130568172784148))), 0.1), - new ArrayList(8)))); - - final double camPitch = Units.degreesToRadians(30.0); - final Transform3d kRobotToCam = - new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0)); + new ArrayList(8))), + Optional.of(kRobotToCam)); PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), kRobotToCam); + new PhotonPoseEstimator(AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo)); var multiTagEstimate = estimator.estimateCoprocMultiTagPose(result); estimator.addHeadingData(result.getTimestampSeconds(), Rotation2d.kZero); From 63d7e10bdc315edff3766ca899faba3d687c3e07 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Mon, 9 Mar 2026 12:27:58 -0400 Subject: [PATCH 26/62] updated LegacyPhotonPoseEstimatorTest --- .../LegacyPhotonPoseEstimatorTest.java | 45 ++++++++++++------- 1 file changed, 29 insertions(+), 16 deletions(-) diff --git a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java index ad5ed6940c..487fb66fc4 100644 --- a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java @@ -168,10 +168,12 @@ void testLowestAmbiguityStrategy() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of(new Transform3d())); PhotonPoseEstimator estimator = - new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d()); + new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, Transform3d.kZero); Optional estimatedPose = estimator.update(cameraOne.result); Pose3d pose = estimatedPose.get().estimatedPose; @@ -253,13 +255,13 @@ void testClosestToCameraHeightStrategy() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of(new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()))); PhotonPoseEstimator estimator = new PhotonPoseEstimator( - aprilTags, - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, - new Transform3d(new Translation3d(0, 0, 4), new Rotation3d())); + aprilTags, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, Transform3d.kZero); Optional estimatedPose = estimator.update(cameraOne.result); Pose3d pose = estimatedPose.get().estimatedPose; @@ -341,13 +343,13 @@ void closestToReferencePoseStrategy() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of(new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()))); PhotonPoseEstimator estimator = new PhotonPoseEstimator( - aprilTags, - PoseStrategy.CLOSEST_TO_REFERENCE_POSE, - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())); + aprilTags, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, new Transform3d()); estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d())); Optional estimatedPose = estimator.update(cameraOne.result); @@ -430,7 +432,9 @@ void closestToLastPose() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of(new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()))); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -512,7 +516,9 @@ void closestToLastPose() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of(new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()))); estimatedPose = estimator.update(cameraOne.result); pose = estimatedPose.get().estimatedPose; @@ -610,7 +616,9 @@ void cacheIsInvalidated() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of(new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()))); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -728,7 +736,10 @@ void averageBestPoses() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); // 3 3 3 ambig .4 + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of( + new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()))); // 3 3 3 ambig .4 PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -795,7 +806,9 @@ void testMultiTagOnRioFallback() { new TargetCorner(1, 2), new TargetCorner(3, 4), new TargetCorner(5, 6), - new TargetCorner(7, 8))))); + new TargetCorner(7, 8)))), + Optional.empty(), + Optional.of(Transform3d.kZero)); PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero); estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); @@ -882,7 +895,7 @@ public void testConstrainedPnpOneTag() { 0.9130568172784148))), 0.1), new ArrayList(8))), - Optional.of(new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(4, 5, 6)))); + Optional.of(kRobotToCam)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( From e786a1d63df343d3db0b3745201b7cd82ddb3023 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Mon, 9 Mar 2026 12:50:22 -0400 Subject: [PATCH 27/62] hopefully fixed the problems in LegacyPhotonPoseEstimatorTest --- .../LegacyPhotonPoseEstimatorTest.java | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java index 487fb66fc4..d4fca046b7 100644 --- a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java @@ -535,19 +535,18 @@ void pnpDistanceTrigSolve() { aprilTags.getTags().stream() .map((AprilTag x) -> new VisionTargetSim(x.pose, TargetModel.kAprilTag36h11, x.ID)) .toList(); + /* Compound Rolled + Pitched + Yaw */ + Transform3d compoundTestTransform = + new Transform3d( + -Units.inchesToMeters(12), + -Units.inchesToMeters(11), + 3, + new Rotation3d( + Units.degreesToRadians(37), Units.degreesToRadians(6), Units.degreesToRadians(60))); + + cameraOne.setCameraTransform(compoundTestTransform); try (PhotonCameraSim cameraOneSim = new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())) { - /* Compound Rolled + Pitched + Yaw */ - Transform3d compoundTestTransform = - new Transform3d( - -Units.inchesToMeters(12), - -Units.inchesToMeters(11), - 3, - new Rotation3d( - Units.degreesToRadians(37), - Units.degreesToRadians(6), - Units.degreesToRadians(60))); - var estimator = new PhotonPoseEstimator( aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform); @@ -572,6 +571,7 @@ void pnpDistanceTrigSolve() { Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero); // estimator.setRobotToCameraTransform(straightOnTestTransform); + cameraOne.setCameraTransform(straightOnTestTransform); /* Pose to compare with */ realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818)); From 496c996b3cbf6187377a9cb89440232074a1dbb9 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Mon, 9 Mar 2026 13:10:41 -0400 Subject: [PATCH 28/62] i fixed it yaaay --- .../photonvision/PhotonPoseEstimatorTest.java | 23 +++++++++---------- 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 727a2f1ed3..e525fe19d4 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -522,19 +522,17 @@ void pnpDistanceTrigSolve() { aprilTags.getTags().stream() .map((AprilTag x) -> new VisionTargetSim(x.pose, TargetModel.kAprilTag36h11, x.ID)) .toList(); + /* Compound Rolled + Pitched + Yaw */ + Transform3d compoundTestTransform = + new Transform3d( + -Units.inchesToMeters(12), + -Units.inchesToMeters(11), + 3, + new Rotation3d( + Units.degreesToRadians(37), Units.degreesToRadians(6), Units.degreesToRadians(60))); + cameraOne.setCameraTransform(compoundTestTransform); try (PhotonCameraSim cameraOneSim = new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())) { - /* Compound Rolled + Pitched + Yaw */ - Transform3d compoundTestTransform = - new Transform3d( - -Units.inchesToMeters(12), - -Units.inchesToMeters(11), - 3, - new Rotation3d( - Units.degreesToRadians(37), - Units.degreesToRadians(6), - Units.degreesToRadians(60))); - var estimator = new PhotonPoseEstimator(aprilTags); /* this is the real pose of the robot base we test against */ @@ -556,7 +554,7 @@ void pnpDistanceTrigSolve() { /* Straight on */ Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero); - // estimator.setRobotToCameraTransform(straightOnTestTransform); + cameraOne.setCameraTransform(straightOnTestTransform); /* Pose to compare with */ realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818)); @@ -729,6 +727,7 @@ void testMultiTagOnCoprocFallback() { assertEquals(1, pose.getX(), 1e-9); assertEquals(3, pose.getY(), 1e-9); assertEquals(2, pose.getZ(), 1e-9); + camera.close(); } @Test From 38f635688df3125d5e5c1ecfbb0edd91b02acad9 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Mon, 9 Mar 2026 17:45:20 -0400 Subject: [PATCH 29/62] updated C++ PhotonPoseEstimator --- .../native/cpp/photon/PhotonPoseEstimator.cpp | 48 +++++++++++++------ .../main/native/include/photon/PhotonCamera.h | 7 +++ .../include/photon/PhotonPoseEstimator.h | 17 ++++++- .../cpp/LegacyPhotonPoseEstimatorTest.cpp | 47 ++++++++---------- .../native/cpp/PhotonPoseEstimatorTest.cpp | 33 +++++-------- .../photon/targeting/PhotonPipelineResult.h | 4 ++ 6 files changed, 93 insertions(+), 63 deletions(-) diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index b51efcf634..600df488a1 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -66,6 +66,19 @@ cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX, units::meter_t cornerY, frc::Pose3d tagPose); } // namespace detail + +PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags) + : aprilTags(tags), + m_robotToCamera(), + lastPose(frc::Pose3d()), + referencePose(frc::Pose3d()), + poseCacheTimestamp(-1_s), + headingBuffer(frc::TimeInterpolatableBuffer(1_s)) { + HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator, + InstanceCount); + InstanceCount++; +} + PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, frc::Transform3d robotToCamera) : aprilTags(tags), @@ -212,7 +225,7 @@ std::optional PhotonPoseEstimator::Update( if (result.MultiTagResult().has_value()) { fieldToRobotSeed = frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best + - m_robotToCamera.Inverse()); + result.GetRobotToCamera().value().Inverse()); } else { std::optional nestedUpdate = Update(result, cameraMatrixData, cameraDistCoeffs, {}, @@ -259,6 +272,13 @@ bool ShouldEstimate(const PhotonPipelineResult& result) { return false; } + // Result has no robot to camera transform -- can't do estimation + if (!result.GetRobotToCamera().has_value()) { + FRC_ReportError(frc::warn::Warning, + "Result has no robot to camera transform!"); + return false; + } + // If no targets seen, trivial case -- can't do estimation return result.HasTargets(); } @@ -296,7 +316,7 @@ PhotonPoseEstimator::EstimateLowestAmbiguityPose( return EstimatedRobotPose{ fiducialPose->TransformBy(bestTarget.GetBestCameraToTarget().Inverse()) - .TransformBy(m_robotToCamera.Inverse()), + .TransformBy(cameraResult.GetRobotToCamera().value().Inverse()), cameraResult.GetTimestamp(), cameraResult.GetTargets(), LOWEST_AMBIGUITY}; } @@ -323,19 +343,19 @@ PhotonPoseEstimator::EstimateClosestToCameraHeightPose( frc::Pose3d const targetPose = *fiducialPose; units::meter_t const alternativeDifference = units::math::abs( - m_robotToCamera.Z() - + cameraResult.GetRobotToCamera().value().Z() - targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse()) .Z()); units::meter_t const bestDifference = units::math::abs( - m_robotToCamera.Z() - + cameraResult.GetRobotToCamera().value().Z() - targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z()); if (alternativeDifference < smallestHeightDifference) { smallestHeightDifference = alternativeDifference; pose = EstimatedRobotPose{ targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse()) - .TransformBy(m_robotToCamera.Inverse()), + .TransformBy(cameraResult.GetRobotToCamera().value().Inverse()), cameraResult.GetTimestamp(), cameraResult.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT}; } @@ -343,7 +363,7 @@ PhotonPoseEstimator::EstimateClosestToCameraHeightPose( smallestHeightDifference = bestDifference; pose = EstimatedRobotPose{ targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()) - .TransformBy(m_robotToCamera.Inverse()), + .TransformBy(cameraResult.GetRobotToCamera().value().Inverse()), cameraResult.GetTimestamp(), cameraResult.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT}; } @@ -377,10 +397,10 @@ PhotonPoseEstimator::EstimateClosestToReferencePose( const auto altPose = targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse()) - .TransformBy(m_robotToCamera.Inverse()); + .TransformBy(cameraResult.GetRobotToCamera().value().Inverse()); const auto bestPose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()) - .TransformBy(m_robotToCamera.Inverse()); + .TransformBy(cameraResult.GetRobotToCamera().value().Inverse()); units::meter_t const alternativeDifference = units::math::abs( referencePose.Translation().Distance(altPose.Translation())); @@ -462,7 +482,7 @@ PhotonPoseEstimator::EstimateCoprocMultiTagPose( const auto field2camera = cameraResult.MultiTagResult()->estimatedPose.best; const auto fieldToRobot = - frc::Pose3d() + field2camera + m_robotToCamera.Inverse(); + frc::Pose3d() + field2camera + cameraResult.GetRobotToCamera().value().Inverse(); return photon::EstimatedRobotPose(fieldToRobot, cameraResult.GetTimestamp(), cameraResult.GetTargets(), MULTI_TAG_PNP_ON_COPROCESSOR); @@ -519,7 +539,7 @@ std::optional PhotonPoseEstimator::EstimateRioMultiTagPose( const frc::Pose3d pose = detail::ToPose3d(tvec, rvec); return photon::EstimatedRobotPose( - pose.TransformBy(m_robotToCamera.Inverse()), cameraResult.GetTimestamp(), + pose.TransformBy(cameraResult.GetRobotToCamera().value().Inverse()), cameraResult.GetTimestamp(), cameraResult.GetTargets(), MULTI_TAG_PNP_ON_RIO); } @@ -545,7 +565,7 @@ PhotonPoseEstimator::EstimatePnpDistanceTrigSolvePose( bestTarget.GetBestCameraToTarget().Translation().Norm(), frc::Rotation3d(0_rad, -units::degree_t(bestTarget.GetPitch()), -units::degree_t(bestTarget.GetYaw()))) - .RotateBy(m_robotToCamera.Rotation()) + .RotateBy(cameraResult.GetRobotToCamera().value().Rotation()) .ToTranslation2d() .RotateBy(headingSample); @@ -564,7 +584,7 @@ PhotonPoseEstimator::EstimatePnpDistanceTrigSolvePose( tagPose.Translation() - camToTagTranslation; frc::Translation2d camToRobotTranslation = - (-m_robotToCamera.Translation().ToTranslation2d()) + (-cameraResult.GetRobotToCamera().value().Translation().ToTranslation2d()) .RotateBy(headingSample); frc::Pose2d robotPose = frc::Pose2d( @@ -600,7 +620,7 @@ PhotonPoseEstimator::EstimateAverageBestTargetsPose( if (target.GetPoseAmbiguity() == 0) { return EstimatedRobotPose{ targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()) - .TransformBy(m_robotToCamera.Inverse()), + .TransformBy(cameraResult.GetRobotToCamera().value().Inverse()), cameraResult.GetTimestamp(), cameraResult.GetTargets(), AVERAGE_BEST_TARGETS}; } @@ -653,7 +673,7 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose( std::optional pnpResult = VisionEstimation::EstimateRobotPoseConstrainedSolvePNP( - cameraMatrix, distCoeffs, targets, m_robotToCamera, seedPose, + cameraMatrix, distCoeffs, targets, cameraResult.GetRobotToCamera().value(), seedPose, aprilTags, photon::kAprilTag36h11, headingFree, frc::Rotation2d{ headingBuffer.Sample(cameraResult.GetTimestamp()).value()}, diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 1454e94239..0ea74269cb 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -102,6 +102,13 @@ class PhotonCamera { */ std::optional GetRobotToCamera() { return robotToCamera; } + + /** + * Sets the robot to camera transform + * + * @param newRobotToCamera the robot to camera transform + */ + void SetRobotToCamera(frc::Transform3d newRobotToCamera) { robotToCamera = std::make_optional(newRobotToCamera);} /** * The list of pipeline results sent by PhotonVision since the last call to * GetAllUnreadResults(). Calling this function clears the internal FIFO diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index 8e3cc8ffcc..a51bb580d0 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -83,6 +83,16 @@ struct EstimatedRobotPose { */ class PhotonPoseEstimator { public: + + + /** + * Create a new PhotonPoseEstimator. + * + * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with + * respect to the FIRST field. + */ + + explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags); /** * Create a new PhotonPoseEstimator. * @@ -90,6 +100,7 @@ class PhotonPoseEstimator { * respect to the FIRST field. * @param robotToCamera Transform3d from the center of the robot to the camera * mount positions (ie, robot âž” camera). + * @deprecated robotToCamera is now retrieved from PhotonPipelineResults */ explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags, frc::Transform3d robotToCamera); @@ -102,11 +113,11 @@ class PhotonPoseEstimator { * @param strategy The strategy it should use to determine the best pose. * @param robotToCamera Transform3d from the center of the robot to the camera * mount positions (ie, robot âž” camera). - * @deprecated Use individual estimation methods with the 2 argument + * @deprecated Use individual estimation methods with the 1 argument * constructor instead. */ [[deprecated( - "Use individual estimation methods with the 2 argument constructor " + "Use individual estimation methods with the 1 argument constructor " "instead.")]] explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags, PoseStrategy strategy, @@ -193,6 +204,8 @@ class PhotonPoseEstimator { * * @param robotToCamera The current transform from the center of the robot to * the camera mount position. + * + * @deprecated robotToCamera is now stored in PhotonPipelineResult */ inline void SetRobotToCameraTransform(frc::Transform3d robotToCamera) { m_robotToCamera = robotToCamera; diff --git a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp index f568880ebc..dd45b66010 100644 --- a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp @@ -92,8 +92,7 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional(frc::Transform3d{})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -155,8 +154,8 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional(frc::Transform3d + {{0_m, 0_m, 4_m}, {}})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator( @@ -206,8 +205,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, @@ -259,8 +257,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, @@ -302,8 +299,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); // std::optional estimatedPose; @@ -338,6 +334,8 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) { frc::Transform3d compoundTestTransform = frc::Transform3d( -12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg)); + cameraOne.SetRobotToCamera(compoundTestTransform); + photon::PhotonPoseEstimator estimator( aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform); @@ -346,7 +344,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) { frc::Pose3d(7.3_m, 4.42_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.197_rad)); photon::PhotonPipelineResult result = cameraOneSim.Process( - 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), + 1_ms, realPose.TransformBy(cameraOne.GetRobotToCamera().value()), targets); cameraOne.testResult = {result}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); @@ -372,11 +370,11 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) { frc::Transform3d straightOnTestTransform = frc::Transform3d(0_m, 0_m, 3_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)); - estimator.SetRobotToCameraTransform(straightOnTestTransform); + cameraOne.SetRobotToCamera(straightOnTestTransform); realPose = frc::Pose3d(4.81_m, 2.38_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.818_rad)); result = cameraOneSim.Process( - 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), + 1_ms, realPose.TransformBy(cameraOne.GetRobotToCamera().value()), targets); cameraOne.testResult = {result}; cameraOne.testResult[0].SetReceiveTimestamp(18_s); @@ -428,8 +426,7 @@ TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, @@ -485,8 +482,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, std::vector{}, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1)); std::optional estimatedPose; @@ -499,8 +495,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { // Set result, and update -- expect present and timestamp to be 15 cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -570,8 +565,7 @@ TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -646,20 +640,19 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PnpResult{poseTransform, poseTransform, 0.1, 0.1, 0.0}, std::vector{8}); + const units::radian_t camPitch = 30_deg; + const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m), + frc::Rotation3d(0_rad, -camPitch, 0_rad)}; + photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, multiTagResult, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + std::make_optional(kRobotToCam)}; cameraOne.test = true; cameraOne.testResult = {result}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); - const units::radian_t camPitch = 30_deg; - const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m), - frc::Rotation3d(0_rad, -camPitch, 0_rad)}; - photon::PhotonPoseEstimator estimator( frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo), photon::CONSTRAINED_SOLVEPNP, kRobotToCam); diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 123e80e6b0..9a9b0a255f 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -91,8 +91,7 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); @@ -153,8 +152,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({{0_m, 0_m, 4_m}, {}})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}}); @@ -203,8 +201,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -255,8 +252,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -297,8 +293,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -333,6 +328,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { -12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg)); photon::PhotonPoseEstimator estimator(aprilTags, compoundTestTransform); + cameraOne.SetRobotToCamera(compoundTestTransform); /* real pose of the robot base to test against */ frc::Pose3d realPose = @@ -366,6 +362,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { frc::Transform3d(0_m, 0_m, 3_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)); estimator.SetRobotToCameraTransform(straightOnTestTransform); + cameraOne.SetRobotToCamera(straightOnTestTransform); realPose = frc::Pose3d(4.81_m, 2.38_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.818_rad)); result = cameraOneSim.Process( @@ -421,8 +418,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, {}); @@ -464,8 +460,7 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); @@ -533,20 +528,18 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PnpResult{poseTransform, poseTransform, 0.1, 0.1, 0.0}, std::vector{8}); + const units::radian_t camPitch = 30_deg; + const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m), + frc::Rotation3d(0_rad, -camPitch, 0_rad)}; photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, multiTagResult, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + std::make_optional(kRobotToCam)}; cameraOne.test = true; cameraOne.testResult = {result}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); - const units::radian_t camPitch = 30_deg; - const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m), - frc::Rotation3d(0_rad, -camPitch, 0_rad)}; - photon::PhotonPoseEstimator estimator( frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo), kRobotToCam); 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 b7cf660357..c29200a50b 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -84,6 +84,10 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { return HasTargets() ? targets[0] : PhotonTrackedTarget{}; } + std::optional GetRobotToCamera() const { + return robotToCamera; + } + /** * Returns the latency in the pipeline. * @return The latency in the pipeline. From f7ebc15272f71b3fa48aab76bfdb2896f67924aa Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Mon, 9 Mar 2026 18:02:29 -0400 Subject: [PATCH 30/62] ran wpiformat --- .../native/cpp/photon/PhotonPoseEstimator.cpp | 15 ++++++++------- .../src/main/native/include/photon/PhotonCamera.h | 7 ++++--- .../native/include/photon/PhotonPoseEstimator.h | 6 ++---- .../native/cpp/LegacyPhotonPoseEstimatorTest.cpp | 10 ++++------ .../test/native/cpp/PhotonPoseEstimatorTest.cpp | 6 ++---- 5 files changed, 20 insertions(+), 24 deletions(-) diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 600df488a1..f759e7f576 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -66,7 +66,6 @@ cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX, units::meter_t cornerY, frc::Pose3d tagPose); } // namespace detail - PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags) : aprilTags(tags), m_robotToCamera(), @@ -481,8 +480,8 @@ PhotonPoseEstimator::EstimateCoprocMultiTagPose( const auto field2camera = cameraResult.MultiTagResult()->estimatedPose.best; - const auto fieldToRobot = - frc::Pose3d() + field2camera + cameraResult.GetRobotToCamera().value().Inverse(); + const auto fieldToRobot = frc::Pose3d() + field2camera + + cameraResult.GetRobotToCamera().value().Inverse(); return photon::EstimatedRobotPose(fieldToRobot, cameraResult.GetTimestamp(), cameraResult.GetTargets(), MULTI_TAG_PNP_ON_COPROCESSOR); @@ -539,8 +538,9 @@ std::optional PhotonPoseEstimator::EstimateRioMultiTagPose( const frc::Pose3d pose = detail::ToPose3d(tvec, rvec); return photon::EstimatedRobotPose( - pose.TransformBy(cameraResult.GetRobotToCamera().value().Inverse()), cameraResult.GetTimestamp(), - cameraResult.GetTargets(), MULTI_TAG_PNP_ON_RIO); + pose.TransformBy(cameraResult.GetRobotToCamera().value().Inverse()), + cameraResult.GetTimestamp(), cameraResult.GetTargets(), + MULTI_TAG_PNP_ON_RIO); } std::optional @@ -673,8 +673,9 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose( std::optional pnpResult = VisionEstimation::EstimateRobotPoseConstrainedSolvePNP( - cameraMatrix, distCoeffs, targets, cameraResult.GetRobotToCamera().value(), seedPose, - aprilTags, photon::kAprilTag36h11, headingFree, + cameraMatrix, distCoeffs, targets, + cameraResult.GetRobotToCamera().value(), seedPose, aprilTags, + photon::kAprilTag36h11, headingFree, frc::Rotation2d{ headingBuffer.Sample(cameraResult.GetTimestamp()).value()}, headingScaleFactor); diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 0ea74269cb..cef00d0cdf 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -102,13 +102,14 @@ class PhotonCamera { */ std::optional GetRobotToCamera() { return robotToCamera; } - /** * Sets the robot to camera transform - * + * * @param newRobotToCamera the robot to camera transform */ - void SetRobotToCamera(frc::Transform3d newRobotToCamera) { robotToCamera = std::make_optional(newRobotToCamera);} + void SetRobotToCamera(frc::Transform3d newRobotToCamera) { + robotToCamera = std::make_optional(newRobotToCamera); + } /** * The list of pipeline results sent by PhotonVision since the last call to * GetAllUnreadResults(). Calling this function clears the internal FIFO diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index a51bb580d0..7bce702e09 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -83,11 +83,9 @@ struct EstimatedRobotPose { */ class PhotonPoseEstimator { public: - - /** * Create a new PhotonPoseEstimator. - * + * * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with * respect to the FIRST field. */ @@ -204,7 +202,7 @@ class PhotonPoseEstimator { * * @param robotToCamera The current transform from the center of the robot to * the camera mount position. - * + * * @deprecated robotToCamera is now stored in PhotonPipelineResult */ inline void SetRobotToCameraTransform(frc::Transform3d robotToCamera) { diff --git a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp index dd45b66010..eb7968630d 100644 --- a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp @@ -154,8 +154,8 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d - {{0_m, 0_m, 4_m}, {}})}}; + std::make_optional( + frc::Transform3d{{0_m, 0_m, 4_m}, {}})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator( @@ -298,8 +298,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, - std::nullopt, - std::make_optional({})}}; + std::nullopt, std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); // std::optional estimatedPose; @@ -646,8 +645,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, - multiTagResult, - std::make_optional(kRobotToCam)}; + multiTagResult, std::make_optional(kRobotToCam)}; cameraOne.test = true; cameraOne.testResult = {result}; diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 9a9b0a255f..de69d58dcc 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -292,8 +292,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, - std::nullopt, - std::make_optional({})}}; + std::nullopt, std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -533,8 +532,7 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { frc::Rotation3d(0_rad, -camPitch, 0_rad)}; photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, - multiTagResult, - std::make_optional(kRobotToCam)}; + multiTagResult, std::make_optional(kRobotToCam)}; cameraOne.test = true; cameraOne.testResult = {result}; From ac00977615d7e5bce88fa536acdc77baa898268e Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Tue, 10 Mar 2026 13:25:39 -0400 Subject: [PATCH 31/62] updated C++ PhotonCamera NT --- photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp | 7 +++++++ photon-lib/src/main/native/include/photon/PhotonCamera.h | 6 +++--- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index bca4e47521..486d5a1120 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -131,6 +131,8 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, .Subscribe( TYPE_STRING, {}, {.pollStorage = 20, .periodic = 0.01, .sendAll = true})), + robotToCameraPublisher( + rootTable->GetStructTopic("robotToCamera").Publish()), inputSaveImgEntry( rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()), inputSaveImgSubscriber( @@ -460,4 +462,9 @@ std::vector PhotonCamera::tablesThatLookLikePhotonCameras() { return ret; } +void PhotonCamera::SetRobotToCamera(frc::Transform3d newRobotToCamera) { + robotToCamera = std::make_optional(newRobotToCamera); + robotToCameraPublisher.Set(robotToCamera.value()); +} + } // namespace photon diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index cef00d0cdf..98686e749e 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -107,9 +108,7 @@ class PhotonCamera { * * @param newRobotToCamera the robot to camera transform */ - void SetRobotToCamera(frc::Transform3d newRobotToCamera) { - robotToCamera = std::make_optional(newRobotToCamera); - } + void SetRobotToCamera(frc::Transform3d newRobotToCamera); /** * The list of pipeline results sent by PhotonVision since the last call to * GetAllUnreadResults(). Calling this function clears the internal FIFO @@ -246,6 +245,7 @@ class PhotonCamera { std::shared_ptr mainTable; std::shared_ptr rootTable; nt::RawSubscriber rawBytesEntry; + nt::StructPublisher robotToCameraPublisher; nt::IntegerPublisher inputSaveImgEntry; nt::IntegerSubscriber inputSaveImgSubscriber; nt::IntegerPublisher outputSaveImgEntry; From 93e679015aef823ad04f5074450673d013eaf97f Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Tue, 10 Mar 2026 16:35:55 -0400 Subject: [PATCH 32/62] updated C++ and python photonCameras to publish robottocamera transform --- photon-lib/py/photonlibpy/photonCamera.py | 26 ++++++++++++++++--- .../main/native/cpp/photon/PhotonCamera.cpp | 1 + 2 files changed, 24 insertions(+), 3 deletions(-) diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index a8961113cd..9c66cd80b4 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -16,7 +16,7 @@ ############################################################################### from enum import Enum -from typing import List +from typing import List, Optional import hal import ntcore @@ -24,6 +24,7 @@ # magical import to make serde stuff work import photonlibpy.generated # noqa import wpilib +from wpilib.geometry import Transform3d from wpilib import RobotController, Timer from .packet import Packet @@ -51,7 +52,7 @@ def setVersionCheckEnabled(enabled: bool): class PhotonCamera: instance_count = 1 - def __init__(self, cameraName: str): + def __init__(self, cameraName: str, robotToCamera: Transform3d): """Constructs a PhotonCamera from the name of the camera. :param cameraName: The nickname of the camera (found in the PhotonVision UI). @@ -67,7 +68,10 @@ def __init__(self, cameraName: str): bytes([]), ntcore.PubSubOptions(periodic=0.01, sendAll=True), ) - + self._robotToCameraPublisher = self._cameraTable.getStructTopic( + "robotToCamera", + Transform3d + ).publish() self._driverModePublisher = self._cameraTable.getBooleanTopic( "driverModeRequest" ).publish() @@ -111,6 +115,8 @@ def __init__(self, cameraName: str): instance, ["/photonvision/"], ntcore.PubSubOptions(topicsOnly=True) ) + self._robotToCameraPublisher.set(robotToCamera) + self._prevHeartbeat = 0 self._prevHeartbeatChangeTime = Timer.getFPGATimestamp() @@ -179,6 +185,20 @@ def getLatestResult(self) -> PhotonPipelineResult: # We don't trust NT4 time, hack around retVal.ntReceiveTimestampMicros = now return retVal + + def getRobotToCamera(self) -> Optional[Transform3d]: + """ + returns the robot to camera transform, or None if not present + + :returns: the robot to camera transform, or None if not present""" + return self._robotToCamera + + def setRobotToCamera(self, robotToCamera: Transform3d) -> None: + """ + Set the robotToCamera, and publish to NT + """ + self._robotToCamera = robotToCamera + self._robotToCameraPublisher.set(robotToCamera) def getDriverMode(self) -> bool: """Returns whether the camera is in driver mode. diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 486d5a1120..0034b333d2 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -171,6 +171,7 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, timesyncAlert(PHOTON_ALERT_GROUP, "", frc::Alert::AlertType::kWarning), robotToCamera(robotToCamera) { verifyDependencies(); + if (robotToCamera.is_present()) robotToCameraPublisher.Set(robotToCamera.value()); HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount); InstanceCount++; From aed5bc6a3085f123e0972129b56eaabf849e3a53 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Tue, 10 Mar 2026 17:10:43 -0400 Subject: [PATCH 33/62] fixed bug --- photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 0034b333d2..8faa737922 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -171,7 +171,7 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, timesyncAlert(PHOTON_ALERT_GROUP, "", frc::Alert::AlertType::kWarning), robotToCamera(robotToCamera) { verifyDependencies(); - if (robotToCamera.is_present()) robotToCameraPublisher.Set(robotToCamera.value()); + if (robotToCamera.has_value()) robotToCameraPublisher.Set(robotToCamera.value()); HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount); InstanceCount++; From b91c2b694d1aef4d942f5d985966248610e76978 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Wed, 11 Mar 2026 15:47:42 -0400 Subject: [PATCH 34/62] fixed photon-docs build error --- photon-docs/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photon-docs/build.gradle b/photon-docs/build.gradle index 6d80846933..238d756109 100644 --- a/photon-docs/build.gradle +++ b/photon-docs/build.gradle @@ -89,7 +89,7 @@ doxygen.sourceSets.main { tasks.register("zipCppDocs", Zip) { archiveBaseName = zipBaseNameCpp destinationDirectory = outputsFolder - dependsOn doxygen + dependsOn doxygenDox from ("$buildDir/docs/doxygen/html") into '/' } From 824eb8d4358a09346ce24e9f97faa7d38c2c8dde Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 12 Mar 2026 00:50:09 -0400 Subject: [PATCH 35/62] fixed issue where the backend would read NT before photonlib published the robottocamera transform --- .../networktables/NTDataPublisher.java | 21 +++++++++++++++++-- .../frame/provider/USBFrameProvider.java | 6 +++++- .../vision/processes/VisionModule.java | 4 +--- .../vision/processes/VisionRunner.java | 2 ++ .../common/networktables/NTTopicSet.java | 12 ++++++++--- 5 files changed, 36 insertions(+), 9 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 31dbb2648b..07b7883723 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -23,6 +23,8 @@ import edu.wpi.first.networktables.NetworkTablesJNI; import java.util.List; import java.util.Optional; +import java.util.concurrent.ExecutorService; +import java.util.concurrent.Executors; import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.Supplier; @@ -78,6 +80,16 @@ public NTDataPublisher( updateCameraNickname(cameraNickname); updateEntries(); + while (!ts.robotToCameraExists()) { + try { + Thread.sleep(100); // yield + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + } + initialTransform = ts.robotToCameraSubscriber.get(); + // if a robotToCamera is already being published on NT, update + updateRobotToCamera(ts.robotToCameraSubscriber.get(null)); } private void onPipelineIndexChange(NetworkTableEvent entryNotification) { @@ -106,8 +118,12 @@ private void onPipelineIndexChange(NetworkTableEvent entryNotification) { private void onRobotToCameraChange(NetworkTableEvent entryNotification) { var newRobotToCamera = (Transform3d) entryNotification.valueData.value.getValue(); - robotToCameraConsumer.accept(newRobotToCamera); - logger.debug("Updated robot to camera transform to " + newRobotToCamera); + updateRobotToCamera(newRobotToCamera); + } + + private void updateRobotToCamera(Transform3d robotToCamera) { + robotToCameraConsumer.accept(robotToCamera); + logger.debug("Updated robot to camera transform to " + robotToCamera); } private void onDriverModeChange(NetworkTableEvent entryNotification) { @@ -146,6 +162,7 @@ private void updateEntries() { if (pipelineIndexListener != null) pipelineIndexListener.remove(); if (driverModeListener != null) driverModeListener.remove(); if (fpsLimitListener != null) fpsLimitListener.remove(); + if (robotToCameraListener != null) robotToCameraListener.remove(); ts.updateEntries(); diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java index 3cd11990bf..e76b58eac7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java @@ -91,7 +91,11 @@ public CapturedFrame getInputMat() { logger.error("Error grabbing image: " + error); } - return new CapturedFrame(mat, settables.getFrameStaticProperties(), captureTimeNs); + return new CapturedFrame( + mat, + settables.getFrameStaticProperties(), + captureTimeNs, + settables.getConfiguration().robotToCamera); } else { // We allocate memory so we don't fill a Mat in use by another thread (memory model is easier) // TODO - consider a frame pool diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index f83b76cd44..4890324f3b 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -90,8 +90,6 @@ public class VisionModule { private int fpsLimit = -1; - private Transform3d robotToCameraTransform = null; - FileSaveFrameConsumer inputFrameSaver; FileSaveFrameConsumer outputFrameSaver; @@ -647,7 +645,7 @@ public void setFPSLimit(int fps) { * the robot's coordinate system. This should be provided in meters. */ public void setRobotToCameraTransform(Transform3d robotToCameraTransform) { - this.robotToCameraTransform = robotToCameraTransform; + this.visionSource.getSettables().setRobotToCamera(robotToCameraTransform); saveAndBroadcastAll(); } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java index 6835bf1e9c..a9dc7fb618 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java @@ -38,6 +38,8 @@ import org.photonvision.vision.pipeline.CVPipeline; import org.photonvision.vision.pipeline.result.CVPipelineResult; +import edu.wpi.first.math.geometry.Transform3d; + /** VisionRunner has a frame supplier, a pipeline supplier, and a result consumer */ @SuppressWarnings("rawtypes") public class VisionRunner { diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index 7f9c68c38e..f874b2508f 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -31,6 +31,8 @@ import edu.wpi.first.networktables.PubSubOption; import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.networktables.StructSubscriber; +import edu.wpi.first.networktables.StructTopic; + import org.photonvision.targeting.PhotonPipelineResult; /** @@ -80,8 +82,8 @@ public class NTTopicSet { public DoubleArrayPublisher cameraDistortionPublisher; // Camera Intrinsics + public StructTopic robotToCameraTopic; public StructSubscriber robotToCameraSubscriber; - public StructPublisher robotToCameraPublisher; public void updateEntries() { var rawBytesEntry = @@ -132,8 +134,12 @@ public void updateEntries() { cameraIntrinsicsPublisher = subTable.getDoubleArrayTopic("cameraIntrinsics").publish(); cameraDistortionPublisher = subTable.getDoubleArrayTopic("cameraDistortion").publish(); - robotToCameraSubscriber = - subTable.getStructTopic("robotToCamera", Transform3d.struct).subscribe(null); + robotToCameraTopic = subTable.getStructTopic("robotToCamera", Transform3d.struct); + robotToCameraSubscriber = robotToCameraTopic.subscribe(null); + } + + public boolean robotToCameraExists() { + return robotToCameraTopic.exists(); } @SuppressWarnings("DuplicatedCode") From 92c67be0364034e0d3f778bea301f936a2d8002c Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 12 Mar 2026 11:41:52 -0400 Subject: [PATCH 36/62] fixed bug in NTDataPublisher --- .../common/dataflow/networktables/NTDataPublisher.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 07b7883723..0f34de658c 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -80,6 +80,7 @@ public NTDataPublisher( updateCameraNickname(cameraNickname); updateEntries(); + // Wait to connect to NT before polling robotToCamera while (!ts.robotToCameraExists()) { try { Thread.sleep(100); // yield @@ -87,8 +88,6 @@ public NTDataPublisher( Thread.currentThread().interrupt(); } } - initialTransform = ts.robotToCameraSubscriber.get(); - // if a robotToCamera is already being published on NT, update updateRobotToCamera(ts.robotToCameraSubscriber.get(null)); } From 39f3989df4a6d85df67d636890646da8a7353563 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 12 Mar 2026 11:45:46 -0400 Subject: [PATCH 37/62] ran wpiformat --- .../common/dataflow/networktables/NTDataPublisher.java | 2 +- photon-lib/py/photonlibpy/photonCamera.py | 9 ++++----- photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp | 6 ++++-- photon-lib/src/main/native/include/photon/PhotonCamera.h | 2 +- 4 files changed, 10 insertions(+), 9 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 0f34de658c..897e7a2913 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -80,7 +80,7 @@ public NTDataPublisher( updateCameraNickname(cameraNickname); updateEntries(); - // Wait to connect to NT before polling robotToCamera + // HACK: Wait to connect to NT before polling robotToCamera while (!ts.robotToCameraExists()) { try { Thread.sleep(100); // yield diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index 9c66cd80b4..4cdb38ed24 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -24,8 +24,8 @@ # magical import to make serde stuff work import photonlibpy.generated # noqa import wpilib -from wpilib.geometry import Transform3d from wpilib import RobotController, Timer +from wpilib.geometry import Transform3d from .packet import Packet from .targeting.photonPipelineResult import PhotonPipelineResult @@ -69,8 +69,7 @@ def __init__(self, cameraName: str, robotToCamera: Transform3d): ntcore.PubSubOptions(periodic=0.01, sendAll=True), ) self._robotToCameraPublisher = self._cameraTable.getStructTopic( - "robotToCamera", - Transform3d + "robotToCamera", Transform3d ).publish() self._driverModePublisher = self._cameraTable.getBooleanTopic( "driverModeRequest" @@ -185,14 +184,14 @@ def getLatestResult(self) -> PhotonPipelineResult: # We don't trust NT4 time, hack around retVal.ntReceiveTimestampMicros = now return retVal - + def getRobotToCamera(self) -> Optional[Transform3d]: """ returns the robot to camera transform, or None if not present :returns: the robot to camera transform, or None if not present""" return self._robotToCamera - + def setRobotToCamera(self, robotToCamera: Transform3d) -> None: """ Set the robotToCamera, and publish to NT diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 8faa737922..0aa4d97506 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -132,7 +132,8 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, TYPE_STRING, {}, {.pollStorage = 20, .periodic = 0.01, .sendAll = true})), robotToCameraPublisher( - rootTable->GetStructTopic("robotToCamera").Publish()), + rootTable->GetStructTopic("robotToCamera") + .Publish()), inputSaveImgEntry( rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()), inputSaveImgSubscriber( @@ -171,7 +172,8 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, timesyncAlert(PHOTON_ALERT_GROUP, "", frc::Alert::AlertType::kWarning), robotToCamera(robotToCamera) { verifyDependencies(); - if (robotToCamera.has_value()) robotToCameraPublisher.Set(robotToCamera.value()); + if (robotToCamera.has_value()) + robotToCameraPublisher.Set(robotToCamera.value()); HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount); InstanceCount++; diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 98686e749e..e79478741b 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -38,8 +38,8 @@ #include #include #include -#include #include +#include #include #include "photon/targeting/PhotonPipelineResult.h" From 5a8b3cee710c904926df6ff20acc19d381950a94 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 12 Mar 2026 14:01:53 -0400 Subject: [PATCH 38/62] fixed rNTDataPublisher::onRobotToCameraChange, removed hacky robotToCamera NT polling system --- .../configuration/CameraConfiguration.java | 10 ---------- .../networktables/NTDataPublisher.java | 19 +++---------------- .../vision/camera/FileVisionSource.java | 2 +- .../provider/LibcameraGpuFrameProvider.java | 2 +- .../frame/provider/USBFrameProvider.java | 7 ++----- .../vision/processes/VisionRunner.java | 2 -- .../processes/VisionSourceSettables.java | 7 +++++-- .../common/networktables/NTTopicSet.java | 1 - 8 files changed, 12 insertions(+), 38 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java index d19171e5f0..176ddc755a 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java @@ -211,16 +211,6 @@ public void removeCalibration(Size unrotatedImageSize) { }); } - /** - * Set the robot to camera transform for this camera configuration. - * - * @param robotToCamera the transform from the robot's origin to the camera's origin, in the - * robot's coordinate system. - */ - public void setRobotToCamera(Transform3d robotToCamera) { - this.robotToCamera = robotToCamera; - } - /** * cscore will auto-reconnect to the camera path we give it. v4l does not guarantee that if i swap * cameras around, the same /dev/videoN ID will be assigned to that camera. So instead default to diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 897e7a2913..697b0031d6 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -23,8 +23,6 @@ import edu.wpi.first.networktables.NetworkTablesJNI; import java.util.List; import java.util.Optional; -import java.util.concurrent.ExecutorService; -import java.util.concurrent.Executors; import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.Supplier; @@ -80,15 +78,6 @@ public NTDataPublisher( updateCameraNickname(cameraNickname); updateEntries(); - // HACK: Wait to connect to NT before polling robotToCamera - while (!ts.robotToCameraExists()) { - try { - Thread.sleep(100); // yield - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); - } - } - updateRobotToCamera(ts.robotToCameraSubscriber.get(null)); } private void onPipelineIndexChange(NetworkTableEvent entryNotification) { @@ -116,11 +105,9 @@ private void onPipelineIndexChange(NetworkTableEvent entryNotification) { } private void onRobotToCameraChange(NetworkTableEvent entryNotification) { - var newRobotToCamera = (Transform3d) entryNotification.valueData.value.getValue(); - updateRobotToCamera(newRobotToCamera); - } - - private void updateRobotToCamera(Transform3d robotToCamera) { + // HACK: the entryNotification's value can't be cast to Transform3d, so we read directly from + // the subscriber + var robotToCamera = ts.robotToCameraSubscriber.get(); robotToCameraConsumer.accept(robotToCamera); logger.debug("Updated robot to camera transform to " + robotToCamera); } diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java index e5629a493e..1738e17236 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java @@ -60,7 +60,7 @@ public Transform3d getRobotToCamera() { if (settables == null) { return null; } - return settables.getConfiguration().robotToCamera; + return settables.getRobotToCamera(); } public FileVisionSource(String name, String imagePath, double fov) { diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java index fa5ecffec2..21938f0ff6 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java @@ -104,7 +104,7 @@ public Frame get() { type, MathUtils.wpiNanoTime() - latency, settables.getFrameStaticProperties().rotate(settables.getRotation()), - settables.getConfiguration().robotToCamera); + settables.getRobotToCamera()); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java index e76b58eac7..324fd776f0 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java @@ -92,10 +92,7 @@ public CapturedFrame getInputMat() { } return new CapturedFrame( - mat, - settables.getFrameStaticProperties(), - captureTimeNs, - settables.getConfiguration().robotToCamera); + mat, settables.getFrameStaticProperties(), captureTimeNs, settables.getRobotToCamera()); } else { // We allocate memory so we don't fill a Mat in use by another thread (memory model is easier) // TODO - consider a frame pool @@ -135,7 +132,7 @@ public CapturedFrame getInputMat() { ret, settables.getFrameStaticProperties(), captureTimeUs * 1000, - settables.getConfiguration().robotToCamera); + settables.getRobotToCamera()); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java index a9dc7fb618..6835bf1e9c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java @@ -38,8 +38,6 @@ import org.photonvision.vision.pipeline.CVPipeline; import org.photonvision.vision.pipeline.result.CVPipelineResult; -import edu.wpi.first.math.geometry.Transform3d; - /** VisionRunner has a frame supplier, a pipeline supplier, and a result consumer */ @SuppressWarnings("rawtypes") public class VisionRunner { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index 2a94dd6617..e50a31edfe 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java @@ -128,8 +128,11 @@ public void removeCalibration(Size unrotatedImageSize) { } public void setRobotToCamera(Transform3d robotToCamera) { - configuration.setRobotToCamera(robotToCamera); - // calculateFrameStaticProps(); // This shouldn't be necessary + configuration.robotToCamera = robotToCamera; + } + + public Transform3d getRobotToCamera() { + return configuration.robotToCamera; } protected void calculateFrameStaticProps() { diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index f874b2508f..2a1bf2f7a9 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -32,7 +32,6 @@ import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.networktables.StructSubscriber; import edu.wpi.first.networktables.StructTopic; - import org.photonvision.targeting.PhotonPipelineResult; /** From 7ed34bcf3d594d02d6248bd01125852e7dd36be8 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Fri, 13 Mar 2026 22:27:36 -0400 Subject: [PATCH 39/62] updated photonlibpy pose estimator --- photon-lib/py/photonlibpy/photonPoseEstimator.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py index e47251360f..ae9ea937ce 100644 --- a/photon-lib/py/photonlibpy/photonPoseEstimator.py +++ b/photon-lib/py/photonlibpy/photonPoseEstimator.py @@ -126,6 +126,9 @@ def _shouldEstimate(self, cameraResult: PhotonPipelineResult) -> bool: """ if cameraResult.getTimestampSeconds() < 0: return False + + if cameraResult.robotToCamera == None: + return False # If no targets seen, trivial case -- can't do estimation return len(cameraResult.targets) > 0 @@ -180,7 +183,7 @@ def estimatePnpDistanceTrigSolvePose( tagPose.toPose2d().translation() - camToTagTranslation ) camToRobotTranslation: Translation2d = -( - self.robotToCamera.translation().toTranslation2d() + result.robotToCamera.translation().toTranslation2d() ) camToRobotTranslation = camToRobotTranslation.rotateBy(headingSample) robotPose = Pose2d( @@ -209,7 +212,7 @@ def estimateCoprocMultiTagPose( Pose3d() .transformBy(best_tf) # field-to-camera .relativeTo(self._fieldTags.getOrigin()) - .transformBy(self.robotToCamera.inverse()) # field-to-robot + .transformBy(result.robotToCamera.inverse()) # field-to-robot ) return EstimatedRobotPose( best, @@ -258,7 +261,7 @@ def estimateLowestAmbiguityPose( return EstimatedRobotPose( targetPosition.transformBy( lowestAmbiguityTarget.getBestCameraToTarget().inverse() - ).transformBy(self.robotToCamera.inverse()), + ).transformBy(result.robotToCamera.inverse()), result.getTimestampSeconds(), result.targets, ) From 21c5ee8f5ef243c030552b98e455d73c08198f6b Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Wed, 25 Mar 2026 12:49:28 -0400 Subject: [PATCH 40/62] made robotToCamera thread-safe --- .../common/configuration/CameraConfiguration.java | 2 -- .../photonvision/vision/processes/VisionModule.java | 4 +--- .../vision/processes/VisionSourceSettables.java | 10 ++++++++-- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java index 176ddc755a..26367d380f 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java @@ -57,8 +57,6 @@ public class CameraConfiguration { public QuirkyCamera cameraQuirks; - public Transform3d robotToCamera = null; - public double FOV = 70; public List calibrations = new ArrayList<>(); public int currentPipelineIndex = 0; diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index 4890324f3b..97edecf0f7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -638,15 +638,13 @@ public void setFPSLimit(int fps) { } /** - * Set camera transform for this vision module. As of now this doesn't affect vision processing in - * any way + * Set camera transform for this vision module. * * @param robotToCameraTransform the transform from the robot's origin to the camera's origin, in * the robot's coordinate system. This should be provided in meters. */ public void setRobotToCameraTransform(Transform3d robotToCameraTransform) { this.visionSource.getSettables().setRobotToCamera(robotToCameraTransform); - saveAndBroadcastAll(); } /** diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index e50a31edfe..f77a2c5b6f 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java @@ -20,6 +20,10 @@ import edu.wpi.first.cscore.VideoMode; import edu.wpi.first.math.geometry.Transform3d; import java.util.HashMap; +import java.util.concurrent.atomic.AtomicReference; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; + import org.opencv.core.Size; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.logging.LogGroup; @@ -32,6 +36,8 @@ public abstract class VisionSourceSettables { private final CameraConfiguration configuration; + private AtomicReference robotToCamera = new AtomicReference<>(); + protected VisionSourceSettables(CameraConfiguration configuration) { this.configuration = configuration; this.logger = @@ -128,11 +134,11 @@ public void removeCalibration(Size unrotatedImageSize) { } public void setRobotToCamera(Transform3d robotToCamera) { - configuration.robotToCamera = robotToCamera; + this.robotToCamera.set(robotToCamera); } public Transform3d getRobotToCamera() { - return configuration.robotToCamera; + return this.robotToCamera.get(); } protected void calculateFrameStaticProps() { From 3ffbd8546479f40749445c0df87a19321d0f2741 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 26 Mar 2026 15:04:44 -0400 Subject: [PATCH 41/62] switched to a lockless atomicreference system --- .../photonvision/vision/processes/VisionSourceSettables.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index f77a2c5b6f..b9249cbfb9 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java @@ -36,7 +36,7 @@ public abstract class VisionSourceSettables { private final CameraConfiguration configuration; - private AtomicReference robotToCamera = new AtomicReference<>(); + private final AtomicReference robotToCamera = new AtomicReference<>(); protected VisionSourceSettables(CameraConfiguration configuration) { this.configuration = configuration; From 1e925b729bbd6de5e3b0be3431987fe33b17444c Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sun, 29 Mar 2026 14:17:22 -0400 Subject: [PATCH 42/62] ran spotlessApply --- .../photonvision/common/configuration/CameraConfiguration.java | 1 - .../photonvision/vision/processes/VisionSourceSettables.java | 3 --- 2 files changed, 4 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java index 26367d380f..e51175f36c 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java @@ -21,7 +21,6 @@ import com.fasterxml.jackson.annotation.JsonIgnore; import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.cscore.UsbCameraInfo; -import edu.wpi.first.math.geometry.Transform3d; import java.util.ArrayList; import java.util.List; import java.util.UUID; diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index b9249cbfb9..1bee69c526 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java @@ -21,9 +21,6 @@ import edu.wpi.first.math.geometry.Transform3d; import java.util.HashMap; import java.util.concurrent.atomic.AtomicReference; -import java.util.concurrent.locks.Lock; -import java.util.concurrent.locks.ReentrantLock; - import org.opencv.core.Size; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.logging.LogGroup; From fbcb7127c56f59c8f3dc29c67898da092f15a839 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sun, 29 Mar 2026 14:27:47 -0400 Subject: [PATCH 43/62] ran wpiformat --- photon-lib/py/photonlibpy/photonPoseEstimator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py index ae9ea937ce..9d596ed94c 100644 --- a/photon-lib/py/photonlibpy/photonPoseEstimator.py +++ b/photon-lib/py/photonlibpy/photonPoseEstimator.py @@ -126,7 +126,7 @@ def _shouldEstimate(self, cameraResult: PhotonPipelineResult) -> bool: """ if cameraResult.getTimestampSeconds() < 0: return False - + if cameraResult.robotToCamera == None: return False From c7701e99a2201611a034f13978eb7a8ebab72447 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sun, 29 Mar 2026 17:23:32 -0400 Subject: [PATCH 44/62] removed robotToCamera from photonposeestimator entirely --- .../py/photonlibpy/photonPoseEstimator.py | 7 +-- .../photonlibpy/simulation/photonCameraSim.py | 1 + .../py/test/photonPoseEstimator_test.py | 18 +++---- .../java/org/photonvision/PhotonCamera.java | 52 +++++++++---------- .../org/photonvision/PhotonPoseEstimator.java | 32 +----------- .../simulation/PhotonCameraSim.java | 2 +- .../main/native/cpp/photon/PhotonCamera.cpp | 12 ++++- .../native/cpp/photon/PhotonPoseEstimator.cpp | 18 +------ .../main/native/include/photon/PhotonCamera.h | 26 ++++++++-- .../include/photon/PhotonPoseEstimator.h | 38 +------------- .../LegacyPhotonPoseEstimatorTest.java | 39 +++++--------- .../photonvision/PhotonPoseEstimatorTest.java | 4 +- .../cpp/LegacyPhotonPoseEstimatorTest.cpp | 32 ++++++------ .../native/cpp/PhotonPoseEstimatorTest.cpp | 22 ++++---- .../src/main/java/frc/robot/Vision.java | 4 +- 15 files changed, 116 insertions(+), 191 deletions(-) diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py index 9d596ed94c..27f3d6b76f 100644 --- a/photon-lib/py/photonlibpy/photonPoseEstimator.py +++ b/photon-lib/py/photonlibpy/photonPoseEstimator.py @@ -26,7 +26,6 @@ Pose3d, Rotation2d, Rotation3d, - Transform3d, Translation2d, Translation3d, ) @@ -48,7 +47,6 @@ class PhotonPoseEstimator: def __init__( self, fieldTags: AprilTagFieldLayout, - robotToCamera: Transform3d, ): """Create a new PhotonPoseEstimator. @@ -56,11 +54,8 @@ def __init__( with respect to the FIRST field using the Field Coordinate System. Note that setting the origin of this layout object will affect the results from this class. - :param robotToCamera: Transform3d from the center of the robot to the camera mount position (i.e., - robot âž” camera) in the Robot Coordinate System. """ self._fieldTags = fieldTags - self.robotToCamera = robotToCamera self._reportedErrors: set[int] = set() self._headingBuffer = TimeInterpolatableRotation2dBuffer(1) @@ -174,7 +169,7 @@ def estimatePnpDistanceTrigSolvePose( -wpimath.units.degreesToRadians(bestTarget.getYaw()), ), ) - .rotateBy(self.robotToCamera.rotation()) + .rotateBy(result.robotToCamera.rotation()) .toTranslation2d() .rotateBy(headingSample) ) diff --git a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py index a1ee052a1d..691cb6eabf 100644 --- a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py +++ b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py @@ -435,6 +435,7 @@ def distance(target: VisionTargetSim): ), targets=detectableTgts, multitagResult=multiTagResults, + robotToCamera=self.cam.getRobotToCamera(), ) def submitProcessedFrame( diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py index ad9693664b..942322e51a 100644 --- a/photon-lib/py/test/photonPoseEstimator_test.py +++ b/photon-lib/py/test/photonPoseEstimator_test.py @@ -34,8 +34,8 @@ class PhotonCameraInjector(PhotonCamera): result: PhotonPipelineResult - def __init__(self, cameraName="camera"): - super().__init__(cameraName) + def __init__(self, cameraName="camera", robotToCamera=Transform3d()): + super().__init__(cameraName, robotToCamera) def getLatestResult(self) -> PhotonPipelineResult: return self.result @@ -134,11 +134,11 @@ def test_lowestAmbiguityStrategy(): ], metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0), multitagResult=None, + robotToCamera=Transform3d() ) estimator = PhotonPoseEstimator( - aprilTags, - Transform3d(), + aprilTags ) estimatedPose = estimator.estimateLowestAmbiguityPose(cameraOne.result) @@ -178,9 +178,9 @@ def test_pnpDistanceTrigSolve(): ) estimator = PhotonPoseEstimator( - aprilTags, - compoundTestTransform, + aprilTags ) + cameraOne.setRobotToCamera(compoundTestTransform) realPose = Pose3d(7.3, 4.42, 0, Rotation3d(0, 0, 2.197)) # Pose to compare with result = cameraOneSim.process( @@ -207,7 +207,7 @@ def test_pnpDistanceTrigSolve(): # Straight on fakeTimestampSecs += 60 straightOnTestTransform = Transform3d(0, 0, 3, Rotation3d()) - estimator.robotToCamera = straightOnTestTransform + cameraOne.setRobotToCamera(straightOnTestTransform) realPose = Pose3d(4.81, 2.38, 0, Rotation3d(0, 0, 2.818)) # Pose to compare with result = cameraOneSim.process( latencySecs, realPose.transformBy(estimator.robotToCamera), simTargets @@ -265,11 +265,11 @@ def test_multiTagOnCoprocStrategy(): multitagResult=MultiTargetPNPResult( PnpResult(Transform3d(1, 3, 2, Rotation3d())) ), + robotToCamera=Transform3d() ) estimator = PhotonPoseEstimator( - AprilTagFieldLayout(), - Transform3d(), + AprilTagFieldLayout() ) estimatedPose = estimator.estimateCoprocMultiTagPose(cameraOne.result) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 0a980161b6..20445381f8 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -138,8 +138,10 @@ public static void setVersionCheckEnabled(boolean enabled) { * simulation, but should *usually* be the default NTInstance from * NetworkTableInstance::getDefault * @param cameraName The name of the camera, as seen in the UI. - * @param robotToCamera The transform from the robot to the camera. This is used for pose - * estimation + * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, + * robot âž” camera) in the Robot + * Coordinate System. */ public PhotonCamera(NetworkTableInstance instance, String cameraName, Transform3d robotToCamera) { this(instance, cameraName, Optional.of(robotToCamera)); @@ -157,16 +159,7 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { this(instance, cameraName, Optional.empty()); } - /** - * Internal implementation of the constructor - * - * @param instance The NetworkTableInstance to pull data from. This can be a custom instance in - * simulation, but should *usually* be the default NTInstance from - * NetworkTableInstance::getDefault - * @param cameraName The name of the camera, as seen in the UI. - * @param robotToCamera The transform from the robot to the camera. This is used for pose - * estimation - */ + /** Internal implementation of the constructor */ private PhotonCamera( NetworkTableInstance instance, String cameraName, Optional robotToCamera) { name = cameraName; @@ -224,19 +217,10 @@ private PhotonCamera( verifyDependencies(); if (robotToCamera.isPresent()) { - publishRobotToCameraTransform(); + robotToCameraPublisher.set(robotToCamera.get()); } } - private void publishRobotToCameraTransform() { - robotToCamera.ifPresentOrElse( - (transform) -> robotToCameraPublisher.set(transform), - () -> - robotToCameraPublisher.set( - new Transform3d()) // TODO: See if this default value can cause any issues. - ); - } - static void verifyDependencies() { // spotless:off if (!Core.VERSION.equals(PhotonVersion.opencvTargetVersion)) { @@ -295,6 +279,19 @@ public PhotonCamera(String cameraName) { this(NetworkTableInstance.getDefault(), cameraName); } + /** + * Constructs a PhotonCamera from the name of the camera and its transform from the robot. + * + * @param cameraName the name of the camera, as seen in the UI + * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, + * robot âž” camera) in the Robot + * Coordinate System. + */ + public PhotonCamera(String cameraName, Transform3d robotToCamera) { + this(NetworkTableInstance.getDefault(), cameraName, Optional.of(robotToCamera)); + } + /** * The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults(). * Calling this function clears the internal FIFO queue, and multiple calls to @@ -556,18 +553,21 @@ public final NetworkTable getCameraTable() { * * @return The camera's transform from the robot, if it is set. Empty otherwise. */ - public Optional getCameraTransform() { + public Optional getRobotToCamera() { return robotToCamera; } /** * Sets the camera's transform from the robot. This is used for pose estimation. * - * @param robotToCamera The transform from the robot to the camera. + * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, + * robot âž” camera) in the Robot + * Coordinate System. */ - public void setCameraTransform(Transform3d robotToCamera) { + public void setRobotToCamera(Transform3d robotToCamera) { this.robotToCamera = Optional.of(robotToCamera); - publishRobotToCameraTransform(); + robotToCameraPublisher.set(robotToCamera); } void verifyVersion() { diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 4b42b31e0b..0562aa9a3a 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -34,7 +34,6 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; @@ -135,28 +134,6 @@ public static final record ConstrainedSolvepnpParams( private final TimeInterpolatableBuffer headingBuffer = TimeInterpolatableBuffer.createBuffer(1.0); - /** - * Create a new PhotonPoseEstimator. - * - * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects - * with respect to the FIRST field using the Field - * Coordinate System. Note that setting the origin of this layout object will affect the - * results from this class. - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot âž” camera) in the Robot - * Coordinate System. - * @deprecated PhotonPoseEstimator robotToCamera is now pulled from the Frame's robotToCamera, so - * this constructor is no longer necessary. Use the 1 argument constructor instead. - */ - public PhotonPoseEstimator(AprilTagFieldLayout fieldTags, Transform3d robotToCamera) { - this.fieldTags = fieldTags; - - HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); - InstanceCount++; - } - /** * Create a new PhotonPoseEstimator. * @@ -178,15 +155,10 @@ public PhotonPoseEstimator(AprilTagFieldLayout fieldTags) { * Coordinate System. Note that setting the origin of this layout object will affect the * results from this class. * @param strategy The strategy it should use to determine the best pose. - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot âž” camera) in the Robot - * Coordinate System. - * @deprecated Use individual estimation methods with the 2 argument constructor instead. + * @deprecated Use individual estimation methods with the 1 argument constructor instead. */ @Deprecated(forRemoval = true, since = "2026") - public PhotonPoseEstimator( - AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) { + public PhotonPoseEstimator(AprilTagFieldLayout fieldTags, PoseStrategy strategy) { this.fieldTags = fieldTags; this.primaryStrategy = strategy; 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 44c2af4132..98f4088ab7 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -650,7 +650,7 @@ public PhotonPipelineResult process( 1000L + (long) ((Math.random() - 0.5) * 50), detectableTgts, multitagResult, - cam.getCameraTransform()); + cam.getRobotToCamera()); 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 0aa4d97506..d2714bc2c8 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -123,7 +123,7 @@ static const std::string TYPE_STRING = PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, const std::string_view cameraName, - const std::optional& robotToCamera) + std::optional robotToCamera) : mainTable(instance.GetTable("photonvision")), rootTable(mainTable->GetSubTable(cameraName)), rawBytesEntry( @@ -187,9 +187,19 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, const std::string_view cameraName) : PhotonCamera(instance, cameraName, std::nullopt) {} +PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, + const std::string_view cameraName, + const frc::Transform3d robotToCamera) + : PhotonCamera(instance, cameraName, std::make_optional(robotToCamera)) {} + PhotonCamera::PhotonCamera(const std::string_view cameraName) : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {} +PhotonCamera::PhotonCamera(const std::string_view cameraName, + const frc::Transform3d robotToCamera) + : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName, + std::make_optional(robotToCamera)) {} + PhotonPipelineResult PhotonCamera::GetLatestResult() { if (test) { if (testResult.size()) diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index f759e7f576..e950d0a5cf 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -68,7 +68,6 @@ cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX, PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags) : aprilTags(tags), - m_robotToCamera(), lastPose(frc::Pose3d()), referencePose(frc::Pose3d()), poseCacheTimestamp(-1_s), @@ -79,24 +78,9 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags) } PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, - frc::Transform3d robotToCamera) - : aprilTags(tags), - m_robotToCamera(robotToCamera), - lastPose(frc::Pose3d()), - referencePose(frc::Pose3d()), - poseCacheTimestamp(-1_s), - headingBuffer(frc::TimeInterpolatableBuffer(1_s)) { - HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator, - InstanceCount); - InstanceCount++; -} - -PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, - PoseStrategy strat, - frc::Transform3d robotToCamera) + PoseStrategy strat) : aprilTags(tags), strategy(strat), - m_robotToCamera(robotToCamera), lastPose(frc::Pose3d()), referencePose(frc::Pose3d()), poseCacheTimestamp(-1_s), diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index e79478741b..c8a8fd1c26 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -76,6 +76,17 @@ class PhotonCamera { */ explicit PhotonCamera(const std::string_view cameraName); + /** + * Constructs a PhotonCamera from the name of the camera and a robot to camera + * transform. + * @param cameraName The nickname of the camera (found in the PhotonVision + * UI). + * @param robotToCamera Transform3d from the center of the robot to the camera + * mount positions (ie, robot âž” camera). + */ + explicit PhotonCamera(const std::string_view cameraName, + frc::Transform3d robotToCamera); + /** * Constructs a PhotonCamera from a root table. * @@ -84,12 +95,12 @@ class PhotonCamera { * NTInstance from {@link NetworkTableInstance::getDefault} * @param cameraName The name of the camera, as seen in the UI. * over. - * @param robotToCamera The transform from the robot's center to the camera. - * This is used for pose estimation + * @param robotToCamera Transform3d from the center of the robot to the camera + * mount positions (ie, robot âž” camera). */ explicit PhotonCamera(nt::NetworkTableInstance instance, const std::string_view cameraName, - const std::optional& robotToCamera); + frc::Transform3d robotToCamera); PhotonCamera(PhotonCamera&&) = default; @@ -106,7 +117,8 @@ class PhotonCamera { /** * Sets the robot to camera transform * - * @param newRobotToCamera the robot to camera transform + * @param newRobotToCamera Transform3d from the center of the robot to the + * camera mount positions (ie, robot âž” camera). */ void SetRobotToCamera(frc::Transform3d newRobotToCamera); /** @@ -277,6 +289,12 @@ class PhotonCamera { frc::Alert timesyncAlert; private: + /** + * Internal implementation of the constructor. + */ + explicit PhotonCamera(nt::NetworkTableInstance instance, + const std::string_view cameraName, + std::optional robotToCamera); units::second_t lastVersionCheckTime = 0_s; static bool VERSION_CHECK_ENABLED; inline static int InstanceCount = 1; diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index 7bce702e09..a21a449d1c 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -91,17 +91,6 @@ class PhotonPoseEstimator { */ explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags); - /** - * Create a new PhotonPoseEstimator. - * - * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with - * respect to the FIRST field. - * @param robotToCamera Transform3d from the center of the robot to the camera - * mount positions (ie, robot âž” camera). - * @deprecated robotToCamera is now retrieved from PhotonPipelineResults - */ - explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags, - frc::Transform3d robotToCamera); /** * Create a new PhotonPoseEstimator. @@ -109,8 +98,6 @@ class PhotonPoseEstimator { * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with * respect to the FIRST field. * @param strategy The strategy it should use to determine the best pose. - * @param robotToCamera Transform3d from the center of the robot to the camera - * mount positions (ie, robot âž” camera). * @deprecated Use individual estimation methods with the 1 argument * constructor instead. */ @@ -118,8 +105,7 @@ class PhotonPoseEstimator { "Use individual estimation methods with the 1 argument constructor " "instead.")]] explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags, - PoseStrategy strategy, - frc::Transform3d robotToCamera); + PoseStrategy strategy); /** * Get the AprilTagFieldLayout being used by the PositionEstimator. @@ -189,26 +175,6 @@ class PhotonPoseEstimator { this->referencePose = referencePose; } - /** - * @return The current transform from the center of the robot to the camera - * mount position. - */ - inline frc::Transform3d GetRobotToCameraTransform() { - return m_robotToCamera; - } - - /** - * Useful for pan and tilt mechanisms, or cameras on turrets - * - * @param robotToCamera The current transform from the center of the robot to - * the camera mount position. - * - * @deprecated robotToCamera is now stored in PhotonPipelineResult - */ - inline void SetRobotToCameraTransform(frc::Transform3d robotToCamera) { - m_robotToCamera = robotToCamera; - } - /** * Update the stored last pose. Useful for setting the initial estimate when * using the CLOSEST_TO_LAST_POSE strategy. @@ -429,8 +395,6 @@ class PhotonPoseEstimator { PoseStrategy strategy; PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY; - frc::Transform3d m_robotToCamera; - frc::Pose3d lastPose; frc::Pose3d referencePose; diff --git a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java index d4fca046b7..29f00b95b8 100644 --- a/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java @@ -173,7 +173,7 @@ void testLowestAmbiguityStrategy() { Optional.of(new Transform3d())); PhotonPoseEstimator estimator = - new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, Transform3d.kZero); + new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY); Optional estimatedPose = estimator.update(cameraOne.result); Pose3d pose = estimatedPose.get().estimatedPose; @@ -260,8 +260,7 @@ void testClosestToCameraHeightStrategy() { Optional.of(new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()))); PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, Transform3d.kZero); + new PhotonPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); Optional estimatedPose = estimator.update(cameraOne.result); Pose3d pose = estimatedPose.get().estimatedPose; @@ -348,8 +347,7 @@ void closestToReferencePoseStrategy() { Optional.of(new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()))); PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, new Transform3d()); + new PhotonPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_REFERENCE_POSE); estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d())); Optional estimatedPose = estimator.update(cameraOne.result); @@ -437,10 +435,7 @@ void closestToLastPose() { Optional.of(new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()))); PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, - PoseStrategy.CLOSEST_TO_LAST_POSE, - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())); + new PhotonPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_LAST_POSE); estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d())); @@ -544,12 +539,10 @@ void pnpDistanceTrigSolve() { new Rotation3d( Units.degreesToRadians(37), Units.degreesToRadians(6), Units.degreesToRadians(60))); - cameraOne.setCameraTransform(compoundTestTransform); + cameraOne.setRobotToCamera(compoundTestTransform); try (PhotonCameraSim cameraOneSim = new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())) { - var estimator = - new PhotonPoseEstimator( - aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform); + var estimator = new PhotonPoseEstimator(aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); /* this is the real pose of the robot base we test against */ var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197)); @@ -571,7 +564,7 @@ void pnpDistanceTrigSolve() { Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero); // estimator.setRobotToCameraTransform(straightOnTestTransform); - cameraOne.setCameraTransform(straightOnTestTransform); + cameraOne.setRobotToCamera(straightOnTestTransform); /* Pose to compare with */ realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818)); @@ -621,10 +614,7 @@ void cacheIsInvalidated() { Optional.of(new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()))); PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, - PoseStrategy.AVERAGE_BEST_TARGETS, - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())); + new PhotonPoseEstimator(aprilTags, PoseStrategy.AVERAGE_BEST_TARGETS); // Initial state, expect no timestamp assertEquals(-1, estimator.poseCacheTimestampSeconds); @@ -742,10 +732,7 @@ void averageBestPoses() { new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()))); // 3 3 3 ambig .4 PhotonPoseEstimator estimator = - new PhotonPoseEstimator( - aprilTags, - PoseStrategy.AVERAGE_BEST_TARGETS, - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())); + new PhotonPoseEstimator(aprilTags, PoseStrategy.AVERAGE_BEST_TARGETS); Optional estimatedPose = estimator.update(cameraOne.result); Pose3d pose = estimatedPose.get().estimatedPose; @@ -810,7 +797,7 @@ void testMultiTagOnRioFallback() { Optional.empty(), Optional.of(Transform3d.kZero)); PhotonPoseEstimator estimator = - new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero); + new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO); estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); Optional estimatedPose = estimator.update(cameraOne.result); @@ -900,8 +887,7 @@ public void testConstrainedPnpOneTag() { PhotonPoseEstimator estimator = new PhotonPoseEstimator( AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), - PoseStrategy.CONSTRAINED_SOLVEPNP, - kRobotToCam); + PoseStrategy.CONSTRAINED_SOLVEPNP); estimator.addHeadingData(result.getTimestampSeconds(), Rotation2d.kZero); @@ -920,8 +906,7 @@ void testConstrainedPnpEmptyCase() { PhotonPoseEstimator estimator = new PhotonPoseEstimator( AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), - PoseStrategy.CONSTRAINED_SOLVEPNP, - Transform3d.kZero); + PoseStrategy.CONSTRAINED_SOLVEPNP); PhotonPipelineResult result = new PhotonPipelineResult(); var estimate = estimator.update(result); assertEquals(estimate, Optional.empty()); diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index e525fe19d4..08ce2811d3 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -530,7 +530,7 @@ void pnpDistanceTrigSolve() { 3, new Rotation3d( Units.degreesToRadians(37), Units.degreesToRadians(6), Units.degreesToRadians(60))); - cameraOne.setCameraTransform(compoundTestTransform); + cameraOne.setRobotToCamera(compoundTestTransform); try (PhotonCameraSim cameraOneSim = new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())) { var estimator = new PhotonPoseEstimator(aprilTags); @@ -554,7 +554,7 @@ void pnpDistanceTrigSolve() { /* Straight on */ Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero); - cameraOne.setCameraTransform(straightOnTestTransform); + cameraOne.setRobotToCamera(straightOnTestTransform); /* Pose to compare with */ realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818)); diff --git a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp index eb7968630d..b5fe1f599f 100644 --- a/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp @@ -95,8 +95,7 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) { std::make_optional(frc::Transform3d{})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); - photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, - frc::Transform3d{}); + photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -158,8 +157,8 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { frc::Transform3d{{0_m, 0_m, 4_m}, {}})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); - photon::PhotonPoseEstimator estimator( - aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}}); + photon::PhotonPoseEstimator estimator(aprilTags, + photon::CLOSEST_TO_CAMERA_HEIGHT); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -209,7 +208,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, - photon::CLOSEST_TO_REFERENCE_POSE, {}); + photon::CLOSEST_TO_REFERENCE_POSE); estimator.SetReferencePose( frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad))); @@ -260,8 +259,8 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) { std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); - photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, - {}); + photon::PhotonPoseEstimator estimator(aprilTags, + photon::CLOSEST_TO_LAST_POSE); estimator.SetLastPose( frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad))); @@ -335,8 +334,8 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) { cameraOne.SetRobotToCamera(compoundTestTransform); - photon::PhotonPoseEstimator estimator( - aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform); + photon::PhotonPoseEstimator estimator(aprilTags, + photon::PNP_DISTANCE_TRIG_SOLVE); /* real pose of the robot base to test against */ frc::Pose3d realPose = @@ -428,8 +427,8 @@ TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) { std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); - photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, - {}); + photon::PhotonPoseEstimator estimator(aprilTags, + photon::AVERAGE_BEST_TARGETS); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -474,8 +473,8 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) { cameraOne.test = true; - photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, - {}); + photon::PhotonPoseEstimator estimator(aprilTags, + photon::AVERAGE_BEST_TARGETS); // empty input, expect empty out cameraOne.testResult = {photon::PhotonPipelineResult{ @@ -567,8 +566,7 @@ TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) { std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); - photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, - frc::Transform3d{}); + photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -603,7 +601,7 @@ TEST(LegacyPhotonPoseEstimatorTest, CopyResult) { TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) { photon::PhotonPoseEstimator estimator( frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo), - photon::CONSTRAINED_SOLVEPNP, frc::Transform3d()); + photon::CONSTRAINED_SOLVEPNP); photon::PhotonPipelineResult result; auto estimate = estimator.Update(result); @@ -653,7 +651,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PhotonPoseEstimator estimator( frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo), - photon::CONSTRAINED_SOLVEPNP, kRobotToCam); + photon::CONSTRAINED_SOLVEPNP); estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(), frc::Rotation2d()); diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index de69d58dcc..8cf735e130 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -94,7 +94,7 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); - photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); + photon::PhotonPoseEstimator estimator(aprilTags); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -155,7 +155,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { std::make_optional({{0_m, 0_m, 4_m}, {}})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); - photon::PhotonPoseEstimator estimator(aprilTags, {{0_m, 0_m, 4_m}, {}}); + photon::PhotonPoseEstimator estimator(aprilTags); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -204,7 +204,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); - photon::PhotonPoseEstimator estimator(aprilTags, {}); + photon::PhotonPoseEstimator estimator(aprilTags); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -255,7 +255,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); - photon::PhotonPoseEstimator estimator(aprilTags, {}); + photon::PhotonPoseEstimator estimator(aprilTags); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -326,7 +326,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { frc::Transform3d compoundTestTransform = frc::Transform3d( -12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg)); - photon::PhotonPoseEstimator estimator(aprilTags, compoundTestTransform); + photon::PhotonPoseEstimator estimator(aprilTags); cameraOne.SetRobotToCamera(compoundTestTransform); /* real pose of the robot base to test against */ @@ -334,7 +334,7 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { frc::Pose3d(7.3_m, 4.42_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.197_rad)); photon::PhotonPipelineResult result = cameraOneSim.Process( - 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), + 1_ms, realPose.TransformBy(cameraOne.GetRobotToCamera().value()), targets); cameraOne.testResult = {result}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); @@ -360,12 +360,11 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) { frc::Transform3d straightOnTestTransform = frc::Transform3d(0_m, 0_m, 3_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)); - estimator.SetRobotToCameraTransform(straightOnTestTransform); cameraOne.SetRobotToCamera(straightOnTestTransform); realPose = frc::Pose3d(4.81_m, 2.38_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.818_rad)); result = cameraOneSim.Process( - 1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()), + 1_ms, realPose.TransformBy(cameraOne.GetRobotToCamera().value()), targets); cameraOne.testResult = {result}; cameraOne.testResult[0].SetReceiveTimestamp(18_s); @@ -420,7 +419,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); - photon::PhotonPoseEstimator estimator(aprilTags, {}); + photon::PhotonPoseEstimator estimator(aprilTags); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -462,7 +461,7 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) { std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); - photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{}); + photon::PhotonPoseEstimator estimator(aprilTags); std::optional estimatedPose; for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -539,8 +538,7 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator( - frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo), - kRobotToCam); + frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo)); auto estimatedMultiTagPose = estimator.EstimateCoprocMultiTagPose(cameraOne.testResult[0]); diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java index 4fe409ab69..b235616e81 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java @@ -59,8 +59,8 @@ public class Vision { */ public Vision(EstimateConsumer estConsumer) { this.estConsumer = estConsumer; - camera = new PhotonCamera(kCameraName); - photonEstimator = new PhotonPoseEstimator(kTagLayout, kRobotToCam); + camera = new PhotonCamera(kCameraName, kRobotToCam); + photonEstimator = new PhotonPoseEstimator(kTagLayout); // ----- Simulation if (Robot.isSimulation()) { From b6f70a7664d82a38e97a9cbcc322bc2854246126 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sun, 29 Mar 2026 17:26:36 -0400 Subject: [PATCH 45/62] wpiformat --- photon-lib/py/test/photonPoseEstimator_test.py | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py index 942322e51a..17ca430333 100644 --- a/photon-lib/py/test/photonPoseEstimator_test.py +++ b/photon-lib/py/test/photonPoseEstimator_test.py @@ -134,12 +134,10 @@ def test_lowestAmbiguityStrategy(): ], metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0), multitagResult=None, - robotToCamera=Transform3d() + robotToCamera=Transform3d(), ) - estimator = PhotonPoseEstimator( - aprilTags - ) + estimator = PhotonPoseEstimator(aprilTags) estimatedPose = estimator.estimateLowestAmbiguityPose(cameraOne.result) @@ -177,9 +175,7 @@ def test_pnpDistanceTrigSolve(): ), ) - estimator = PhotonPoseEstimator( - aprilTags - ) + estimator = PhotonPoseEstimator(aprilTags) cameraOne.setRobotToCamera(compoundTestTransform) realPose = Pose3d(7.3, 4.42, 0, Rotation3d(0, 0, 2.197)) # Pose to compare with @@ -265,12 +261,10 @@ def test_multiTagOnCoprocStrategy(): multitagResult=MultiTargetPNPResult( PnpResult(Transform3d(1, 3, 2, Rotation3d())) ), - robotToCamera=Transform3d() + robotToCamera=Transform3d(), ) - estimator = PhotonPoseEstimator( - AprilTagFieldLayout() - ) + estimator = PhotonPoseEstimator(AprilTagFieldLayout()) estimatedPose = estimator.estimateCoprocMultiTagPose(cameraOne.result) assert estimatedPose is not None From 970854af2375a58ac77ac8bcc054c1527644bbef Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Tue, 31 Mar 2026 11:11:52 -0400 Subject: [PATCH 46/62] updated cpp poseest example --- photonlib-cpp-examples/poseest/src/main/include/Vision.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/photonlib-cpp-examples/poseest/src/main/include/Vision.h b/photonlib-cpp-examples/poseest/src/main/include/Vision.h index b3dd50589d..96cc8f07c1 100644 --- a/photonlib-cpp-examples/poseest/src/main/include/Vision.h +++ b/photonlib-cpp-examples/poseest/src/main/include/Vision.h @@ -146,9 +146,9 @@ class Vision { frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } private: - photon::PhotonPoseEstimator photonEstimator{constants::Vision::kTagLayout, - constants::Vision::kRobotToCam}; - photon::PhotonCamera camera{constants::Vision::kCameraName}; + photon::PhotonPoseEstimator photonEstimator{constants::Vision::kTagLayout}; + photon::PhotonCamera camera{constants::Vision::kCameraName, + constants::Vision::kRobotToCam}; std::unique_ptr visionSim; std::unique_ptr cameraProp; std::shared_ptr cameraSim; From f45dd729182f8d02d32d7d454703c5b582996512 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Tue, 31 Mar 2026 12:42:48 -0400 Subject: [PATCH 47/62] privated FileVisionSource getRobotToCamera() --- .../java/org/photonvision/vision/camera/FileVisionSource.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java index 1738e17236..cfc7c675e1 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java @@ -52,11 +52,12 @@ public FileVisionSource(CameraConfiguration cameraConfiguration) { if (getCameraConfiguration().cameraQuirks == null) getCameraConfiguration().cameraQuirks = QuirkyCamera.DefaultCamera; + // The getRobotToCamera method has a null check, so passing it to the FileFrameProvider should be safe even before the settables are initialized settables = new FileSourceSettables(cameraConfiguration, frameProvider.get().frameStaticProperties); } - public Transform3d getRobotToCamera() { + private Transform3d getRobotToCamera() { if (settables == null) { return null; } From dc2af8f654d721863b4ce15888fd2c5b7210738a Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Tue, 31 Mar 2026 12:46:20 -0400 Subject: [PATCH 48/62] linting --- .../java/org/photonvision/vision/camera/FileVisionSource.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java index cfc7c675e1..83606e2174 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java @@ -52,7 +52,8 @@ public FileVisionSource(CameraConfiguration cameraConfiguration) { if (getCameraConfiguration().cameraQuirks == null) getCameraConfiguration().cameraQuirks = QuirkyCamera.DefaultCamera; - // The getRobotToCamera method has a null check, so passing it to the FileFrameProvider should be safe even before the settables are initialized + // The getRobotToCamera method has a null check, so passing it to the FileFrameProvider should + // be safe even before the settables are initialized settables = new FileSourceSettables(cameraConfiguration, frameProvider.get().frameStaticProperties); } From 9a679762a7bca8f37348843dd8e02f531af826bb Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Tue, 31 Mar 2026 16:21:07 -0400 Subject: [PATCH 49/62] refactored backend --- .../networktables/NTDataPublisher.java | 3 +- .../vision/camera/FileVisionSource.java | 13 +-- .../org/photonvision/vision/frame/Frame.java | 19 +---- .../frame/provider/CpuImageProcessor.java | 16 +--- .../frame/provider/FileFrameProvider.java | 23 ++---- .../provider/LibcameraGpuFrameProvider.java | 3 +- .../frame/provider/USBFrameProvider.java | 9 +-- .../vision/pipeline/AprilTagPipeline.java | 16 +++- .../vision/pipeline/ArucoPipeline.java | 16 +++- .../vision/pipeline/CVPipeline.java | 6 +- .../vision/pipeline/Calibrate3dPipeline.java | 2 +- .../vision/pipeline/ColoredShapePipeline.java | 18 ++++- .../vision/pipeline/DriverModePipeline.java | 9 ++- .../vision/pipeline/FocusPipeline.java | 9 ++- .../pipeline/ObjectDetectionPipeline.java | 17 +++- .../vision/pipeline/ReflectivePipeline.java | 18 ++++- .../pipeline/result/CVPipelineResult.java | 79 +++++++++++++++++++ .../vision/processes/PipelineManager.java | 29 +++++-- .../vision/processes/VisionModule.java | 12 ++- .../processes/VisionSourceSettables.java | 12 --- 20 files changed, 214 insertions(+), 115 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 697b0031d6..688cc0b6b0 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -22,7 +22,6 @@ import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTablesJNI; import java.util.List; -import java.util.Optional; import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.Supplier; @@ -206,7 +205,7 @@ public void accept(CVPipelineResult result) { NetworkTablesManager.getInstance().getTimeSinceLastPong(), TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets), acceptedResult.multiTagResult, - Optional.ofNullable(acceptedResult.inputAndOutputFrame.robotToCamera)); + acceptedResult.robotToCamera); // random guess at size of the array ts.resultPublisher.set(simplified, 1024); diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java index 83606e2174..76737fbf20 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java @@ -19,7 +19,6 @@ import edu.wpi.first.cscore.UsbCameraInfo; import edu.wpi.first.cscore.VideoMode; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.util.PixelFormat; import java.nio.file.Path; import java.util.HashMap; @@ -46,25 +45,15 @@ public FileVisionSource(CameraConfiguration cameraConfiguration) { Path.of(cameraConfiguration.getDevicePath()), cameraConfiguration.FOV, FileFrameProvider.MAX_FPS, - calibration, - this::getRobotToCamera); + calibration); if (getCameraConfiguration().cameraQuirks == null) getCameraConfiguration().cameraQuirks = QuirkyCamera.DefaultCamera; - // The getRobotToCamera method has a null check, so passing it to the FileFrameProvider should - // be safe even before the settables are initialized settables = new FileSourceSettables(cameraConfiguration, frameProvider.get().frameStaticProperties); } - private Transform3d getRobotToCamera() { - if (settables == null) { - return null; - } - return settables.getRobotToCamera(); - } - public FileVisionSource(String name, String imagePath, double fov) { // TODO - create new File/replay camera info type super( diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/Frame.java b/photon-core/src/main/java/org/photonvision/vision/frame/Frame.java index 9d99c6acb5..b37c03a8a2 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/Frame.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/Frame.java @@ -17,7 +17,6 @@ package org.photonvision.vision.frame; -import edu.wpi.first.math.geometry.Transform3d; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.math.MathUtils; @@ -37,23 +36,19 @@ public class Frame implements Releasable { public final FrameStaticProperties frameStaticProperties; - public final Transform3d robotToCamera; - public Frame( long sequenceID, CVMat color, CVMat processed, FrameThresholdType type, long timestampNanos, - FrameStaticProperties frameStaticProperties, - Transform3d robotToCamera) { + FrameStaticProperties frameStaticProperties) { this.sequenceID = sequenceID; this.colorImage = color; this.processedImage = processed; this.type = type; this.timestampNanos = timestampNanos; this.frameStaticProperties = frameStaticProperties; - this.robotToCamera = robotToCamera; logger.trace( () -> @@ -71,14 +66,7 @@ public Frame( CVMat processed, FrameThresholdType processType, FrameStaticProperties frameStaticProperties) { - this( - sequenceID, - color, - processed, - processType, - MathUtils.wpiNanoTime(), - frameStaticProperties, - null); + this(sequenceID, color, processed, processType, MathUtils.wpiNanoTime(), frameStaticProperties); } public Frame() { @@ -88,8 +76,7 @@ public Frame() { new CVMat(), FrameThresholdType.NONE, MathUtils.wpiNanoTime(), - new FrameStaticProperties(0, 0, 0, null), - null); + new FrameStaticProperties(0, 0, 0, null)); } public void copyTo(Frame destFrame) { diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java index 453441156a..173aae518a 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/CpuImageProcessor.java @@ -17,7 +17,6 @@ package org.photonvision.vision.frame.provider; -import edu.wpi.first.math.geometry.Transform3d; import org.photonvision.common.util.numbers.IntegerCouple; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameProvider; @@ -34,22 +33,12 @@ protected static class CapturedFrame { CVMat colorImage; FrameStaticProperties staticProps; long captureTimestamp; - Transform3d robotToCamera; public CapturedFrame( - CVMat colorImage, - FrameStaticProperties staticProps, - long captureTimestampNanos, - Transform3d robotToCamera) { + CVMat colorImage, FrameStaticProperties staticProps, long captureTimestampNanos) { this.colorImage = colorImage; this.staticProps = staticProps; this.captureTimestamp = captureTimestampNanos; - this.robotToCamera = robotToCamera; - } - - public CapturedFrame( - CVMat colorImage, FrameStaticProperties staticProps, long captureTimestampNanos) { - this(colorImage, staticProps, captureTimestampNanos, null); } } @@ -104,8 +93,7 @@ public final Frame get() { input.captureTimestamp, input.staticProps != null ? input.staticProps.rotate(m_rImagePipe.getParams().rotation()) - : input.staticProps, - input.robotToCamera); + : input.staticProps); } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java index 6ceab6e3da..ae0b7530d7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/FileFrameProvider.java @@ -17,11 +17,9 @@ package org.photonvision.vision.frame.provider; -import edu.wpi.first.math.geometry.Transform3d; import java.nio.file.Files; import java.nio.file.Path; import java.nio.file.Paths; -import java.util.function.Supplier; import org.opencv.core.Mat; import org.opencv.imgcodecs.Imgcodecs; import org.photonvision.common.util.math.MathUtils; @@ -43,7 +41,6 @@ public class FileFrameProvider extends CpuImageProcessor implements Releasable { private final Path path; private final int millisDelay; private final CVMat originalFrame; - private final Supplier robotToCameraSupplier; private final FrameStaticProperties properties; @@ -57,20 +54,11 @@ public class FileFrameProvider extends CpuImageProcessor implements Releasable { * @param maxFPS The max framerate to provide the image at. */ public FileFrameProvider(Path path, double fov, int maxFPS) { - this(path, fov, maxFPS, null, () -> null); - } - - public FileFrameProvider(Path path, double fov, CameraCalibrationCoefficients calibration) { - this(path, fov, MAX_FPS, calibration, () -> null); + this(path, fov, maxFPS, null); } public FileFrameProvider( - Path path, - double fov, - int maxFPS, - CameraCalibrationCoefficients calibration, - Supplier robotToCameraSupplier) { - this.robotToCameraSupplier = robotToCameraSupplier; + Path path, double fov, int maxFPS, CameraCalibrationCoefficients calibration) { if (!Files.exists(path)) throw new RuntimeException("Invalid path for image: " + path.toAbsolutePath()); this.path = path; @@ -105,6 +93,10 @@ public FileFrameProvider(Path path, double fov) { this(path, fov, MAX_FPS); } + public FileFrameProvider(Path path, double fov, CameraCalibrationCoefficients calibration) { + this(path, fov, MAX_FPS, calibration); + } + @Override public CapturedFrame getInputMat() { var out = new CVMat(); @@ -122,8 +114,7 @@ public CapturedFrame getInputMat() { } lastGetMillis = System.currentTimeMillis(); - return new CapturedFrame( - out, properties, MathUtils.wpiNanoTime(), this.robotToCameraSupplier.get()); + return new CapturedFrame(out, properties, MathUtils.wpiNanoTime()); } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java index 21938f0ff6..e805602fe0 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/LibcameraGpuFrameProvider.java @@ -103,8 +103,7 @@ public Frame get() { processedMat, type, MathUtils.wpiNanoTime() - latency, - settables.getFrameStaticProperties().rotate(settables.getRotation()), - settables.getRobotToCamera()); + settables.getFrameStaticProperties().rotate(settables.getRotation())); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java index 324fd776f0..85d461ca18 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java @@ -91,8 +91,7 @@ public CapturedFrame getInputMat() { logger.error("Error grabbing image: " + error); } - return new CapturedFrame( - mat, settables.getFrameStaticProperties(), captureTimeNs, settables.getRobotToCamera()); + return new CapturedFrame(mat, settables.getFrameStaticProperties(), captureTimeNs); } else { // We allocate memory so we don't fill a Mat in use by another thread (memory model is easier) // TODO - consider a frame pool @@ -128,11 +127,7 @@ public CapturedFrame getInputMat() { ret = new CVMat(mat, frame); } - return new CapturedFrame( - ret, - settables.getFrameStaticProperties(), - captureTimeUs * 1000, - settables.getRobotToCamera()); + return new CapturedFrame(ret, settables.getFrameStaticProperties(), captureTimeUs * 1000); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index 941e16cfaa..60f970d553 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -29,6 +29,7 @@ import java.util.ArrayList; import java.util.List; import java.util.Optional; +import java.util.function.Supplier; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.logging.LogGroup; @@ -63,12 +64,13 @@ public class AprilTagPipeline extends CVPipeline null); settings = new AprilTagPipelineSettings(); } - public AprilTagPipeline(AprilTagPipelineSettings settings) { - super(PROCESSING_TYPE); + public AprilTagPipeline( + AprilTagPipelineSettings settings, Supplier robotToCameraSupplier) { + super(PROCESSING_TYPE, robotToCameraSupplier); this.settings = settings; } @@ -247,7 +249,13 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting var fps = fpsResult.output; return new CVPipelineResult( - frame.sequenceID, sumPipeNanosElapsed, fps, targetList, multiTagResult, frame); + frame.sequenceID, + sumPipeNanosElapsed, + fps, + targetList, + multiTagResult, + frame, + Optional.ofNullable(robotToCameraSupplier.get())); } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java index 49d5068252..d8863b894e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java @@ -25,6 +25,7 @@ import java.util.ArrayList; import java.util.List; import java.util.Optional; +import java.util.function.Supplier; import org.opencv.core.Mat; import org.opencv.imgproc.Imgproc; import org.opencv.objdetect.Objdetect; @@ -55,12 +56,13 @@ public class ArucoPipeline extends CVPipeline null); settings = new ArucoPipelineSettings(); } - public ArucoPipeline(ArucoPipelineSettings settings) { - super(FrameThresholdType.GREYSCALE); + public ArucoPipeline( + ArucoPipelineSettings settings, Supplier robotToCameraSupplier) { + super(FrameThresholdType.GREYSCALE, robotToCameraSupplier); this.settings = settings; } @@ -235,7 +237,13 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings) var fps = fpsResult.output; return new CVPipelineResult( - frame.sequenceID, sumPipeNanosElapsed, fps, targetList, multiTagResult, frame); + frame.sequenceID, + sumPipeNanosElapsed, + fps, + targetList, + multiTagResult, + frame, + Optional.ofNullable(robotToCameraSupplier.get())); } private void drawThresholdFrame(Mat greyMat, Mat outputMat, int windowSize, double constant) { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java index 1d5167909a..e368f97245 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java @@ -17,6 +17,8 @@ package org.photonvision.vision.pipeline; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.function.Supplier; import org.photonvision.vision.camera.QuirkyCamera; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameStaticProperties; @@ -29,14 +31,16 @@ public abstract class CVPipeline robotToCameraSupplier; private final FrameThresholdType thresholdType; // So releaseable doesn't keep track of if we double-free something. so (ew) remember that here protected volatile boolean released = false; - public CVPipeline(FrameThresholdType thresholdType) { + public CVPipeline(FrameThresholdType thresholdType, Supplier robotToCameraSupplier) { this.thresholdType = thresholdType; + this.robotToCameraSupplier = robotToCameraSupplier; } public FrameThresholdType getThresholdType() { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java index c1b03a2a21..023ceac7ce 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java @@ -74,7 +74,7 @@ public Calibrate3dPipeline() { } public Calibrate3dPipeline(int minSnapshots) { - super(PROCESSING_TYPE); + super(PROCESSING_TYPE, () -> null); this.settings = new Calibration3dPipelineSettings(); this.foundCornersList = new ArrayList<>(); this.minSnapshots = minSnapshots; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java index 0443e04576..46c47fdcbc 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java @@ -18,8 +18,11 @@ package org.photonvision.vision.pipeline; import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Transform3d; import java.util.Arrays; import java.util.List; +import java.util.Optional; +import java.util.function.Supplier; import org.opencv.core.Point; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -54,12 +57,13 @@ public class ColoredShapePipeline private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.HSV; public ColoredShapePipeline() { - super(PROCESSING_TYPE); + super(PROCESSING_TYPE, () -> null); settings = new ColoredShapePipelineSettings(); } - public ColoredShapePipeline(ColoredShapePipelineSettings settings) { - super(PROCESSING_TYPE); + public ColoredShapePipeline( + ColoredShapePipelineSettings settings, Supplier robotToCameraSupplier) { + super(PROCESSING_TYPE, robotToCameraSupplier); this.settings = settings; } @@ -213,6 +217,12 @@ protected CVPipelineResult process(Frame frame, ColoredShapePipelineSettings set var fpsResult = calculateFPSPipe.run(null); var fps = fpsResult.output; - return new CVPipelineResult(frame.sequenceID, sumPipeNanosElapsed, fps, targetList, frame); + return new CVPipelineResult( + frame.sequenceID, + sumPipeNanosElapsed, + fps, + targetList, + frame, + Optional.ofNullable(robotToCameraSupplier.get())); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java index a8c52efc85..793c157b44 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java @@ -18,7 +18,9 @@ package org.photonvision.vision.pipeline; import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Transform3d; import java.util.List; +import java.util.function.Supplier; import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -36,12 +38,13 @@ public class DriverModePipeline private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE; public DriverModePipeline() { - super(PROCESSING_TYPE); + super(PROCESSING_TYPE, () -> null); settings = new DriverModePipelineSettings(); } - public DriverModePipeline(DriverModePipelineSettings settings) { - super(PROCESSING_TYPE); + public DriverModePipeline( + DriverModePipelineSettings settings, Supplier robotToCameraSupplier) { + super(PROCESSING_TYPE, robotToCameraSupplier); this.settings = settings; } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/FocusPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/FocusPipeline.java index 83ad1107af..1a14a91324 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/FocusPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/FocusPipeline.java @@ -17,6 +17,8 @@ package org.photonvision.vision.pipeline; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.function.Supplier; import org.opencv.core.Mat; import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.frame.Frame; @@ -35,12 +37,13 @@ public class FocusPipeline extends CVPipeline null); settings = new FocusPipelineSettings(); } - public FocusPipeline(FocusPipelineSettings settings) { - super(PROCESSING_TYPE); + public FocusPipeline( + FocusPipelineSettings settings, Supplier robotToCameraSupplier) { + super(PROCESSING_TYPE, robotToCameraSupplier); this.settings = settings; } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ObjectDetectionPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ObjectDetectionPipeline.java index a009d2cc9e..b2d6750c11 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ObjectDetectionPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ObjectDetectionPipeline.java @@ -17,8 +17,10 @@ package org.photonvision.vision.pipeline; +import edu.wpi.first.math.geometry.Transform3d; import java.util.List; import java.util.Optional; +import java.util.function.Supplier; import org.photonvision.common.configuration.NeuralNetworkModelManager; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -44,12 +46,13 @@ public class ObjectDetectionPipeline private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE; public ObjectDetectionPipeline() { - super(PROCESSING_TYPE); + super(PROCESSING_TYPE, () -> null); settings = new ObjectDetectionPipelineSettings(); } - public ObjectDetectionPipeline(ObjectDetectionPipelineSettings settings) { - super(PROCESSING_TYPE); + public ObjectDetectionPipeline( + ObjectDetectionPipelineSettings settings, Supplier robotToCameraSupplier) { + super(PROCESSING_TYPE, robotToCameraSupplier); this.settings = settings; } @@ -129,7 +132,13 @@ protected CVPipelineResult process(Frame frame, ObjectDetectionPipelineSettings var fps = fpsResult.output; return new CVPipelineResult( - frame.sequenceID, sumPipeNanosElapsed, fps, collect2dTargetsResult.output, frame, names); + frame.sequenceID, + sumPipeNanosElapsed, + fps, + collect2dTargetsResult.output, + frame, + names, + Optional.ofNullable(robotToCameraSupplier.get())); } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java index 0d78d978fb..90bca51b2e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java @@ -17,7 +17,9 @@ package org.photonvision.vision.pipeline; +import edu.wpi.first.math.geometry.Transform3d; import java.util.List; +import java.util.Optional; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; import org.photonvision.vision.opencv.Contour; @@ -46,12 +48,14 @@ public class ReflectivePipeline extends CVPipeline null); settings = new ReflectivePipelineSettings(); } - public ReflectivePipeline(ReflectivePipelineSettings settings) { - super(PROCESSING_TYPE); + public ReflectivePipeline( + ReflectivePipelineSettings settings, + java.util.function.Supplier robotToCameraSupplier) { + super(PROCESSING_TYPE, robotToCameraSupplier); this.settings = settings; } @@ -159,6 +163,12 @@ public CVPipelineResult process(Frame frame, ReflectivePipelineSettings settings PipelineProfiler.printReflectiveProfile(pipeProfileNanos); - return new CVPipelineResult(frame.sequenceID, sumPipeNanosElapsed, fps, targetList, frame); + return new CVPipelineResult( + frame.sequenceID, + sumPipeNanosElapsed, + fps, + targetList, + frame, + Optional.ofNullable(robotToCameraSupplier.get())); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java index 51773c9f16..2d83003fc1 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java @@ -17,6 +17,7 @@ package org.photonvision.vision.pipeline.result; +import edu.wpi.first.math.geometry.Transform3d; import java.util.Collections; import java.util.List; import java.util.Optional; @@ -35,6 +36,7 @@ public class CVPipelineResult implements Releasable { public final Frame inputAndOutputFrame; public Optional multiTagResult; public final List objectDetectionClassNames; + public Optional robotToCamera; public CVPipelineResult( long sequenceID, @@ -45,6 +47,24 @@ public CVPipelineResult( this(sequenceID, processingNanos, fps, targets, Optional.empty(), inputFrame, List.of()); } + public CVPipelineResult( + long sequenceID, + double processingNanos, + double fps, + List targets, + Frame inputFrame, + Optional robotToCamera) { + this( + sequenceID, + processingNanos, + fps, + targets, + Optional.empty(), + inputFrame, + List.of(), + robotToCamera); + } + public CVPipelineResult( long sequenceID, double processingNanos, @@ -55,6 +75,25 @@ public CVPipelineResult( this(sequenceID, processingNanos, fps, targets, Optional.empty(), inputFrame, classNames); } + public CVPipelineResult( + long sequenceID, + double processingNanos, + double fps, + List targets, + Frame inputFrame, + List classNames, + Optional robotToCamera) { + this( + sequenceID, + processingNanos, + fps, + targets, + Optional.empty(), + inputFrame, + classNames, + robotToCamera); + } + public CVPipelineResult( long sequenceID, double processingNanos, @@ -73,12 +112,52 @@ public CVPipelineResult( Optional multiTagResult, Frame inputFrame, List classNames) { + this( + sequenceID, + processingNanos, + fps, + targets, + multiTagResult, + inputFrame, + classNames, + Optional.empty()); + } + + public CVPipelineResult( + long sequenceID, + double processingNanos, + double fps, + List targets, + Optional multiTagResult, + Frame inputFrame, + Optional robotToCamera) { + this( + sequenceID, + processingNanos, + fps, + targets, + multiTagResult, + inputFrame, + List.of(), + robotToCamera); + } + + public CVPipelineResult( + long sequenceID, + double processingNanos, + double fps, + List targets, + Optional multiTagResult, + Frame inputFrame, + List classNames, + Optional robotToCamera) { this.sequenceID = sequenceID; this.processingNanos = processingNanos; this.fps = fps; this.targets = targets != null ? targets : Collections.emptyList(); this.multiTagResult = multiTagResult; this.objectDetectionClassNames = classNames; + this.robotToCamera = robotToCamera; this.inputAndOutputFrame = inputFrame; } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java index 02ecc509a2..089a00585c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java @@ -17,11 +17,13 @@ package org.photonvision.vision.processes; +import edu.wpi.first.math.geometry.Transform3d; import java.lang.reflect.Field; import java.util.ArrayList; import java.util.Arrays; import java.util.Comparator; import java.util.List; +import java.util.concurrent.atomic.AtomicReference; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.DataChangeService; @@ -50,6 +52,9 @@ public class PipelineManager { /** The currently active pipeline. */ private CVPipeline currentUserPipeline = driverModePipeline; + /** The current robot to camera transform. */ + private AtomicReference robotToCamera = new AtomicReference<>(); + /** * Index of the last active user-created pipeline.
*
@@ -182,6 +187,14 @@ public int getRequestedIndex() { return requestedIndex; } + public Transform3d getRobotToCamera() { + return robotToCamera.get(); + } + + public void setRobotToCamera(Transform3d newTransform) { + robotToCamera.set(newTransform); + } + /** * Internal method for setting the active pipeline.
*
@@ -245,26 +258,32 @@ private void recreateUserPipeline() { case Reflective -> { logger.debug("Creating Reflective pipeline"); currentUserPipeline = - new ReflectivePipeline((ReflectivePipelineSettings) desiredPipelineSettings); + new ReflectivePipeline( + (ReflectivePipelineSettings) desiredPipelineSettings, this::getRobotToCamera); } case ColoredShape -> { logger.debug("Creating ColoredShape pipeline"); currentUserPipeline = - new ColoredShapePipeline((ColoredShapePipelineSettings) desiredPipelineSettings); + new ColoredShapePipeline( + (ColoredShapePipelineSettings) desiredPipelineSettings, this::getRobotToCamera); } case AprilTag -> { logger.debug("Creating AprilTag pipeline"); currentUserPipeline = - new AprilTagPipeline((AprilTagPipelineSettings) desiredPipelineSettings); + new AprilTagPipeline( + (AprilTagPipelineSettings) desiredPipelineSettings, this::getRobotToCamera); } case Aruco -> { logger.debug("Creating ArUco Pipeline"); - currentUserPipeline = new ArucoPipeline((ArucoPipelineSettings) desiredPipelineSettings); + currentUserPipeline = + new ArucoPipeline( + (ArucoPipelineSettings) desiredPipelineSettings, this::getRobotToCamera); } case ObjectDetection -> { logger.debug("Creating ObjectDetection Pipeline"); currentUserPipeline = - new ObjectDetectionPipeline((ObjectDetectionPipelineSettings) desiredPipelineSettings); + new ObjectDetectionPipeline( + (ObjectDetectionPipelineSettings) desiredPipelineSettings, this::getRobotToCamera); } case Calib3d, DriverMode, FocusCamera -> {} } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index 97edecf0f7..bf1d57a640 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -644,7 +644,17 @@ public void setFPSLimit(int fps) { * the robot's coordinate system. This should be provided in meters. */ public void setRobotToCameraTransform(Transform3d robotToCameraTransform) { - this.visionSource.getSettables().setRobotToCamera(robotToCameraTransform); + this.pipelineManager.setRobotToCamera(robotToCameraTransform); + } + + /** + * Get robot to camera transform for this vision module. + * + * @return the transform from the robot's origin to the camera's origin, in the robot's coordinate + * system, in meters. May return null if no transform is set. + */ + public Transform3d getRobotToCameraTransform() { + return this.pipelineManager.getRobotToCamera(); } /** diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index 1bee69c526..331ea10b8b 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java @@ -18,9 +18,7 @@ package org.photonvision.vision.processes; import edu.wpi.first.cscore.VideoMode; -import edu.wpi.first.math.geometry.Transform3d; import java.util.HashMap; -import java.util.concurrent.atomic.AtomicReference; import org.opencv.core.Size; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.logging.LogGroup; @@ -33,8 +31,6 @@ public abstract class VisionSourceSettables { private final CameraConfiguration configuration; - private final AtomicReference robotToCamera = new AtomicReference<>(); - protected VisionSourceSettables(CameraConfiguration configuration) { this.configuration = configuration; this.logger = @@ -130,14 +126,6 @@ public void removeCalibration(Size unrotatedImageSize) { calculateFrameStaticProps(); } - public void setRobotToCamera(Transform3d robotToCamera) { - this.robotToCamera.set(robotToCamera); - } - - public Transform3d getRobotToCamera() { - return this.robotToCamera.get(); - } - protected void calculateFrameStaticProps() { var videoMode = getCurrentVideoMode(); this.frameStaticProperties = From c9ec8bb5efa669e475eef8be1c8933d89fff13c4 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 9 Apr 2026 15:19:39 -0400 Subject: [PATCH 50/62] fixed python photonPoseEstimator test --- photon-lib/py/test/photonPoseEstimator_test.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py index 17ca430333..8e88a5711f 100644 --- a/photon-lib/py/test/photonPoseEstimator_test.py +++ b/photon-lib/py/test/photonPoseEstimator_test.py @@ -180,7 +180,7 @@ def test_pnpDistanceTrigSolve(): realPose = Pose3d(7.3, 4.42, 0, Rotation3d(0, 0, 2.197)) # Pose to compare with result = cameraOneSim.process( - latencySecs, realPose.transformBy(estimator.robotToCamera), simTargets + latencySecs, realPose.transformBy(cameraOne.getRobotToCamera()), simTargets ) bestTarget = result.getBestTarget() assert bestTarget is not None @@ -206,7 +206,7 @@ def test_pnpDistanceTrigSolve(): cameraOne.setRobotToCamera(straightOnTestTransform) realPose = Pose3d(4.81, 2.38, 0, Rotation3d(0, 0, 2.818)) # Pose to compare with result = cameraOneSim.process( - latencySecs, realPose.transformBy(estimator.robotToCamera), simTargets + latencySecs, realPose.transformBy(cameraOne.getRobotToCamera()), simTargets ) bestTarget = result.getBestTarget() assert bestTarget is not None From d020a5ab4237e959cad89b4a8a2efa32670802dd Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Tue, 21 Apr 2026 20:11:53 -0400 Subject: [PATCH 51/62] worked on rebase --- .../vision/pipeline/CVPipeline.java | 2 +- .../vision/pipeline/ColoredShapePipeline.java | 3 +- .../vision/pipeline/DriverModePipeline.java | 3 +- .../vision/pipeline/FocusPipeline.java | 2 +- .../pipeline/ObjectDetectionPipeline.java | 2 +- .../vision/pipeline/ReflectivePipeline.java | 2 +- .../pipeline/result/CVPipelineResult.java | 2 +- .../vision/processes/PipelineManager.java | 2 +- .../vision/processes/VisionModule.java | 5 +- .../java/org/photonvision/PhotonCamera.java | 2 + .../org/photonvision/PhotonPoseEstimator.java | 247 ------------------ .../main/native/include/photon/PhotonCamera.h | 2 +- .../struct/MultiTargetPNPResultSerde.java | 1 + .../struct/PhotonPipelineMetadataSerde.java | 1 + .../struct/PhotonPipelineResultSerde.java | 5 +- .../struct/TargetCornerSerde.java | 1 + .../serde/PhotonPipelineResultSerde.cpp | 4 +- .../photon/serde/PhotonPipelineResultSerde.h | 2 +- .../photon/serde/PhotonTrackedTargetSerde.h | 2 +- .../include/photon/serde/PnpResultSerde.h | 2 +- .../struct/PhotonPipelineResultStruct.h | 4 +- .../photon/struct/PhotonTrackedTargetStruct.h | 2 +- .../include/photon/struct/PnpResultStruct.h | 2 +- .../targeting/PhotonPipelineResult.java | 2 +- .../proto/PhotonPipelineResultProto.java | 2 +- .../photon/dataflow/structures/Packet.h | 2 +- 26 files changed, 30 insertions(+), 276 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java index e368f97245..3156493604 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.math.geometry.Transform3d; import java.util.function.Supplier; import org.photonvision.vision.camera.QuirkyCamera; import org.photonvision.vision.frame.Frame; @@ -25,6 +24,7 @@ import org.photonvision.vision.frame.FrameThresholdType; import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.pipeline.result.CVPipelineResult; +import org.wpilib.math.geometry.Transform3d; public abstract class CVPipeline implements Releasable { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java index f87bc5e090..ece3819b29 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java @@ -17,8 +17,6 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Transform3d; import java.util.Arrays; import java.util.List; import java.util.Optional; @@ -35,6 +33,7 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.PotentialTarget; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.geometry.Transform3d; import org.wpilib.math.util.Pair; public class ColoredShapePipeline diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java index 66940875aa..034bad6d8f 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java @@ -17,8 +17,6 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Transform3d; import java.util.List; import java.util.function.Supplier; import org.photonvision.common.util.math.MathUtils; @@ -28,6 +26,7 @@ import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe; import org.photonvision.vision.pipe.impl.ResizeImagePipe; import org.photonvision.vision.pipeline.result.DriverModePipelineResult; +import org.wpilib.math.geometry.Transform3d; import org.wpilib.math.util.Pair; public class DriverModePipeline diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/FocusPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/FocusPipeline.java index 1a14a91324..342b5b6e71 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/FocusPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/FocusPipeline.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.math.geometry.Transform3d; import java.util.function.Supplier; import org.opencv.core.Mat; import org.photonvision.common.util.math.MathUtils; @@ -28,6 +27,7 @@ import org.photonvision.vision.pipe.impl.FocusPipe; import org.photonvision.vision.pipe.impl.ResizeImagePipe; import org.photonvision.vision.pipeline.result.FocusPipelineResult; +import org.wpilib.math.geometry.Transform3d; public class FocusPipeline extends CVPipeline { private final FocusPipe focusPipe = new FocusPipe(); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ObjectDetectionPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ObjectDetectionPipeline.java index b2d6750c11..a70b918535 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ObjectDetectionPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ObjectDetectionPipeline.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.math.geometry.Transform3d; import java.util.List; import java.util.Optional; import java.util.function.Supplier; @@ -34,6 +33,7 @@ import org.photonvision.vision.target.PotentialTarget; import org.photonvision.vision.target.TargetOrientation; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.geometry.Transform3d; public class ObjectDetectionPipeline extends CVPipeline { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java index 90bca51b2e..a213c0a844 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipeline; -import edu.wpi.first.math.geometry.Transform3d; import java.util.List; import java.util.Optional; import org.photonvision.vision.frame.Frame; @@ -30,6 +29,7 @@ import org.photonvision.vision.target.PotentialTarget; import org.photonvision.vision.target.TargetOrientation; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.geometry.Transform3d; /** Represents a pipeline for tracking retro-reflective targets. */ public class ReflectivePipeline extends CVPipeline { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java index 9c25aca490..4c308f14aa 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java @@ -17,7 +17,6 @@ package org.photonvision.vision.pipeline.result; -import edu.wpi.first.math.geometry.Transform3d; import java.util.Collections; import java.util.List; import java.util.Optional; @@ -26,6 +25,7 @@ import org.photonvision.vision.frame.Frame; import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.geometry.Transform3d; public class CVPipelineResult implements Releasable { public final long sequenceID; diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java index 089a00585c..68215f4ef6 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java @@ -17,7 +17,6 @@ package org.photonvision.vision.processes; -import edu.wpi.first.math.geometry.Transform3d; import java.lang.reflect.Field; import java.util.ArrayList; import java.util.Arrays; @@ -32,6 +31,7 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.vision.pipeline.*; +import org.wpilib.math.geometry.Transform3d; @SuppressWarnings({"rawtypes", "unused"}) public class PipelineManager { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index d6bc89c562..e9998cef4e 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -17,10 +17,6 @@ package org.photonvision.vision.processes; -import edu.wpi.first.cscore.CameraServerJNI; -import edu.wpi.first.cscore.VideoException; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.util.Units; import io.javalin.websocket.WsContext; import java.util.ArrayList; import java.util.HashMap; @@ -58,6 +54,7 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TargetModel; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.geometry.Transform3d; import org.wpilib.math.util.Units; import org.wpilib.vision.camera.CameraServerJNI; import org.wpilib.vision.camera.VideoException; diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 27ce8c5e4c..7976f17c41 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -36,6 +36,7 @@ import org.wpilib.driverstation.Alert; import org.wpilib.driverstation.DriverStation; import org.wpilib.hardware.hal.HAL; +import org.wpilib.math.geometry.Transform3d; import org.wpilib.math.linalg.MatBuilder; import org.wpilib.math.linalg.Matrix; import org.wpilib.math.numbers.*; @@ -51,6 +52,7 @@ import org.wpilib.networktables.NetworkTableInstance; import org.wpilib.networktables.PubSubOption; import org.wpilib.networktables.StringSubscriber; +import org.wpilib.networktables.StructPublisher; import org.wpilib.system.Timer; /** Represents a camera that is connected to PhotonVision. */ diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index b7a084b373..aac6d0f70c 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -247,253 +247,6 @@ public void setRobotToCameraTransform(Transform3d robotToCamera) { this.robotToCamera = robotToCamera; } - /** - * Updates the estimated position of the robot, assuming no camera calibration is required for the - * selected strategy. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * Will report a warning if strategy is multi-tag-on-rio because camera calibration data is not - * provided in this overload. - * - * @param cameraResult The latest pipeline result from the camera - * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public Optional update(PhotonPipelineResult cameraResult) { - return update(cameraResult, Optional.empty(), Optional.empty()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
  • The strategy is CONSTRAINED_SOLVEPNP, but no constrainedPnpParams were provided (use the - * other function overload). - *
- * - * @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty - * otherwise - * @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty - * otherwise - * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs) { - return update(cameraResult, cameraMatrix, distCoeffs, Optional.empty()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
  • The strategy is CONSTRAINED_SOLVEPNP, but the provided constrainedPnpParams are empty. - *
- * - * @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty - * otherwise - * @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty - * otherwise - * @param constrainedPnpParams Constrained SolvePNP params, if needed. - * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - * @deprecated Use individual estimation methods instead. - */ - @Deprecated(forRemoval = true, since = "2026") - public Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs, - Optional constrainedPnpParams) { - // Time in the past -- give up, since the following if expects times > 0 - if (cameraResult.getTimestampSeconds() < 0) { - return Optional.empty(); - } - - // If the pose cache timestamp was set, and the result is from the same - // timestamp, return an - // empty result - if (poseCacheTimestampSeconds > 0 - && Math.abs(poseCacheTimestampSeconds - cameraResult.getTimestampSeconds()) < 1e-6) { - return Optional.empty(); - } - - // Remember the timestamp of the current result used - poseCacheTimestampSeconds = cameraResult.getTimestampSeconds(); - - // If no targets seen, trivial case -- return empty result - if (!cameraResult.hasTargets()) { - return Optional.empty(); - } - - return update( - cameraResult, cameraMatrix, distCoeffs, constrainedPnpParams, this.primaryStrategy); - } - - /** - * Internal convenience method for using a fallback strategy for update(). This should only be - * called after timestamp checks have been done by another update() overload. - * - * @param cameraResult The latest pipeline result from the camera - * @param strategy The pose strategy to use. Can't be CONSTRAINED_SOLVEPNP. - * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - private Optional update( - PhotonPipelineResult cameraResult, PoseStrategy strategy) { - return update(cameraResult, Optional.empty(), Optional.empty(), Optional.empty(), strategy); - } - - private Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs, - Optional constrainedPnpParams, - PoseStrategy strategy) { - Optional estimatedPose = - switch (strategy) { - case LOWEST_AMBIGUITY -> estimateLowestAmbiguityPose(cameraResult); - case CLOSEST_TO_CAMERA_HEIGHT -> estimateClosestToCameraHeightPose(cameraResult); - case CLOSEST_TO_REFERENCE_POSE -> - estimateClosestToReferencePose(cameraResult, referencePose); - case CLOSEST_TO_LAST_POSE -> { - setReferencePose(lastPose); - yield estimateClosestToReferencePose(cameraResult, referencePose); - } - case AVERAGE_BEST_TARGETS -> estimateAverageBestTargetsPose(cameraResult); - case MULTI_TAG_PNP_ON_RIO -> { - if (cameraMatrix.isEmpty() || distCoeffs.isEmpty()) { - DriverStation.reportWarning( - "No camera calibration data provided for multi-tag-on-rio", - Thread.currentThread().getStackTrace()); - yield update(cameraResult, this.multiTagFallbackStrategy); - } - var res = estimateRioMultiTagPose(cameraResult, cameraMatrix.get(), distCoeffs.get()); - if (res.isEmpty()) { - yield update( - cameraResult, - cameraMatrix, - distCoeffs, - constrainedPnpParams, - this.multiTagFallbackStrategy); - } - yield res; - } - case MULTI_TAG_PNP_ON_COPROCESSOR -> { - if (cameraResult.getMultiTagResult().isEmpty()) { - yield update(cameraResult, this.multiTagFallbackStrategy); - } - yield estimateCoprocMultiTagPose(cameraResult); - } - case PNP_DISTANCE_TRIG_SOLVE -> estimatePnpDistanceTrigSolvePose(cameraResult); - case CONSTRAINED_SOLVEPNP -> { - boolean hasCalibData = cameraMatrix.isPresent() && distCoeffs.isPresent(); - // cannot run multitagPNP, use fallback strategy - if (!hasCalibData) { - yield update( - cameraResult, - cameraMatrix, - distCoeffs, - Optional.empty(), - this.multiTagFallbackStrategy); - } - - if (constrainedPnpParams.isEmpty()) { - yield Optional.empty(); - } - - // Need heading if heading fixed - if (!constrainedPnpParams.get().headingFree - && headingBuffer.getSample(cameraResult.getTimestampSeconds()).isEmpty()) { - yield update( - cameraResult, - cameraMatrix, - distCoeffs, - Optional.empty(), - this.multiTagFallbackStrategy); - } - - Pose3d fieldToRobotSeed; - // Attempt to use multi-tag to get a pose estimate seed - if (cameraResult.getMultiTagResult().isPresent()) { - fieldToRobotSeed = - Pose3d.kZero.plus( - cameraResult - .getMultiTagResult() - .get() - .estimatedPose - .best - .plus(robotToCamera.inverse())); - } else { - // HACK - use fallback strategy to gimme a seed pose - // TODO - make sure nested update doesn't break state - var nestedUpdate = - update( - cameraResult, - cameraMatrix, - distCoeffs, - Optional.empty(), - this.multiTagFallbackStrategy); - if (nestedUpdate.isEmpty()) { - // best i can do is bail - yield Optional.empty(); - } - fieldToRobotSeed = nestedUpdate.get().estimatedPose; - } - - if (!constrainedPnpParams.get().headingFree) { - // If heading fixed, force rotation component - fieldToRobotSeed = - new Pose3d( - fieldToRobotSeed.getTranslation(), - new Rotation3d( - headingBuffer.getSample(cameraResult.getTimestampSeconds()).get())); - } - - var pnpResult = - estimateConstrainedSolvepnpPose( - cameraResult, - cameraMatrix.get(), - distCoeffs.get(), - fieldToRobotSeed, - constrainedPnpParams.get().headingFree, - constrainedPnpParams.get().headingScaleFactor); - if (!pnpResult.isPresent()) { - yield update( - cameraResult, - cameraMatrix, - distCoeffs, - Optional.empty(), - this.multiTagFallbackStrategy); - } - yield pnpResult; - } - }; - - if (estimatedPose.isPresent()) { - lastPose = estimatedPose.get().estimatedPose; - } - - return estimatedPose; - } - /** * @param cameraResult A pipeline result from the camera. * @return Whether or not pose estimation should be performed. diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index fee34baf26..d8437a4ad2 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -29,7 +29,7 @@ #include #include -#include +#include #include #include #include diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java index 4fc09a1539..c30411cc73 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java @@ -28,6 +28,7 @@ import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; +import org.photonvision.utils.PacketUtils; // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java index 8be1c3fab5..7b1723f57a 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java @@ -28,6 +28,7 @@ import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; +import org.photonvision.utils.PacketUtils; // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java index 688011477a..1dc2528618 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java @@ -28,6 +28,7 @@ import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; +import org.photonvision.utils.PacketUtils; // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; @@ -38,7 +39,7 @@ // WPILib imports (if any) import org.wpilib.util.struct.Struct; -import edu.wpi.first.math.geometry.Transform3d; +import org.wpilib.math.geometry.Transform3d; /** * Auto-generated serialization/deserialization helper for PhotonPipelineResult @@ -98,7 +99,7 @@ public PhotonPipelineResult unpack(Packet packet) { @Override public PacketSerde[] getNestedPhotonMessages() { return new PacketSerde[] { - PhotonTrackedTarget.photonStruct,MultiTargetPNPResult.photonStruct,PhotonPipelineMetadata.photonStruct + PhotonTrackedTarget.photonStruct,PhotonPipelineMetadata.photonStruct,MultiTargetPNPResult.photonStruct }; } diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java index b4e4d668ba..b0f5865174 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java @@ -28,6 +28,7 @@ import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; +import org.photonvision.utils.PacketUtils; // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; diff --git a/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineResultSerde.cpp b/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineResultSerde.cpp index 14540b6e18..17cbb7c634 100644 --- a/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineResultSerde.cpp +++ b/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineResultSerde.cpp @@ -34,7 +34,7 @@ void StructType::Pack(Packet& packet, const PhotonPipelineResult& value) { packet.Pack(value.metadata); packet.Pack>(value.targets); packet.Pack>(value.multitagResult); - packet.Pack>(value.robotToCamera); + packet.Pack>(value.robotToCamera); } PhotonPipelineResult StructType::Unpack(Packet& packet) { @@ -42,7 +42,7 @@ PhotonPipelineResult StructType::Unpack(Packet& packet) { .metadata = packet.Unpack(), .targets = packet.Unpack>(), .multitagResult = packet.Unpack>(), - .robotToCamera = packet.Unpack>(), + .robotToCamera = packet.Unpack>(), }}; } diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h index 4a6aef967b..73cc82ac65 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h @@ -36,10 +36,10 @@ #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PhotonPipelineMetadata.h" #include "photon/targeting/PhotonTrackedTarget.h" -#include #include #include #include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h index 6a03b44d70..468be90211 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h @@ -36,7 +36,7 @@ #include "photon/targeting/TargetCorner.h" #include #include -#include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h index 554a266ab9..8dee1e8a49 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h @@ -34,7 +34,7 @@ // Includes for dependant types #include -#include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h index 65f913b6ee..8314405594 100644 --- a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h +++ b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h @@ -30,10 +30,10 @@ #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PhotonPipelineMetadata.h" #include "photon/targeting/PhotonTrackedTarget.h" -#include #include #include #include +#include namespace photon { @@ -42,7 +42,7 @@ struct PhotonPipelineResult_PhotonStruct { photon::PhotonPipelineMetadata metadata; std::vector targets; std::optional multitagResult; - std::optional robotToCamera; + std::optional robotToCamera; friend bool operator==(PhotonPipelineResult_PhotonStruct const&, PhotonPipelineResult_PhotonStruct const&) = default; }; diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h index a9eace8962..40586156d8 100644 --- a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h +++ b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h @@ -30,7 +30,7 @@ #include "photon/targeting/TargetCorner.h" #include #include -#include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h index b6442585d3..7da4c5ad0a 100644 --- a/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h +++ b/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h @@ -28,7 +28,7 @@ // Includes for dependant types #include -#include +#include namespace photon { 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 ff90d8a416..53bec91ec1 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -17,7 +17,6 @@ package org.photonvision.targeting; -import edu.wpi.first.math.geometry.Transform3d; import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -25,6 +24,7 @@ import org.photonvision.struct.PhotonPipelineResultSerde; import org.photonvision.targeting.proto.PhotonPipelineResultProto; import org.photonvision.targeting.serde.PhotonStructSerializable; +import org.wpilib.math.geometry.Transform3d; import org.wpilib.util.protobuf.ProtobufSerializable; /** Represents a pipeline result from a PhotonCamera. */ diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java index 8100f4871d..64ba6f6729 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java @@ -17,12 +17,12 @@ package org.photonvision.targeting.proto; -import edu.wpi.first.math.geometry.Transform3d; import java.util.Optional; import org.photonvision.proto.Photon.ProtobufPhotonPipelineResult; import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +import org.wpilib.math.geometry.Transform3d; import org.wpilib.util.protobuf.Protobuf; import us.hebi.quickbuf.Descriptors.Descriptor; 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 dc0996b45d..128465022b 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 @@ -52,7 +52,7 @@ concept Optional = is_optional>::value; template concept OptionalWPIStructSerializable = - Optional && wpi::StructSerializable, I...>; + Optional && wpi::util::StructSerializable, I...>; // Struct is where all our actual ser/de methods are implemented template From 63817e0154da6c0ebbb38242778d8415ead8bfd5 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Tue, 21 Apr 2026 20:15:00 -0400 Subject: [PATCH 52/62] wpiformat --- photon-lib/src/main/native/include/photon/PhotonCamera.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index d8437a4ad2..eb67b39062 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -28,6 +28,7 @@ #include #include +#include #include #include #include @@ -39,7 +40,6 @@ #include #include #include -#include #include #include "photon/targeting/PhotonPipelineResult.h" From 29b113b98168616e60e61dfe9077ab9e19020729 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Wed, 22 Apr 2026 10:06:52 -0400 Subject: [PATCH 53/62] it finally builds again horay --- .../main/native/cpp/photon/PhotonCamera.cpp | 10 ++--- .../native/cpp/photon/PhotonPoseEstimator.cpp | 4 +- .../main/native/include/photon/PhotonCamera.h | 21 +++++----- .../src/test/native/cpp/PhotonCameraTest.cpp | 4 +- .../native/cpp/PhotonPoseEstimatorTest.cpp | 37 +++++++--------- photon-serde/messages.yaml | 2 +- .../struct/PhotonPipelineResultSerde.java | 2 +- .../photon/serde/PhotonPipelineResultSerde.h | 2 +- .../photon/serde/PhotonTrackedTargetSerde.h | 2 +- .../include/photon/serde/PnpResultSerde.h | 2 +- .../struct/PhotonPipelineResultStruct.h | 2 +- .../photon/struct/PhotonTrackedTargetStruct.h | 2 +- .../include/photon/struct/PnpResultStruct.h | 2 +- .../struct/RobotToCameraTransformStruct.h | 42 ------------------- .../photon/targeting/PhotonPipelineResult.h | 2 +- .../src/test/native/cpp/PacketTest.cpp | 8 ++-- 16 files changed, 48 insertions(+), 96 deletions(-) delete mode 100644 photon-targeting/src/generated/main/native/include/photon/struct/RobotToCameraTransformStruct.h diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 117553f857..ec8feb632c 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -124,7 +124,7 @@ static const std::string TYPE_STRING = PhotonCamera::PhotonCamera(wpi::nt::NetworkTableInstance instance, const std::string_view cameraName, - std::optional robotToCamera) + std::optional robotToCamera) : mainTable(instance.GetTable("photonvision")), rootTable(mainTable->GetSubTable(cameraName)), rawBytesEntry( @@ -133,7 +133,7 @@ PhotonCamera::PhotonCamera(wpi::nt::NetworkTableInstance instance, TYPE_STRING, {}, {.pollStorage = 20, .periodic = 0.01, .sendAll = true})), robotToCameraPublisher( - rootTable->GetStructTopic("robotToCamera") + rootTable->GetStructTopic("robotToCamera") .Publish()), inputSaveImgEntry( rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()), @@ -190,14 +190,14 @@ PhotonCamera::PhotonCamera(wpi::nt::NetworkTableInstance instance, PhotonCamera::PhotonCamera(wpi::nt::NetworkTableInstance instance, const std::string_view cameraName, - const frc::Transform3d robotToCamera) + const wpi::math::Transform3d robotToCamera) : PhotonCamera(instance, cameraName, std::make_optional(robotToCamera)) {} PhotonCamera::PhotonCamera(const std::string_view cameraName) : PhotonCamera(wpi::nt::NetworkTableInstance::GetDefault(), cameraName) {} PhotonCamera::PhotonCamera(const std::string_view cameraName, - const frc::Transform3d robotToCamera) + const wpi::math::Transform3d robotToCamera) : PhotonCamera(wpi::nt::NetworkTableInstance::GetDefault(), cameraName, std::make_optional(robotToCamera)) {} @@ -476,7 +476,7 @@ std::vector PhotonCamera::tablesThatLookLikePhotonCameras() { return ret; } -void PhotonCamera::SetRobotToCamera(frc::Transform3d newRobotToCamera) { +void PhotonCamera::SetRobotToCamera(wpi::math::Transform3d newRobotToCamera) { robotToCamera = std::make_optional(newRobotToCamera); robotToCameraPublisher.Set(robotToCamera.value()); } diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 4f2b3ae39b..5e2b1a4202 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -62,7 +62,7 @@ cv::Point3d TagCornerToObjectPoint(wpi::units::meter_t cornerX, wpi::math::Pose3d tagPose); } // namespace detail -PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags) +PhotonPoseEstimator::PhotonPoseEstimator(wpi::apriltag::AprilTagFieldLayout tags) : aprilTags(tags), headingBuffer( wpi::math::TimeInterpolatableBuffer(1_s)) { @@ -80,7 +80,7 @@ bool ShouldEstimate(const PhotonPipelineResult& result) { // Result has no robot to camera transform -- can't do estimation if (!result.GetRobotToCamera().has_value()) { - FRC_ReportError(frc::warn::Warning, + WPILIB_ReportError(wpi::warn::Warning, "Result has no robot to camera transform!"); return false; } diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index eb67b39062..a478889fa9 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -28,9 +28,9 @@ #include #include -#include +#include #include -#include +#include #include #include #include @@ -41,6 +41,7 @@ #include #include #include +#include #include "photon/targeting/PhotonPipelineResult.h" @@ -85,7 +86,7 @@ class PhotonCamera { * mount positions (ie, robot âž” camera). */ explicit PhotonCamera(const std::string_view cameraName, - frc::Transform3d robotToCamera); + wpi::math::Transform3d robotToCamera); /** * Constructs a PhotonCamera from a root table. @@ -98,9 +99,9 @@ class PhotonCamera { * @param robotToCamera Transform3d from the center of the robot to the camera * mount positions (ie, robot âž” camera). */ - explicit PhotonCamera(nt::NetworkTableInstance instance, + explicit PhotonCamera(wpi::nt::NetworkTableInstance instance, const std::string_view cameraName, - frc::Transform3d robotToCamera); + wpi::math::Transform3d robotToCamera); PhotonCamera(PhotonCamera&&) = default; @@ -112,7 +113,7 @@ class PhotonCamera { * @return The transform from the robot's center to the camera, if it was set. * Empty otherwise. */ - std::optional GetRobotToCamera() { return robotToCamera; } + std::optional GetRobotToCamera() { return robotToCamera; } /** * Sets the robot to camera transform @@ -120,7 +121,7 @@ class PhotonCamera { * @param newRobotToCamera Transform3d from the center of the robot to the * camera mount positions (ie, robot âž” camera). */ - void SetRobotToCamera(frc::Transform3d newRobotToCamera); + void SetRobotToCamera(wpi::math::Transform3d newRobotToCamera); /** * The list of pipeline results sent by PhotonVision since the last call to * GetAllUnreadResults(). Calling this function clears the internal FIFO @@ -259,7 +260,7 @@ class PhotonCamera { std::shared_ptr mainTable; std::shared_ptr rootTable; wpi::nt::RawSubscriber rawBytesEntry; - nt::StructPublisher robotToCameraPublisher; + wpi::nt::StructPublisher robotToCameraPublisher; wpi::nt::IntegerPublisher inputSaveImgEntry; wpi::nt::IntegerSubscriber inputSaveImgSubscriber; wpi::nt::IntegerPublisher outputSaveImgEntry; @@ -296,7 +297,7 @@ class PhotonCamera { */ explicit PhotonCamera(wpi::nt::NetworkTableInstance instance, const std::string_view cameraName, - std::optional robotToCamera); + std::optional robotToCamera); wpi::units::second_t lastVersionCheckTime = 0_s; static bool VERSION_CHECK_ENABLED; inline static int InstanceCount = 1; @@ -312,7 +313,7 @@ class PhotonCamera { void CheckTimeSyncOrWarn(photon::PhotonPipelineResult& result); std::vector tablesThatLookLikePhotonCameras(); - std::optional robotToCamera; + std::optional robotToCamera; }; } // namespace photon diff --git a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp index f163c4f716..2991355fb0 100644 --- a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp @@ -110,8 +110,8 @@ TEST(PhotonCameraTest, Alerts) { photon::PhotonPipelineMetadata metadata{3, 1, 2, 10 * 1000000}; photon::PhotonPipelineResult noPongResult{ metadata, std::vector{}, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + std::make_optional(wpi::math::Transform3d{ + wpi::math::Translation3d(0_m, 0_m, 1_m), wpi::math::Rotation3d()})}; // Loop to hit cases past first iteration for (int i = 0; i < 10; i++) { diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index d9b4deeaf3..4473a21497 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -90,7 +90,7 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional({})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags); @@ -151,7 +151,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional({{0_m, 0_m, 4_m}, {}})}}; + std::make_optional({{0_m, 0_m, 4_m}, {}})}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator(aprilTags); @@ -200,7 +200,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional({})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags); @@ -251,7 +251,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional({})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags); @@ -291,7 +291,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, - std::nullopt, std::make_optional({})}}; + std::nullopt, std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(21)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -416,7 +416,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional({})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags); @@ -459,7 +459,7 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional({})}}; + std::make_optional({})}}; cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags); @@ -488,8 +488,8 @@ TEST(PhotonPoseEstimatorTest, CopyResult) { auto testResult = photon::PhotonPipelineResult{ photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})}; + std::make_optional(wpi::math::Transform3d{ + wpi::math::Translation3d(0_m, 0_m, 1_m), wpi::math::Rotation3d()})}; testResult.SetReceiveTimestamp(wpi::units::second_t(11)); auto test2 = testResult; @@ -501,8 +501,7 @@ TEST(PhotonPoseEstimatorTest, CopyResult) { TEST(PhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) { photon::PhotonPoseEstimator estimator( wpi::apriltag::AprilTagFieldLayout::LoadField( - wpi::apriltag::AprilTagField::k2024Crescendo), - wpi::math::Transform3d()); + wpi::apriltag::AprilTagField::k2024Crescendo)); photon::PhotonPipelineResult result; auto distortion = Eigen::VectorXd::Zero(8); @@ -543,26 +542,20 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { photon::PnpResult{poseTransform, poseTransform, 0.1, 0.1, 0.0}, std::vector{8}); - const units::radian_t camPitch = 30_deg; - const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m), - frc::Rotation3d(0_rad, -camPitch, 0_rad)}; + const wpi::units::radian_t camPitch = 30_deg; + const wpi::math::Transform3d kRobotToCam{wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m), + wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)}; photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, - multiTagResult, std::make_optional(kRobotToCam)}; + multiTagResult, std::make_optional(kRobotToCam)}; 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{ - wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m), - wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)}; - photon::PhotonPoseEstimator estimator( wpi::apriltag::AprilTagFieldLayout::LoadField( - wpi::apriltag::AprilTagField::k2024Crescendo), - kRobotToCam); + wpi::apriltag::AprilTagField::k2024Crescendo)); auto estimatedMultiTagPose = estimator.EstimateCoprocMultiTagPose(cameraOne.testResult[0]); diff --git a/photon-serde/messages.yaml b/photon-serde/messages.yaml index 0327b120b1..a922c9072e 100644 --- a/photon-serde/messages.yaml +++ b/photon-serde/messages.yaml @@ -15,7 +15,7 @@ java_decode_shim: PacketUtils.unpackTransform3d java_encode_shim: PacketUtils.packTransform3d cpp_type: wpi::math::Transform3d - cpp_include: "" + cpp_include: "" python_decode_shim: decodeTransform python_encode_shim: encodeTransform java_import: org.wpilib.math.geometry.Transform3d diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java index 1dc2528618..954b948d26 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java @@ -99,7 +99,7 @@ public PhotonPipelineResult unpack(Packet packet) { @Override public PacketSerde[] getNestedPhotonMessages() { return new PacketSerde[] { - PhotonTrackedTarget.photonStruct,PhotonPipelineMetadata.photonStruct,MultiTargetPNPResult.photonStruct + MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct,PhotonPipelineMetadata.photonStruct }; } diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h index 73cc82ac65..65f2cc3553 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h @@ -39,7 +39,7 @@ #include #include #include -#include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h index 468be90211..6a03b44d70 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h @@ -36,7 +36,7 @@ #include "photon/targeting/TargetCorner.h" #include #include -#include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h index 8dee1e8a49..554a266ab9 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h @@ -34,7 +34,7 @@ // Includes for dependant types #include -#include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h index 8314405594..1ff6f9da24 100644 --- a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h +++ b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h @@ -33,7 +33,7 @@ #include #include #include -#include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h index 40586156d8..a9eace8962 100644 --- a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h +++ b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h @@ -30,7 +30,7 @@ #include "photon/targeting/TargetCorner.h" #include #include -#include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h index 7da4c5ad0a..b6442585d3 100644 --- a/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h +++ b/photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h @@ -28,7 +28,7 @@ // Includes for dependant types #include -#include +#include namespace photon { diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/RobotToCameraTransformStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/RobotToCameraTransformStruct.h deleted file mode 100644 index 575f2efce7..0000000000 --- a/photon-targeting/src/generated/main/native/include/photon/struct/RobotToCameraTransformStruct.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#pragma once - -// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY - -// Includes for dependant types -#include -#include - - -namespace photon { - -struct RobotToCameraTransform_PhotonStruct { - frc::Transform3d robotToCamera; - - friend bool operator==(RobotToCameraTransform_PhotonStruct const&, RobotToCameraTransform_PhotonStruct const&) = default; -}; - -} // 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 63e01a1162..82e7b5bc1b 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -81,7 +81,7 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { return HasTargets() ? targets[0] : PhotonTrackedTarget{}; } - std::optional GetRobotToCamera() const { + std::optional GetRobotToCamera() const { return robotToCamera; } diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp index 6b707c1fa8..e61f21a315 100644 --- a/photon-targeting/src/test/native/cpp/PacketTest.cpp +++ b/photon-targeting/src/test/native/cpp/PacketTest.cpp @@ -90,8 +90,8 @@ TEST(PacketTest, PhotonPipelineResult) { PhotonPipelineResult result( PhotonPipelineMetadata(0, 0, 1, 2), std::vector{}, std::nullopt, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})); + std::make_optional(wpi::math::Transform3d{ + wpi::math::Translation3d(0_m, 0_m, 1_m), wpi::math::Rotation3d()})); Packet p; p.Pack(result); @@ -137,8 +137,8 @@ TEST(PacketTest, PhotonPipelineResult) { PhotonPipelineResult result2( PhotonPipelineMetadata{0, 0, 1, 1}, targets, mtResult, - std::make_optional(frc::Transform3d{ - frc::Translation3d(0_m, 0_m, 1_m), frc::Rotation3d()})); + std::make_optional(wpi::math::Transform3d{ + wpi::math::Translation3d(0_m, 0_m, 1_m), wpi::math::Rotation3d()})); Packet p2; auto t1 = std::chrono::steady_clock::now(); From 3e0b03dfaa5d7978735e77507f1467cd3e90ba73 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Wed, 22 Apr 2026 10:09:20 -0400 Subject: [PATCH 54/62] linting --- .../src/main/native/cpp/photon/PhotonPoseEstimator.cpp | 5 +++-- photon-lib/src/main/native/include/photon/PhotonCamera.h | 7 ++++--- photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp | 5 +++-- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 5e2b1a4202..9a2f017d75 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -62,7 +62,8 @@ cv::Point3d TagCornerToObjectPoint(wpi::units::meter_t cornerX, wpi::math::Pose3d tagPose); } // namespace detail -PhotonPoseEstimator::PhotonPoseEstimator(wpi::apriltag::AprilTagFieldLayout tags) +PhotonPoseEstimator::PhotonPoseEstimator( + wpi::apriltag::AprilTagFieldLayout tags) : aprilTags(tags), headingBuffer( wpi::math::TimeInterpolatableBuffer(1_s)) { @@ -81,7 +82,7 @@ bool ShouldEstimate(const PhotonPipelineResult& result) { // Result has no robot to camera transform -- can't do estimation if (!result.GetRobotToCamera().has_value()) { WPILIB_ReportError(wpi::warn::Warning, - "Result has no robot to camera transform!"); + "Result has no robot to camera transform!"); return false; } diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index a478889fa9..7c4734bcab 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -28,7 +28,6 @@ #include #include -#include #include #include #include @@ -40,8 +39,8 @@ #include #include #include +#include #include -#include #include "photon/targeting/PhotonPipelineResult.h" @@ -113,7 +112,9 @@ class PhotonCamera { * @return The transform from the robot's center to the camera, if it was set. * Empty otherwise. */ - std::optional GetRobotToCamera() { return robotToCamera; } + std::optional GetRobotToCamera() { + return robotToCamera; + } /** * Sets the robot to camera transform diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 4473a21497..8f711b4ac5 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -543,8 +543,9 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) { std::vector{8}); const wpi::units::radian_t camPitch = 30_deg; - const wpi::math::Transform3d kRobotToCam{wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m), - wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)}; + const wpi::math::Transform3d kRobotToCam{ + wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m), + wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)}; photon::PhotonPipelineResult result{ photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets, multiTagResult, std::make_optional(kRobotToCam)}; From 72a9d609c2e9572c16dc0c4cb7ac305014973b54 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Wed, 22 Apr 2026 21:06:21 -0400 Subject: [PATCH 55/62] switched to using getters in photonposeestimator.java --- .../org/photonvision/PhotonPoseEstimator.java | 46 ++++++------------- 1 file changed, 14 insertions(+), 32 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index aac6d0f70c..3681abbf1a 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -122,7 +122,6 @@ public static final record ConstrainedSolvepnpParams( private AprilTagFieldLayout fieldTags; private TargetModel tagModel = TargetModel.kAprilTag36h11; - private Transform3d robotToCamera; private final Set reportedErrors = new HashSet<>(); private final TimeInterpolatableBuffer headingBuffer = @@ -230,23 +229,6 @@ public void resetHeadingData(double timestampSeconds, Rotation2d heading) { addHeadingData(timestampSeconds, heading); } - /** - * @return The current transform from the center of the robot to the camera mount position - */ - public Transform3d getRobotToCameraTransform() { - return robotToCamera; - } - - /** - * Useful for pan and tilt mechanisms and such. - * - * @param robotToCamera The current transform from the center of the robot to the camera mount - * position - */ - public void setRobotToCameraTransform(Transform3d robotToCamera) { - this.robotToCamera = robotToCamera; - } - /** * @param cameraResult A pipeline result from the camera. * @return Whether or not pose estimation should be performed. @@ -300,7 +282,7 @@ public Optional estimatePnpDistanceTrigSolvePose( 0, -Math.toRadians(bestTarget.getPitch()), -Math.toRadians(bestTarget.getYaw()))) - .rotateBy(cameraResult.robotToCamera.get().getRotation()) + .rotateBy(cameraResult.getRobotToCamera().get().getRotation()) .toTranslation2d() .rotateBy(headingSample); @@ -315,7 +297,7 @@ public Optional estimatePnpDistanceTrigSolvePose( Translation2d camToRobotTranslation = cameraResult - .robotToCamera + .getRobotToCamera() .get() .getTranslation() .toTranslation2d() @@ -384,7 +366,7 @@ public Optional estimateConstrainedSolvepnpPose( cameraMatrix, distCoeffs, cameraResult.getTargets(), - cameraResult.robotToCamera.get(), + cameraResult.getRobotToCamera().get(), seedPose, fieldTags, tagModel, @@ -422,7 +404,7 @@ public Optional estimateCoprocMultiTagPose( Pose3d.kZero .plus(best_tf) // field-to-camera .relativeTo(fieldTags.getOrigin()) - .plus(cameraResult.robotToCamera.get().inverse()); // field-to-robot + .plus(cameraResult.getRobotToCamera().get().inverse()); // field-to-robot return Optional.of( new EstimatedRobotPose( best, @@ -456,7 +438,7 @@ public Optional estimateRioMultiTagPose( var best = Pose3d.kZero .plus(pnpResult.get().best) // field-to-camera - .plus(cameraResult.robotToCamera.get().inverse()); // field-to-robot + .plus(cameraResult.getRobotToCamera().get().inverse()); // field-to-robot return Optional.of( new EstimatedRobotPose( @@ -510,7 +492,7 @@ public Optional estimateLowestAmbiguityPose( targetPosition .get() .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) - .transformBy(cameraResult.robotToCamera.get().inverse()), + .transformBy(cameraResult.getRobotToCamera().get().inverse()), cameraResult.getTimestampSeconds(), cameraResult.getTargets(), PoseStrategy.LOWEST_AMBIGUITY)); @@ -549,14 +531,14 @@ public Optional estimateClosestToCameraHeightPose( double alternateTransformDelta = Math.abs( - cameraResult.robotToCamera.get().getZ() + cameraResult.getRobotToCamera().get().getZ() - targetPosition .get() .transformBy(target.getAlternateCameraToTarget().inverse()) .getZ()); double bestTransformDelta = Math.abs( - cameraResult.robotToCamera.get().getZ() + cameraResult.getRobotToCamera().get().getZ() - targetPosition .get() .transformBy(target.getBestCameraToTarget().inverse()) @@ -569,7 +551,7 @@ public Optional estimateClosestToCameraHeightPose( targetPosition .get() .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(cameraResult.robotToCamera.get().inverse()), + .transformBy(cameraResult.getRobotToCamera().get().inverse()), cameraResult.getTimestampSeconds(), cameraResult.getTargets(), PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); @@ -582,7 +564,7 @@ public Optional estimateClosestToCameraHeightPose( targetPosition .get() .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(cameraResult.robotToCamera.get().inverse()), + .transformBy(cameraResult.getRobotToCamera().get().inverse()), cameraResult.getTimestampSeconds(), cameraResult.getTargets(), PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); @@ -636,12 +618,12 @@ public Optional estimateClosestToReferencePose( targetPosition .get() .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(cameraResult.robotToCamera.get().inverse()); + .transformBy(cameraResult.getRobotToCamera().get().inverse()); Pose3d bestTransformPosition = targetPosition .get() .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(cameraResult.robotToCamera.get().inverse()); + .transformBy(cameraResult.getRobotToCamera().get().inverse()); double altDifference = Math.abs(calculateDifference(referencePose, altTransformPosition)); double bestDifference = Math.abs(calculateDifference(referencePose, bestTransformPosition)); @@ -707,7 +689,7 @@ public Optional estimateAverageBestTargetsPose( targetPosition .get() .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(cameraResult.robotToCamera.get().inverse()), + .transformBy(cameraResult.getRobotToCamera().get().inverse()), cameraResult.getTimestampSeconds(), cameraResult.getTargets(), PoseStrategy.AVERAGE_BEST_TARGETS)); @@ -721,7 +703,7 @@ public Optional estimateAverageBestTargetsPose( targetPosition .get() .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(cameraResult.robotToCamera.get().inverse()))); + .transformBy(cameraResult.getRobotToCamera().get().inverse()))); } // Take the average From e6c1db9bc153cf03a59f24c4dcf4fa6672fc5974 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Wed, 22 Apr 2026 21:40:29 -0400 Subject: [PATCH 56/62] linting --- .../src/main/java/org/photonvision/PhotonPoseEstimator.java | 1 - 1 file changed, 1 deletion(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 3681abbf1a..1829d72350 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -35,7 +35,6 @@ import org.wpilib.math.geometry.Pose3d; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.math.geometry.Rotation3d; -import org.wpilib.math.geometry.Transform3d; import org.wpilib.math.geometry.Translation2d; import org.wpilib.math.geometry.Translation3d; import org.wpilib.math.interpolation.TimeInterpolatableBuffer; From 1862c1873b150e96f34350bc6258a57e28631ee8 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 23 Apr 2026 02:28:51 -0400 Subject: [PATCH 57/62] added design doc --- .../design-descriptions/robot-to-camera.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 docs/source/docs/contributing/design-descriptions/robot-to-camera.md diff --git a/docs/source/docs/contributing/design-descriptions/robot-to-camera.md b/docs/source/docs/contributing/design-descriptions/robot-to-camera.md new file mode 100644 index 0000000000..cd3e21dfde --- /dev/null +++ b/docs/source/docs/contributing/design-descriptions/robot-to-camera.md @@ -0,0 +1,14 @@ +# Robot to Camera + +## How 3D pose estimation works + +At its core, Photonvision's 3D pose estimation is built around solving the Perspective-n-Point (PnP) problem. The PnP problem is essentially, 'given a set of points in 3D space, and their projections on a 2D image, determine the pose of the camera'. In photonvision's case, the points are the corners of one or more apriltags. +However, this leaves us with the camera's pose, *not* the robot's pose. The solution, of course, is to apply an offset. For 3D Pose estimation, Photonvision associates to each camera a `Transform3d` object, representing a vector encoding the 6DOF transformation from the robot (or, rather, the point on the robot considered to be its center) to the camera, and the final step of pose estimation is to transform the camera's pose (that got spit out of the PnP solver) by the inverse of this vector, yielding an estimate of the robot's pose. + +## What does the plumbing look like? + +The `PhotonCamera` object is (optionally) passed a `Transform3d` object by the Robot code, corresponding to the robot-to-camera transformation. This `Transform3d` (if passed) is transmitted to the coprocessor by the `PhotonCamera`. The coprocessor then appends that `Transform3d` to every result it sends back to the robot controller, where the `PhotonPoseEstimator` object then applies the transformation to the camera pose (If no `Transform3d` is passed to `PhotonCamera`, then no `Transform3d` is appended to the results, and `PhotonPoseEstimator` simply won't work. The result stores the `Transform3d` as an optional) + +## That seems complicated and silly. Why not just keep the robot to camera transform in PhotonLib? + +We used to! However, a new algorithm for fusing gyroscopic data to PnP pose observations has been added to PhotonVision, Constrained PnP, offering significantly improved accuracy and stability. Notably, this algorithm requires the robot-to-camera transform to run properly. In previous seasons, this wasn't a problem, as Constrained PnP ran entirely on the RoboRIO anyways, in order to access robot gyro data. However, Constrained PnP is very computationally expensive, and so work is being done to offload the work to the coprocessor, and expose gyro data to the coprocessor. The switch to having the robot-to-camera transform exposed to the coprocessor over NetworkTables and becoming an integrated part of Photonvision results is to facilitate the offloading of the Constrained PnP workload (and other estimation workloads that require Gyroscopic data) to the coprocessor From 5fc6a2e8abfb2f49201c36a56e75a8c28bd7e236 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 23 Apr 2026 02:31:13 -0400 Subject: [PATCH 58/62] linting --- .../docs/contributing/design-descriptions/robot-to-camera.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/source/docs/contributing/design-descriptions/robot-to-camera.md b/docs/source/docs/contributing/design-descriptions/robot-to-camera.md index cd3e21dfde..2a3589c557 100644 --- a/docs/source/docs/contributing/design-descriptions/robot-to-camera.md +++ b/docs/source/docs/contributing/design-descriptions/robot-to-camera.md @@ -3,7 +3,7 @@ ## How 3D pose estimation works At its core, Photonvision's 3D pose estimation is built around solving the Perspective-n-Point (PnP) problem. The PnP problem is essentially, 'given a set of points in 3D space, and their projections on a 2D image, determine the pose of the camera'. In photonvision's case, the points are the corners of one or more apriltags. -However, this leaves us with the camera's pose, *not* the robot's pose. The solution, of course, is to apply an offset. For 3D Pose estimation, Photonvision associates to each camera a `Transform3d` object, representing a vector encoding the 6DOF transformation from the robot (or, rather, the point on the robot considered to be its center) to the camera, and the final step of pose estimation is to transform the camera's pose (that got spit out of the PnP solver) by the inverse of this vector, yielding an estimate of the robot's pose. +However, this leaves us with the camera's pose, *not* the robot's pose. The solution, of course, is to apply an offset. For 3D Pose estimation, Photonvision associates to each camera a `Transform3d` object, representing a vector encoding the 6DOF transformation from the robot (or, rather, the point on the robot considered to be its center) to the camera, and the final step of pose estimation is to transform the camera's pose (that got spit out of the PnP solver) by the inverse of this vector, yielding an estimate of the robot's pose. ## What does the plumbing look like? From 245797f711252159e8a2e9e5bd085793b38cc64c Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 23 Apr 2026 02:34:01 -0400 Subject: [PATCH 59/62] added robot-to-camera design doc to index --- docs/source/docs/contributing/design-descriptions/index.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/source/docs/contributing/design-descriptions/index.md b/docs/source/docs/contributing/design-descriptions/index.md index b6e706271a..78dc4463a2 100644 --- a/docs/source/docs/contributing/design-descriptions/index.md +++ b/docs/source/docs/contributing/design-descriptions/index.md @@ -6,4 +6,5 @@ image-rotation time-sync camera-matching e2e-latency +robot-to-camera ``` From 60ac85757f4ecbaa4c8c7079979ae9f68773a06a Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Thu, 23 Apr 2026 12:38:04 -0400 Subject: [PATCH 60/62] moved robotToCamera transform to VisionModule --- .../vision/processes/PipelineManager.java | 46 ++++++++++--------- .../vision/processes/VisionModule.java | 13 ++++-- .../vision/processes/VisionModuleManager.java | 3 +- 3 files changed, 34 insertions(+), 28 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java index 68215f4ef6..332e814cb9 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java @@ -22,7 +22,7 @@ import java.util.Arrays; import java.util.Comparator; import java.util.List; -import java.util.concurrent.atomic.AtomicReference; +import java.util.function.Supplier; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.DataChangeService; @@ -45,6 +45,7 @@ public class PipelineManager { protected final Calibrate3dPipeline calibration3dPipeline; protected final FocusPipeline focusPipeline = new FocusPipeline(); protected final DriverModePipeline driverModePipeline = new DriverModePipeline(); + protected final Supplier robotToCamSupplier; /** Index of the currently active pipeline. Defaults to 0. */ private int currentPipelineIndex = DRIVERMODE_INDEX; @@ -52,9 +53,6 @@ public class PipelineManager { /** The currently active pipeline. */ private CVPipeline currentUserPipeline = driverModePipeline; - /** The current robot to camera transform. */ - private AtomicReference robotToCamera = new AtomicReference<>(); - /** * Index of the last active user-created pipeline.
*
@@ -63,13 +61,15 @@ public class PipelineManager { private int lastUserPipelineIdx; /** - * Creates a PipelineManager with a DriverModePipeline, a Calibration3dPipeline, and all provided - * pipelines. + * Creates a PipelineManager with a DriverModePipeline, a Calibration3dPipeline, a robotToCamera + * supplier, and all provided pipelines. */ PipelineManager( DriverModePipelineSettings driverSettings, List userPipelines, + Supplier robotToCamSupplier, int defaultIndex) { + this.robotToCamSupplier = robotToCamSupplier; this.userPipelineSettings = new ArrayList<>(userPipelines); // This is to respect the default res idx for vendor cameras @@ -86,8 +86,19 @@ public class PipelineManager { updatePipelineFromRequested(); } - public PipelineManager(CameraConfiguration config) { - this(config.driveModeSettings, config.pipelineSettings, config.currentPipelineIndex); + PipelineManager( + DriverModePipelineSettings driverSettings, + List userPipelines, + int defaultIndex) { + this(driverSettings, userPipelines, () -> null, defaultIndex); + } + + public PipelineManager(CameraConfiguration config, Supplier robotToCamSupplier) { + this( + config.driveModeSettings, + config.pipelineSettings, + robotToCamSupplier, + config.currentPipelineIndex); } /** @@ -187,14 +198,6 @@ public int getRequestedIndex() { return requestedIndex; } - public Transform3d getRobotToCamera() { - return robotToCamera.get(); - } - - public void setRobotToCamera(Transform3d newTransform) { - robotToCamera.set(newTransform); - } - /** * Internal method for setting the active pipeline.
*
@@ -259,31 +262,30 @@ private void recreateUserPipeline() { logger.debug("Creating Reflective pipeline"); currentUserPipeline = new ReflectivePipeline( - (ReflectivePipelineSettings) desiredPipelineSettings, this::getRobotToCamera); + (ReflectivePipelineSettings) desiredPipelineSettings, robotToCamSupplier); } case ColoredShape -> { logger.debug("Creating ColoredShape pipeline"); currentUserPipeline = new ColoredShapePipeline( - (ColoredShapePipelineSettings) desiredPipelineSettings, this::getRobotToCamera); + (ColoredShapePipelineSettings) desiredPipelineSettings, robotToCamSupplier); } case AprilTag -> { logger.debug("Creating AprilTag pipeline"); currentUserPipeline = new AprilTagPipeline( - (AprilTagPipelineSettings) desiredPipelineSettings, this::getRobotToCamera); + (AprilTagPipelineSettings) desiredPipelineSettings, robotToCamSupplier); } case Aruco -> { logger.debug("Creating ArUco Pipeline"); currentUserPipeline = - new ArucoPipeline( - (ArucoPipelineSettings) desiredPipelineSettings, this::getRobotToCamera); + new ArucoPipeline((ArucoPipelineSettings) desiredPipelineSettings, robotToCamSupplier); } case ObjectDetection -> { logger.debug("Creating ObjectDetection Pipeline"); currentUserPipeline = new ObjectDetectionPipeline( - (ObjectDetectionPipelineSettings) desiredPipelineSettings, this::getRobotToCamera); + (ObjectDetectionPipelineSettings) desiredPipelineSettings, robotToCamSupplier); } case Calib3d, DriverMode, FocusCamera -> {} } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index e9998cef4e..24ae880006 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -22,6 +22,7 @@ import java.util.HashMap; import java.util.LinkedList; import java.util.List; +import java.util.concurrent.atomic.AtomicReference; import java.util.function.BiConsumer; import org.opencv.core.Size; import org.photonvision.common.configuration.CameraConfiguration; @@ -98,13 +99,18 @@ public class VisionModule { boolean mismatch; - public VisionModule(PipelineManager pipelineManager, VisionSource visionSource) { + private AtomicReference robotToCamera = new AtomicReference<>(); + + public VisionModule(VisionSource visionSource) { logger = new Logger( VisionModule.class, visionSource.getSettables().getConfiguration().nickname, LogGroup.VisionModule); + pipelineManager = + new PipelineManager(visionSource.getCameraConfiguration(), this::getRobotToCameraTransform); + mismatch = false; cameraQuirks = visionSource.getCameraConfiguration().cameraQuirks; @@ -128,7 +134,6 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource) }); } - this.pipelineManager = pipelineManager; this.visionSource = visionSource; changeSubscriber = new VisionModuleChangeSubscriber(this); this.visionRunner = @@ -644,7 +649,7 @@ public void setFPSLimit(int fps) { * the robot's coordinate system. This should be provided in meters. */ public void setRobotToCameraTransform(Transform3d robotToCameraTransform) { - this.pipelineManager.setRobotToCamera(robotToCameraTransform); + this.robotToCamera.set(robotToCameraTransform); } /** @@ -654,7 +659,7 @@ public void setRobotToCameraTransform(Transform3d robotToCameraTransform) { * system, in meters. May return null if no transform is set. */ public Transform3d getRobotToCameraTransform() { - return this.pipelineManager.getRobotToCamera(); + return this.robotToCamera.get(); } /** diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java index 7eb3c86212..20f4d30c33 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java @@ -43,8 +43,7 @@ public VisionModule getModule(String uniqueName) { public synchronized VisionModule addSource(VisionSource visionSource) { visionSource.cameraConfiguration.streamIndex = newCameraIndex(); - var pipelineManager = new PipelineManager(visionSource.getCameraConfiguration()); - var module = new VisionModule(pipelineManager, visionSource); + var module = new VisionModule(visionSource); visionModules.add(module); return module; From 4190a35e01f7bb0f1382990966c2f8082370dd7d Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sun, 26 Apr 2026 14:03:45 -0400 Subject: [PATCH 61/62] added robotToCamera transform buffer with lerp --- .../networktables/NTDataPublisher.java | 13 +++-- .../vision/pipeline/AprilTagPipeline.java | 10 ++-- .../vision/pipeline/ArucoPipeline.java | 10 ++-- .../vision/pipeline/CVPipeline.java | 9 +-- .../vision/pipeline/Calibrate3dPipeline.java | 2 +- .../vision/pipeline/ColoredShapePipeline.java | 10 ++-- .../vision/pipeline/DriverModePipeline.java | 8 +-- .../vision/pipeline/FocusPipeline.java | 8 +-- .../pipeline/ObjectDetectionPipeline.java | 10 ++-- .../vision/pipeline/ReflectivePipeline.java | 10 ++-- .../vision/processes/PipelineManager.java | 28 ++++++---- .../vision/processes/VisionModule.java | 55 ++++++++++++++++--- .../java/org/photonvision/PhotonCamera.java | 4 +- .../common/networktables/NTTopicSet.java | 9 ++- 14 files changed, 117 insertions(+), 69 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 548823c1c0..10c6660c3c 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -18,6 +18,7 @@ package org.photonvision.common.dataflow.networktables; import java.util.List; +import java.util.function.BiConsumer; import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.Supplier; @@ -35,6 +36,7 @@ import org.wpilib.networktables.NetworkTable; import org.wpilib.networktables.NetworkTableEvent; import org.wpilib.networktables.NetworkTablesJNI; +import org.wpilib.networktables.TimestampedObject; public class NTDataPublisher implements CVPipelineResultConsumer { private final Logger logger = new Logger(NTDataPublisher.class, LogGroup.General); @@ -56,7 +58,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer { private final Supplier fpsLimitSupplier; NTDataChangeListener robotToCameraListener; - private final Consumer robotToCameraConsumer; + private final BiConsumer robotToCameraConsumer; public NTDataPublisher( String cameraNickname, @@ -66,7 +68,7 @@ public NTDataPublisher( Consumer driverModeConsumer, Supplier fpsLimitSupplier, Consumer fpsLimitConsumer, - Consumer robotToCameraConsumer) { + BiConsumer robotToCameraConsumer) { this.pipelineIndexSupplier = pipelineIndexSupplier; this.pipelineIndexConsumer = pipelineIndexConsumer; this.driverModeSupplier = driverModeSupplier; @@ -106,9 +108,10 @@ private void onPipelineIndexChange(NetworkTableEvent entryNotification) { private void onRobotToCameraChange(NetworkTableEvent entryNotification) { // HACK: the entryNotification's value can't be cast to Transform3d, so we read directly from // the subscriber - var robotToCamera = ts.robotToCameraSubscriber.get(); - robotToCameraConsumer.accept(robotToCamera); - logger.debug("Updated robot to camera transform to " + robotToCamera); + TimestampedObject robotToCamera = ts.robotToCameraSubscriber.getAtomic(null); + var doubleTime = (robotToCamera.serverTime) * (double) 1e6; + robotToCameraConsumer.accept(doubleTime, robotToCamera.value); + logger.debug("Sampled robot to camera transform as " + robotToCamera + " at t=" + doubleTime); } private void onDriverModeChange(NetworkTableEvent entryNotification) { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index 51d0bcd42a..f983935230 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -20,7 +20,7 @@ import java.util.ArrayList; import java.util.List; import java.util.Optional; -import java.util.function.Supplier; +import java.util.function.Function; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.logging.LogGroup; @@ -64,13 +64,13 @@ public class AprilTagPipeline extends CVPipeline null); + super(PROCESSING_TYPE, (time) -> null); settings = new AprilTagPipelineSettings(); } public AprilTagPipeline( - AprilTagPipelineSettings settings, Supplier robotToCameraSupplier) { - super(PROCESSING_TYPE, robotToCameraSupplier); + AprilTagPipelineSettings settings, Function robotToCameraSampler) { + super(PROCESSING_TYPE, robotToCameraSampler); this.settings = settings; } @@ -255,7 +255,7 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting targetList, multiTagResult, frame, - Optional.ofNullable(robotToCameraSupplier.get())); + Optional.ofNullable(robotToCameraSampler.apply(frame.timestampNanos))); } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java index e4ef3487c9..61377b2309 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java @@ -20,7 +20,7 @@ import java.util.ArrayList; import java.util.List; import java.util.Optional; -import java.util.function.Supplier; +import java.util.function.Function; import org.opencv.core.Mat; import org.opencv.imgproc.Imgproc; import org.opencv.objdetect.Objdetect; @@ -56,13 +56,13 @@ public class ArucoPipeline extends CVPipeline null); + super(FrameThresholdType.GREYSCALE, (time) -> null); settings = new ArucoPipelineSettings(); } public ArucoPipeline( - ArucoPipelineSettings settings, Supplier robotToCameraSupplier) { - super(FrameThresholdType.GREYSCALE, robotToCameraSupplier); + ArucoPipelineSettings settings, Function robotToCameraSampler) { + super(FrameThresholdType.GREYSCALE, robotToCameraSampler); this.settings = settings; } @@ -243,7 +243,7 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings) targetList, multiTagResult, frame, - Optional.ofNullable(robotToCameraSupplier.get())); + Optional.ofNullable(robotToCameraSampler.apply(frame.timestampNanos))); } private void drawThresholdFrame(Mat greyMat, Mat outputMat, int windowSize, double constant) { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java index 3156493604..9ccfe8e2ca 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java @@ -17,7 +17,7 @@ package org.photonvision.vision.pipeline; -import java.util.function.Supplier; +import java.util.function.Function; import org.photonvision.vision.camera.QuirkyCamera; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameStaticProperties; @@ -31,16 +31,17 @@ public abstract class CVPipeline robotToCameraSupplier; + protected Function robotToCameraSampler; private final FrameThresholdType thresholdType; // So releaseable doesn't keep track of if we double-free something. so (ew) remember that here protected volatile boolean released = false; - public CVPipeline(FrameThresholdType thresholdType, Supplier robotToCameraSupplier) { + public CVPipeline( + FrameThresholdType thresholdType, Function robotToCameraSampler) { this.thresholdType = thresholdType; - this.robotToCameraSupplier = robotToCameraSupplier; + this.robotToCameraSampler = robotToCameraSampler; } public FrameThresholdType getThresholdType() { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java index e72dc76385..e416b22b39 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java @@ -74,7 +74,7 @@ public Calibrate3dPipeline() { } public Calibrate3dPipeline(int minSnapshots) { - super(PROCESSING_TYPE, () -> null); + super(PROCESSING_TYPE, (time) -> null); this.settings = new Calibration3dPipelineSettings(); this.foundCornersList = new ArrayList<>(); this.minSnapshots = minSnapshots; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java index ece3819b29..490222a1d9 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java @@ -20,7 +20,7 @@ import java.util.Arrays; import java.util.List; import java.util.Optional; -import java.util.function.Supplier; +import java.util.function.Function; import org.opencv.core.Point; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -57,13 +57,13 @@ public class ColoredShapePipeline private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.HSV; public ColoredShapePipeline() { - super(PROCESSING_TYPE, () -> null); + super(PROCESSING_TYPE, (time) -> null); settings = new ColoredShapePipelineSettings(); } public ColoredShapePipeline( - ColoredShapePipelineSettings settings, Supplier robotToCameraSupplier) { - super(PROCESSING_TYPE, robotToCameraSupplier); + ColoredShapePipelineSettings settings, Function robotToCameraSampler) { + super(PROCESSING_TYPE, robotToCameraSampler); this.settings = settings; } @@ -223,6 +223,6 @@ protected CVPipelineResult process(Frame frame, ColoredShapePipelineSettings set fps, targetList, frame, - Optional.ofNullable(robotToCameraSupplier.get())); + Optional.ofNullable(robotToCameraSampler.apply(frame.timestampNanos))); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java index 034bad6d8f..dd608cc14c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java @@ -18,7 +18,7 @@ package org.photonvision.vision.pipeline; import java.util.List; -import java.util.function.Supplier; +import java.util.function.Function; import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -38,13 +38,13 @@ public class DriverModePipeline private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE; public DriverModePipeline() { - super(PROCESSING_TYPE, () -> null); + super(PROCESSING_TYPE, (time) -> null); settings = new DriverModePipelineSettings(); } public DriverModePipeline( - DriverModePipelineSettings settings, Supplier robotToCameraSupplier) { - super(PROCESSING_TYPE, robotToCameraSupplier); + DriverModePipelineSettings settings, Function robotToCameraSampler) { + super(PROCESSING_TYPE, robotToCameraSampler); this.settings = settings; } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/FocusPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/FocusPipeline.java index 342b5b6e71..f780d72a02 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/FocusPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/FocusPipeline.java @@ -17,7 +17,7 @@ package org.photonvision.vision.pipeline; -import java.util.function.Supplier; +import java.util.function.Function; import org.opencv.core.Mat; import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.frame.Frame; @@ -37,13 +37,13 @@ public class FocusPipeline extends CVPipeline null); + super(PROCESSING_TYPE, (time) -> null); settings = new FocusPipelineSettings(); } public FocusPipeline( - FocusPipelineSettings settings, Supplier robotToCameraSupplier) { - super(PROCESSING_TYPE, robotToCameraSupplier); + FocusPipelineSettings settings, Function robotToCameraSampler) { + super(PROCESSING_TYPE, robotToCameraSampler); this.settings = settings; } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ObjectDetectionPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ObjectDetectionPipeline.java index a70b918535..28a19441f5 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ObjectDetectionPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ObjectDetectionPipeline.java @@ -19,7 +19,7 @@ import java.util.List; import java.util.Optional; -import java.util.function.Supplier; +import java.util.function.Function; import org.photonvision.common.configuration.NeuralNetworkModelManager; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -46,13 +46,13 @@ public class ObjectDetectionPipeline private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE; public ObjectDetectionPipeline() { - super(PROCESSING_TYPE, () -> null); + super(PROCESSING_TYPE, (time) -> null); settings = new ObjectDetectionPipelineSettings(); } public ObjectDetectionPipeline( - ObjectDetectionPipelineSettings settings, Supplier robotToCameraSupplier) { - super(PROCESSING_TYPE, robotToCameraSupplier); + ObjectDetectionPipelineSettings settings, Function robotToCameraSampler) { + super(PROCESSING_TYPE, robotToCameraSampler); this.settings = settings; } @@ -138,7 +138,7 @@ protected CVPipelineResult process(Frame frame, ObjectDetectionPipelineSettings collect2dTargetsResult.output, frame, names, - Optional.ofNullable(robotToCameraSupplier.get())); + Optional.ofNullable(robotToCameraSampler.apply(frame.timestampNanos))); } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java index a213c0a844..522a7ecf32 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java @@ -19,6 +19,7 @@ import java.util.List; import java.util.Optional; +import java.util.function.Function; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; import org.photonvision.vision.opencv.Contour; @@ -48,14 +49,13 @@ public class ReflectivePipeline extends CVPipeline null); + super(PROCESSING_TYPE, (time) -> null); settings = new ReflectivePipelineSettings(); } public ReflectivePipeline( - ReflectivePipelineSettings settings, - java.util.function.Supplier robotToCameraSupplier) { - super(PROCESSING_TYPE, robotToCameraSupplier); + ReflectivePipelineSettings settings, Function robotToCameraSampler) { + super(PROCESSING_TYPE, robotToCameraSampler); this.settings = settings; } @@ -169,6 +169,6 @@ public CVPipelineResult process(Frame frame, ReflectivePipelineSettings settings fps, targetList, frame, - Optional.ofNullable(robotToCameraSupplier.get())); + Optional.ofNullable(robotToCameraSampler.apply(frame.timestampNanos))); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java index 332e814cb9..989cdd5ef7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java @@ -22,7 +22,7 @@ import java.util.Arrays; import java.util.Comparator; import java.util.List; -import java.util.function.Supplier; +import java.util.function.Function; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.DataChangeService; @@ -45,7 +45,10 @@ public class PipelineManager { protected final Calibrate3dPipeline calibration3dPipeline; protected final FocusPipeline focusPipeline = new FocusPipeline(); protected final DriverModePipeline driverModePipeline = new DriverModePipeline(); - protected final Supplier robotToCamSupplier; + // This is a wrapper around the VisionModule's sampling method that automatically converts from + // the local integer-nanosecond + // time into the server double time expected by the VisionModule's sampler + protected final Function robotToCamSampler; /** Index of the currently active pipeline. Defaults to 0. */ private int currentPipelineIndex = DRIVERMODE_INDEX; @@ -67,9 +70,9 @@ public class PipelineManager { PipelineManager( DriverModePipelineSettings driverSettings, List userPipelines, - Supplier robotToCamSupplier, + Function robotToCamSampler, int defaultIndex) { - this.robotToCamSupplier = robotToCamSupplier; + this.robotToCamSampler = robotToCamSampler; this.userPipelineSettings = new ArrayList<>(userPipelines); // This is to respect the default res idx for vendor cameras @@ -90,14 +93,15 @@ public class PipelineManager { DriverModePipelineSettings driverSettings, List userPipelines, int defaultIndex) { - this(driverSettings, userPipelines, () -> null, defaultIndex); + this(driverSettings, userPipelines, (time) -> null, defaultIndex); } - public PipelineManager(CameraConfiguration config, Supplier robotToCamSupplier) { + public PipelineManager( + CameraConfiguration config, Function robotToCamSampler) { this( config.driveModeSettings, config.pipelineSettings, - robotToCamSupplier, + robotToCamSampler, config.currentPipelineIndex); } @@ -262,30 +266,30 @@ private void recreateUserPipeline() { logger.debug("Creating Reflective pipeline"); currentUserPipeline = new ReflectivePipeline( - (ReflectivePipelineSettings) desiredPipelineSettings, robotToCamSupplier); + (ReflectivePipelineSettings) desiredPipelineSettings, robotToCamSampler); } case ColoredShape -> { logger.debug("Creating ColoredShape pipeline"); currentUserPipeline = new ColoredShapePipeline( - (ColoredShapePipelineSettings) desiredPipelineSettings, robotToCamSupplier); + (ColoredShapePipelineSettings) desiredPipelineSettings, robotToCamSampler); } case AprilTag -> { logger.debug("Creating AprilTag pipeline"); currentUserPipeline = new AprilTagPipeline( - (AprilTagPipelineSettings) desiredPipelineSettings, robotToCamSupplier); + (AprilTagPipelineSettings) desiredPipelineSettings, robotToCamSampler); } case Aruco -> { logger.debug("Creating ArUco Pipeline"); currentUserPipeline = - new ArucoPipeline((ArucoPipelineSettings) desiredPipelineSettings, robotToCamSupplier); + new ArucoPipeline((ArucoPipelineSettings) desiredPipelineSettings, robotToCamSampler); } case ObjectDetection -> { logger.debug("Creating ObjectDetection Pipeline"); currentUserPipeline = new ObjectDetectionPipeline( - (ObjectDetectionPipelineSettings) desiredPipelineSettings, robotToCamSupplier); + (ObjectDetectionPipelineSettings) desiredPipelineSettings, robotToCamSampler); } case Calib3d, DriverMode, FocusCamera -> {} } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index 24ae880006..6c21c395ff 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -22,7 +22,6 @@ import java.util.HashMap; import java.util.LinkedList; import java.util.List; -import java.util.concurrent.atomic.AtomicReference; import java.util.function.BiConsumer; import org.opencv.core.Size; import org.photonvision.common.configuration.CameraConfiguration; @@ -32,6 +31,7 @@ import org.photonvision.common.dataflow.DataChangeService.SubscriberHandle; import org.photonvision.common.dataflow.events.OutgoingUIEvent; import org.photonvision.common.dataflow.networktables.NTDataPublisher; +import org.photonvision.common.dataflow.networktables.NetworkTablesManager; import org.photonvision.common.dataflow.statusLEDs.StatusLEDConsumer; import org.photonvision.common.dataflow.websocket.UICameraConfiguration; import org.photonvision.common.dataflow.websocket.UIDataPublisher; @@ -55,7 +55,11 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TargetModel; import org.photonvision.vision.target.TrackedTarget; +import org.wpilib.math.geometry.Rotation3d; import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.interpolation.Interpolator; +import org.wpilib.math.interpolation.TimeInterpolatableBuffer; import org.wpilib.math.util.Units; import org.wpilib.vision.camera.CameraServerJNI; import org.wpilib.vision.camera.VideoException; @@ -99,7 +103,31 @@ public class VisionModule { boolean mismatch; - private AtomicReference robotToCamera = new AtomicReference<>(); + private static final double bufferDuration = 1.5; + + private static class Transform3dInterpolator implements Interpolator { + @Override + public Transform3d interpolate(Transform3d start, Transform3d end, double t) { + // Maybe clamp t here? TimeInterpolabaleBuffer already does so but IDK if thats a guarantee or + // just an implementation detail + + Translation3d p0 = start.getTranslation(); + Translation3d p1 = end.getTranslation(); + + Translation3d interpTranslation = + new Translation3d( + p0.getX() + (p1.getX() - p0.getX()) * t, + p0.getY() + (p1.getY() - p0.getY()) * t, + p0.getZ() + (p1.getZ() - p0.getZ()) * t); + + Rotation3d interpRotation = start.getRotation().interpolate(end.getRotation(), t); + + return new Transform3d(interpTranslation, interpRotation); + } + } + + private final TimeInterpolatableBuffer robotToCameraBuffer = + TimeInterpolatableBuffer.createBuffer(new Transform3dInterpolator(), bufferDuration); public VisionModule(VisionSource visionSource) { logger = @@ -109,7 +137,12 @@ public VisionModule(VisionSource visionSource) { LogGroup.VisionModule); pipelineManager = - new PipelineManager(visionSource.getCameraConfiguration(), this::getRobotToCameraTransform); + new PipelineManager( + visionSource.getCameraConfiguration(), + (localtime_ns) -> + getRobotToCameraSample( + (localtime_ns + NetworkTablesManager.getInstance().getOffset() * 1000) + / (double) 1e9)); mismatch = false; @@ -160,7 +193,7 @@ public VisionModule(VisionSource visionSource) { this::setDriverMode, this::getFPSLimit, this::setFPSLimit, - this::setRobotToCameraTransform); + this::addRobotToCameraSample); uiDataConsumer = new UIDataPublisher(visionSource.getSettables().getConfiguration().uniqueName); statusLEDsConsumer = new StatusLEDConsumer(visionSource.getSettables().getConfiguration().uniqueName); @@ -645,21 +678,27 @@ public void setFPSLimit(int fps) { /** * Set camera transform for this vision module. * + * @param time the time (in seconds) that the transform was published by the RIO, in server time * @param robotToCameraTransform the transform from the robot's origin to the camera's origin, in * the robot's coordinate system. This should be provided in meters. */ - public void setRobotToCameraTransform(Transform3d robotToCameraTransform) { - this.robotToCamera.set(robotToCameraTransform); + public void addRobotToCameraSample(double time, Transform3d robotToCameraTransform) { + synchronized (robotToCameraBuffer) { + robotToCameraBuffer.addSample(time, robotToCameraTransform); + } } /** * Get robot to camera transform for this vision module. * + * @param time the time (in seconds) to sample * @return the transform from the robot's origin to the camera's origin, in the robot's coordinate * system, in meters. May return null if no transform is set. */ - public Transform3d getRobotToCameraTransform() { - return this.robotToCamera.get(); + public Transform3d getRobotToCameraSample(double time) { + synchronized (robotToCameraBuffer) { + return robotToCameraBuffer.getSample(time).orElse(null); + } } /** diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 7976f17c41..ec75948f12 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -181,7 +181,9 @@ private PhotonCamera( resultSubscriber = new PacketSubscriber<>(rawBytesEntry_pipelineResult, PhotonPipelineResult.photonStruct); robotToCameraPublisher = - cameraTable.getStructTopic("robotToCamera", Transform3d.struct).publish(); + cameraTable + .getStructTopic("robotToCamera", Transform3d.struct) + .publish(PubSubOption.periodic(0.01)); driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish(); driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false); fpsLimitPublisher = cameraTable.getIntegerTopic("fpsLimitRequest").publish(); diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index b153194d64..819e6e2787 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -134,11 +134,10 @@ public void updateEntries() { cameraIntrinsicsPublisher = subTable.getDoubleArrayTopic("cameraIntrinsics").publish(); cameraDistortionPublisher = subTable.getDoubleArrayTopic("cameraDistortion").publish(); robotToCameraTopic = subTable.getStructTopic("robotToCamera", Transform3d.struct); - robotToCameraSubscriber = robotToCameraTopic.subscribe(null); - } - - public boolean robotToCameraExists() { - return robotToCameraTopic.exists(); + robotToCameraSubscriber = + subTable + .getStructTopic("robotToCamera", Transform3d.struct) + .subscribe(null, PubSubOption.periodic(0.01)); } @SuppressWarnings("DuplicatedCode") From dee905cf0730fc8540c874f4a870682262862043 Mon Sep 17 00:00:00 2001 From: Jesse Kane Date: Sun, 26 Apr 2026 16:06:59 -0400 Subject: [PATCH 62/62] updated python and cpp photonlibs --- photon-lib/py/photonlibpy/photonCamera.py | 2 +- photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index 893e969c37..962776a71e 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -70,7 +70,7 @@ def __init__(self, cameraName: str, robotToCamera: Transform3d): ) self._robotToCameraPublisher = self._cameraTable.getStructTopic( "robotToCamera", Transform3d - ).publish() + ).publish(ntcore.PubSubOptions(periodic=0.01)) self._driverModePublisher = self._cameraTable.getBooleanTopic( "driverModeRequest" ).publish() diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index ec8feb632c..0dce77f49b 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -134,7 +134,7 @@ PhotonCamera::PhotonCamera(wpi::nt::NetworkTableInstance instance, {.pollStorage = 20, .periodic = 0.01, .sendAll = true})), robotToCameraPublisher( rootTable->GetStructTopic("robotToCamera") - .Publish()), + .Publish({.periodic = 0.01})), inputSaveImgEntry( rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()), inputSaveImgSubscriber(