[Protobuf] Make MultiTarget result optional

build.gradle

@@ -1,9 +1,10 @@
 plugins {
     id "com.diffplug.spotless" version "6.22.0"
-    id "edu.wpi.first.NativeUtils" version "2024.3.2" apply false
+    id "edu.wpi.first.NativeUtils" version "2024.4.0" apply false
     id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2"
     id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3"
     id 'edu.wpi.first.WpilibTools' version '1.3.0'
+    id 'com.google.protobuf' version '0.9.4' apply false
 }
 
 allprojects {
@@ -20,7 +21,7 @@ allprojects {
 apply from: "versioningHelper.gradle"
 
 ext {
-    wpilibVersion = "2024.1.1-beta-3"
+    wpilibVersion = "2024.1.1-beta-3-52-g9280054"
     wpimathVersion = wpilibVersion
     openCVversion = "4.8.0-2"
     joglVersion = "2.4.0-rc-20200307"

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java

@@ -23,7 +23,6 @@
 import java.util.function.Consumer;
 import java.util.function.Supplier;
 import org.photonvision.common.dataflow.CVPipelineResultConsumer;
-import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.networktables.NTTopicSet;
@@ -129,15 +128,13 @@ public void updateCameraNickname(String newCameraNickname) {
 
     @Override
     public void accept(CVPipelineResult result) {
-        var simplified =
+        var res =
                 new PhotonPipelineResult(
                         result.getLatencyMillis(),
                         TrackedTarget.simpleFromTrackedTargets(result.targets),
-                        result.multiTagResult);
-        Packet packet = new Packet(simplified.getPacketSize());
-        simplified.populatePacket(packet);
+                        result.getMultiTagResult().orElse(null));
 
-        ts.rawBytesEntry.set(packet.getData());
+        ts.pipelineResultsPublisher.set(res);
 
         ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get());
         ts.driverModePublisher.set(driverModeSupplier.getAsBoolean());

photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java

@@ -54,15 +54,16 @@ public void accept(CVPipelineResult result) {
         dataMap.put("targets", uiTargets);
 
         // Only send Multitag Results if they are present, similar to 3d pose
-        if (result.multiTagResult.estimatedPose.isPresent) {
-            var multitagData = new HashMap<String, Object>();
-            multitagData.put(
-                    "bestTransform",
-                    SerializationUtils.transformToHashMap(result.multiTagResult.estimatedPose.best));
-            multitagData.put("bestReprojectionError", result.multiTagResult.estimatedPose.bestReprojErr);
-            multitagData.put("fiducialIDsUsed", result.multiTagResult.fiducialIDsUsed);
-            dataMap.put("multitagResult", multitagData);
-        }
+        result
+                .getMultiTagResult()
+                .ifPresent(
+                        v -> {
+                            var multitagData = new HashMap<String, Object>();
+                            multitagData.put("bestTransform", SerializationUtils.transformToHashMap(v.best));
+                            multitagData.put("bestReprojectionError", v.bestReprojErr);
+                            multitagData.put("fiducialIDsUsed", v.fiducialIDsUsed);
+                            dataMap.put("multitagResult", multitagData);
+                        });
 
         var uiMap = new HashMap<Integer, HashMap<String, Object>>();
         uiMap.put(index, dataMap);

photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java

@@ -26,7 +26,6 @@
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
 import org.opencv.core.MatOfDouble;
-import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.vision.opencv.Releasable;
 
 public class JsonMat implements Releasable {
@@ -121,9 +120,4 @@ public <R extends Num, C extends Num> Matrix<R, C> getAsWpilibMat() {
     public void release() {
         getAsMat().release();
     }
-
-    public Packet populatePacket(Packet packet) {
-        packet.encode(this.data);
-        return packet;
-    }
 }

photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java

@@ -20,6 +20,7 @@
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import java.util.ArrayList;
 import java.util.List;
+import java.util.Optional;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.estimation.TargetModel;
@@ -32,13 +33,15 @@
 /** Estimate the camera pose given multiple Apriltag observations */
 public class MultiTargetPNPPipe
         extends CVPipe<
-                List<TrackedTarget>, MultiTargetPNPResult, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
+                List<TrackedTarget>,
+                Optional<MultiTargetPNPResult>,
+                MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
     private static final Logger logger = new Logger(MultiTargetPNPPipe.class, LogGroup.VisionModule);
 
     private boolean hasWarned = false;
 
     @Override
-    protected MultiTargetPNPResult process(List<TrackedTarget> targetList) {
+    protected Optional<MultiTargetPNPResult> process(List<TrackedTarget> targetList) {
         if (params == null
                 || params.cameraCoefficients == null
                 || params.cameraCoefficients.getCameraIntrinsicsMat() == null
@@ -48,13 +51,13 @@ protected MultiTargetPNPResult process(List<TrackedTarget> targetList) {
                         "Cannot perform solvePNP an uncalibrated camera! Please calibrate this resolution...");
                 hasWarned = true;
             }
-            return new MultiTargetPNPResult();
+            return Optional.empty();
         }
 
         return calculateCameraInField(targetList);
     }
 
-    private MultiTargetPNPResult calculateCameraInField(List<TrackedTarget> targetList) {
+    private Optional<MultiTargetPNPResult> calculateCameraInField(List<TrackedTarget> targetList) {
         // Find tag IDs that exist in the tag layout
         var tagIDsUsed = new ArrayList<Integer>();
         for (var target : targetList) {
@@ -64,18 +67,18 @@ private MultiTargetPNPResult calculateCameraInField(List<TrackedTarget> targetLi
 
         // Only run with multiple targets
         if (tagIDsUsed.size() < 2) {
-            return new MultiTargetPNPResult();
+            return Optional.empty();
         }
 
-        var estimatedPose =
+        var estimatedPoseOpt =
                 VisionEstimation.estimateCamPosePNP(
                         params.cameraCoefficients.cameraIntrinsics.getAsWpilibMat(),
                         params.cameraCoefficients.distCoeffs.getAsWpilibMat(),
                         TrackedTarget.simpleFromTrackedTargets(targetList),
                         params.atfl,
                         params.targetModel);
 
-        return new MultiTargetPNPResult(estimatedPose, tagIDsUsed);
+        return estimatedPoseOpt.map(res -> new MultiTargetPNPResult(res, tagIDsUsed));
     }
 
     public static class MultiTargetPNPPipeParams {

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java

@@ -28,6 +28,7 @@
 import edu.wpi.first.math.util.Units;
 import java.util.ArrayList;
 import java.util.List;
+import java.util.Optional;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.estimation.TargetModel;
@@ -149,11 +150,11 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
         }
 
         // Do multi-tag pose estimation
-        MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
+        Optional<MultiTargetPNPResult> multiTagResultOpt = Optional.empty();
         if (settings.solvePNPEnabled && settings.doMultiTarget) {
             var multiTagOutput = multiTagPNPPipe.run(targetList);
             sumPipeNanosElapsed += multiTagOutput.nanosElapsed;
-            multiTagResult = multiTagOutput.output;
+            multiTagResultOpt = multiTagOutput.output;
         }
 
         // Do single-tag pose estimation
@@ -167,7 +168,8 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
                 AprilTagPoseEstimate tagPoseEstimate = null;
                 // Do single-tag estimation when "always enabled" or if a tag was not used for multitag
                 if (settings.doSingleTargetAlways
-                        || !multiTagResult.fiducialIDsUsed.contains(Integer.valueOf(detection.getId()))) {
+                        || multiTagResultOpt.isEmpty()
+                        || !multiTagResultOpt.get().fiducialIDsUsed.contains(detection.getId())) {
                     var poseResult = singleTagPoseEstimatorPipe.run(detection);
                     sumPipeNanosElapsed += poseResult.nanosElapsed;
                     tagPoseEstimate = poseResult.output;
@@ -180,7 +182,8 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
                     if (tagPose.isPresent()) {
                         var camToTag =
                                 new Transform3d(
-                                        new Pose3d().plus(multiTagResult.estimatedPose.best), tagPose.get());
+                                        new Pose3d().plus(multiTagResultOpt.map(v -> v.best).orElse(new Transform3d())),
+                                        tagPose.get());
                         // match expected AprilTag coordinate system
                         camToTag =
                                 CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN());
@@ -219,6 +222,7 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 
-        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
+        return new CVPipelineResult(
+                sumPipeNanosElapsed, fps, targetList, multiTagResultOpt.orElse(null), frame);
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java

@@ -41,6 +41,7 @@
 import edu.wpi.first.math.util.Units;
 import java.util.ArrayList;
 import java.util.List;
+import java.util.Optional;
 import org.opencv.core.Mat;
 import org.opencv.imgproc.Imgproc;
 import org.opencv.objdetect.Objdetect;
@@ -170,11 +171,11 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings)
         }
 
         // Do multi-tag pose estimation
-        MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
+        Optional<MultiTargetPNPResult> multiTagResultOpt = Optional.empty();
         if (settings.solvePNPEnabled && settings.doMultiTarget) {
             var multiTagOutput = multiTagPNPPipe.run(targetList);
             sumPipeNanosElapsed += multiTagOutput.nanosElapsed;
-            multiTagResult = multiTagOutput.output;
+            multiTagResultOpt = multiTagOutput.output;
         }
 
         // Do single-tag pose estimation
@@ -186,9 +187,11 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings)
 
             for (ArucoDetectionResult detection : tagDetectionPipeResult.output) {
                 AprilTagPoseEstimate tagPoseEstimate = null;
+
                 // Do single-tag estimation when "always enabled" or if a tag was not used for multitag
                 if (settings.doSingleTargetAlways
-                        || !multiTagResult.fiducialIDsUsed.contains(detection.getId())) {
+                        || multiTagResultOpt.isEmpty()
+                        || !multiTagResultOpt.get().fiducialIDsUsed.contains(detection.getId())) {
                     var poseResult = singleTagPoseEstimatorPipe.run(detection);
                     sumPipeNanosElapsed += poseResult.nanosElapsed;
                     tagPoseEstimate = poseResult.output;
@@ -198,10 +201,12 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings)
                 if (tagPoseEstimate == null) {
                     // compute this tag's camera-to-tag transform using the multitag result
                     var tagPose = atfl.getTagPose(detection.getId());
+
                     if (tagPose.isPresent()) {
                         var camToTag =
                                 new Transform3d(
-                                        new Pose3d().plus(multiTagResult.estimatedPose.best), tagPose.get());
+                                        new Pose3d().plus(multiTagResultOpt.map(v -> v.best).orElse(new Transform3d())),
+                                        tagPose.get());
                         // match expected OpenCV coordinate system
                         camToTag =
                                 CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN());
@@ -236,7 +241,8 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings)
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 
-        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
+        return new CVPipelineResult(
+                sumPipeNanosElapsed, fps, targetList, multiTagResultOpt.orElse(null), frame);
     }
 
     private void drawThresholdFrame(Mat greyMat, Mat outputMat, int windowSize, double constant) {

photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java

@@ -34,7 +34,6 @@
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.SerializationUtils;
 import org.photonvision.common.util.file.FileUtils;
-import org.photonvision.targeting.MultiTargetPNPResult;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameThresholdType;
@@ -147,7 +146,7 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se
                 sumPipeNanosElapsed,
                 fps, // Unused but here in case
                 Collections.emptyList(),
-                new MultiTargetPNPResult(),
+                null,
                 new Frame(
                         new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties));
     }

photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java

@@ -19,6 +19,7 @@
 
 import java.util.Collections;
 import java.util.List;
+import java.util.Optional;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.targeting.MultiTargetPNPResult;
 import org.photonvision.vision.frame.Frame;
@@ -31,11 +32,11 @@ public class CVPipelineResult implements Releasable {
     public final double fps;
     public final List<TrackedTarget> targets;
     public final Frame inputAndOutputFrame;
-    public MultiTargetPNPResult multiTagResult;
+    private final MultiTargetPNPResult multiTagResult;
 
     public CVPipelineResult(
             double processingNanos, double fps, List<TrackedTarget> targets, Frame inputFrame) {
-        this(processingNanos, fps, targets, new MultiTargetPNPResult(), inputFrame);
+        this(processingNanos, fps, targets, null, inputFrame);
     }
 
     public CVPipelineResult(
@@ -64,11 +65,12 @@ public boolean hasTargets() {
         return !targets.isEmpty();
     }
 
-    public void release() {
-        for (TrackedTarget tt : targets) {
-            tt.release();
-        }
-        if (inputAndOutputFrame != null) inputAndOutputFrame.release();
+    public long getImageCaptureTimestampNanos() {
+        return imageCaptureTimestampNanos;
+    }
+
+    public double getProcessingMillis() {
+        return MathUtils.nanosToMillis(processingNanos);
     }
 
     /**
@@ -82,15 +84,18 @@ public double getLatencyMillis() {
         return MathUtils.nanosToMillis(now - imageCaptureTimestampNanos);
     }
 
-    public double getProcessingMillis() {
-        return MathUtils.nanosToMillis(processingNanos);
-    }
-
-    public long getImageCaptureTimestampNanos() {
-        return imageCaptureTimestampNanos;
+    public Optional<MultiTargetPNPResult> getMultiTagResult() {
+        return Optional.ofNullable(multiTagResult);
     }
 
     public void setImageCaptureTimestampNanos(long imageCaptureTimestampNanos) {
         this.imageCaptureTimestampNanos = imageCaptureTimestampNanos;
     }
+
+    public void release() {
+        for (TrackedTarget tt : targets) {
+            tt.release();
+        }
+        if (inputAndOutputFrame != null) inputAndOutputFrame.release();
+    }
 }

photon-lib/build.gradle

@@ -60,7 +60,35 @@ task writeCurrentVersion {
 }
 
 build.mustRunAfter writeCurrentVersion
+cppHeadersZip.dependsOn writeCurrentVersion
 
+// Building photon-lib requires photon-targeting to generate its proto files. This technically shouldn't be required but is needed for it to build.
+model {
+    components {
+        all {
+            it.sources.each {
+                it.exportedHeaders {
+                    srcDirs "src/main/native/include"
+                    srcDirs "src/generate/native/include"
+                }
+            }
+            it.binaries.all {
+                it.tasks.withType(CppCompile) {
+                    it.dependsOn ":photon-targeting:generateProto"
+                }
+            }
+        }
+    }
+    testSuites {
+        all {
+            it.binaries.all {
+                it.tasks.withType(CppCompile) {
+                    it.dependsOn ":photon-targeting:generateProto"
+                }
+            }
+        }
+    }
+}
 def vendorJson = artifacts.add('archives', file("$photonlibFileOutput"))
 
 model {