Merge branch '2023-10-20_protobuf' of github.com:mcm001/photonvision …

…into 2023-10-20_protobuf

build.gradle

@@ -20,7 +20,7 @@ allprojects {
 apply from: "versioningHelper.gradle"
 
 ext {
-    wpilibVersion = "2024.1.1-beta-3-22-g0f81296"
+    wpilibVersion = "2024.1.1-beta-3-29-g3985c03"
     openCVversion = "4.8.0-2"
     joglVersion = "2.4.0-rc-20200307"
     javalinVersion = "5.6.2"

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

@@ -24,21 +24,21 @@
 import org.photonvision.common.logging.Logger;
 import org.photonvision.estimation.TargetModel;
 import org.photonvision.estimation.VisionEstimation;
-import org.photonvision.targeting.MultiTargetPNPResults;
+import org.photonvision.targeting.MultiTargetPNPResult;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.pipe.CVPipe;
 import org.photonvision.vision.target.TrackedTarget;
 
 /** Estimate the camera pose given multiple Apriltag observations */
 public class MultiTargetPNPPipe
         extends CVPipe<
-                List<TrackedTarget>, MultiTargetPNPResults, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
+                List<TrackedTarget>, MultiTargetPNPResult, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
     private static final Logger logger = new Logger(MultiTargetPNPPipe.class, LogGroup.VisionModule);
 
     private boolean hasWarned = false;
 
     @Override
-    protected MultiTargetPNPResults process(List<TrackedTarget> targetList) {
+    protected MultiTargetPNPResult process(List<TrackedTarget> targetList) {
         if (params == null
                 || params.cameraCoefficients == null
                 || params.cameraCoefficients.getCameraIntrinsicsMat() == null
@@ -48,13 +48,13 @@ protected MultiTargetPNPResults process(List<TrackedTarget> targetList) {
                         "Cannot perform solvePNP an uncalibrated camera! Please calibrate this resolution...");
                 hasWarned = true;
             }
-            return new MultiTargetPNPResults();
+            return new MultiTargetPNPResult();
         }
 
         return calculateCameraInField(targetList);
     }
 
-    private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetList) {
+    private MultiTargetPNPResult calculateCameraInField(List<TrackedTarget> targetList) {
         // Find tag IDs that exist in the tag layout
         var tagIDsUsed = new ArrayList<Integer>();
         for (var target : targetList) {
@@ -64,7 +64,7 @@ private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetL
 
         // Only run with multiple targets
         if (tagIDsUsed.size() < 2) {
-            return new MultiTargetPNPResults();
+            return new MultiTargetPNPResult();
         }
 
         var estimatedPose =
@@ -75,7 +75,7 @@ private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetL
                         params.atfl,
                         params.targetModel);
 
-        return new MultiTargetPNPResults(estimatedPose, tagIDsUsed);
+        return new MultiTargetPNPResult(estimatedPose, tagIDsUsed);
     }
 
     public static class MultiTargetPNPPipeParams {

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

@@ -31,7 +31,7 @@
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.estimation.TargetModel;
-import org.photonvision.targeting.MultiTargetPNPResults;
+import org.photonvision.targeting.MultiTargetPNPResult;
 import org.photonvision.vision.apriltag.AprilTagFamily;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameThresholdType;
@@ -149,7 +149,7 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
         }
 
         // Do multi-tag pose estimation
-        MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
+        MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
         if (settings.solvePNPEnabled && settings.doMultiTarget) {
             var multiTagOutput = multiTagPNPPipe.run(targetList);
             sumPipeNanosElapsed += multiTagOutput.nanosElapsed;

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

@@ -47,7 +47,7 @@
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.estimation.TargetModel;
-import org.photonvision.targeting.MultiTargetPNPResults;
+import org.photonvision.targeting.MultiTargetPNPResult;
 import org.photonvision.vision.aruco.ArucoDetectionResult;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameThresholdType;
@@ -170,7 +170,7 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings)
         }
 
         // Do multi-tag pose estimation
-        MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
+        MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
         if (settings.solvePNPEnabled && settings.doMultiTarget) {
             var multiTagOutput = multiTagPNPPipe.run(targetList);
             sumPipeNanosElapsed += multiTagOutput.nanosElapsed;

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

@@ -34,7 +34,7 @@
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.SerializationUtils;
 import org.photonvision.common.util.file.FileUtils;
-import org.photonvision.targeting.MultiTargetPNPResults;
+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 +147,7 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se
                 sumPipeNanosElapsed,
                 fps, // Unused but here in case
                 Collections.emptyList(),
-                new MultiTargetPNPResults(),
+                new MultiTargetPNPResult(),
                 new Frame(
                         new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties));
     }

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

@@ -20,7 +20,7 @@
 import java.util.Collections;
 import java.util.List;
 import org.photonvision.common.util.math.MathUtils;
-import org.photonvision.targeting.MultiTargetPNPResults;
+import org.photonvision.targeting.MultiTargetPNPResult;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.opencv.Releasable;
 import org.photonvision.vision.target.TrackedTarget;
@@ -31,23 +31,23 @@ public class CVPipelineResult implements Releasable {
     public final double fps;
     public final List<TrackedTarget> targets;
     public final Frame inputAndOutputFrame;
-    public MultiTargetPNPResults multiTagResult;
+    public MultiTargetPNPResult multiTagResult;
 
     public CVPipelineResult(
             double processingNanos, double fps, List<TrackedTarget> targets, Frame inputFrame) {
-        this(processingNanos, fps, targets, new MultiTargetPNPResults(), inputFrame);
+        this(processingNanos, fps, targets, new MultiTargetPNPResult(), inputFrame);
     }
 
     public CVPipelineResult(
             double processingNanos,
             double fps,
             List<TrackedTarget> targets,
-            MultiTargetPNPResults multiTagResults,
+            MultiTargetPNPResult multiTagResult,
             Frame inputFrame) {
         this.processingNanos = processingNanos;
         this.fps = fps;
         this.targets = targets != null ? targets : Collections.emptyList();
-        this.multiTagResult = multiTagResults;
+        this.multiTagResult = multiTagResult;
 
         this.inputAndOutputFrame = inputFrame;
     }
@@ -56,8 +56,8 @@ public CVPipelineResult(
             double processingNanos,
             double fps,
             List<TrackedTarget> targets,
-            MultiTargetPNPResults multiTagResults) {
-        this(processingNanos, fps, targets, multiTagResults, null);
+            MultiTargetPNPResult multiTagResult) {
+        this(processingNanos, fps, targets, multiTagResult, null);
     }
 
     public boolean hasTargets() {

photon-lib/build.gradle

@@ -46,7 +46,7 @@ dependencies {
     implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get()
 
     implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get()
-    implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get();
+    implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get()
 
     // Junit
     testImplementation wpilibTools.deps.wpilibJava("cscore")

photon-lib/publish.gradle

@@ -152,7 +152,7 @@ publishing {
 
 tasks.withType(PublishToMavenRepository) {
     doFirst {
-        println("Publishing to " + repository.url);
+        println("Publishing to " + repository.url)
     }
 }
 

photon-lib/src/main/java/org/photonvision/PhotonCamera.java

@@ -24,8 +24,6 @@
 
 package org.photonvision;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -43,7 +41,6 @@
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.ProtobufSubscriber;
 import edu.wpi.first.networktables.PubSubOption;
-import edu.wpi.first.networktables.StringPublisher;
 import edu.wpi.first.networktables.StringSubscriber;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
@@ -72,9 +69,8 @@ public class PhotonCamera implements AutoCloseable {
     IntegerPublisher pipelineIndexRequest, ledModeRequest;
     IntegerSubscriber pipelineIndexState, ledModeState;
     IntegerSubscriber heartbeatEntry;
-    private DoubleArraySubscriber cameraIntrinsicsSubscriber;
-    private DoubleArraySubscriber cameraDistortionSubscriber;
-    private StringPublisher atflPublisher;
+    DoubleArraySubscriber cameraIntrinsicsSubscriber;
+    DoubleArraySubscriber cameraDistortionSubscriber;
 
     @Override
     public void close() {
@@ -98,7 +94,6 @@ public void close() {
         pipelineIndexRequest.close();
         cameraIntrinsicsSubscriber.close();
         cameraDistortionSubscriber.close();
-        atflPublisher.close();
     }
 
     private final String path;
@@ -116,12 +111,6 @@ public static void setVersionCheckEnabled(boolean enabled) {
         VERSION_CHECK_ENABLED = enabled;
     }
 
-    private static AprilTagFieldLayout lastSetTagLayout =
-            AprilTagFields.kDefaultField.loadAprilTagLayoutField();
-
-    // Existing is enough to make this multisubscriber do its thing
-    private final MultiSubscriber m_topicNameSubscriber;
-
     /**
      * Constructs a PhotonCamera from a root table.
      *
@@ -159,11 +148,8 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) {
         ledModeState = photonvision_root_table.getIntegerTopic("ledModeState").subscribe(-1);
         versionEntry = photonvision_root_table.getStringTopic("version").subscribe("");
 
-        atflPublisher = photonvision_root_table.getStringTopic("apriltag_field_layout").publish();
-        // Save the layout locally on Rio; on reboot, should be pushed out to NT clients
-        atflPublisher.getTopic().setPersistent(true);
-
-        m_topicNameSubscriber =
+        // Existing is enough to make this multisubscriber do its thing
+        MultiSubscriber m_topicNameSubscriber =
                 new MultiSubscriber(
                         instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true));
     }
@@ -323,46 +309,6 @@ public boolean isConnected() {
         return (now - prevHeartbeatChangeTime) < HEARTBEAT_DEBOUNCE_SEC;
     }
 
-    // TODO: Implement ATFL subscribing in backend
-    // /**
-    //  * Get the last set {@link AprilTagFieldLayout}. The tag layout is shared between all
-    // PhotonVision
-    //  * coprocessors on the same NT instance-- this method returns the most recent layout set. If no
-    //  * layout has been set by the user, this is equal to {@link AprilTagFields#kDefaultField}.
-    //  *
-    //  * @return The last set layout
-    //  */
-    // public AprilTagFieldLayout getAprilTagFieldLayout() {
-    //     return lastSetTagLayout;
-    // }
-
-    // /**
-    //  * Set the {@link AprilTagFieldLayout} used by all PhotonVision coprocessors that are (or might
-    //  * later) connect to this robot. The topic is marked as persistent, so even if you only call
-    // this
-    //  * once ever, it will be saved on the RoboRIO and pushed out to all NT clients when code
-    // reboots.
-    //  * PhotonVision will also store a copy of this layout locally on the coprocessor, but
-    // subscribes
-    //  * to this topic and the local copy will be updated when this function is called.
-    //  *
-    //  * @param layout The layout to use for *all* PhotonVision cameras
-    //  * @return Success of serializing the JSON. This does *not* mean that all PhotonVision clients
-    //  *     have updated their internal layouts.
-    //  */
-    // public boolean setAprilTagFieldLayout(AprilTagFieldLayout layout) {
-    //     try {
-    //         var layout_json = new ObjectMapper().writeValueAsString(layout);
-    //         atflPublisher.set(layout_json);
-    //         lastSetTagLayout = layout;
-    //         return true;
-    //     } catch (JsonProcessingException e) {
-    //         MathSharedStore.reportError("Error setting ATFL in " + this.getName(),
-    // e.getStackTrace());
-    //     }
-    //     return false;
-    // }
-
     public Optional<Matrix<N3, N3>> getCameraMatrix() {
         var cameraMatrix = cameraIntrinsicsSubscriber.get();
         if (cameraMatrix != null && cameraMatrix.length == 9) {

photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

@@ -443,15 +443,15 @@ private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
             return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
         }
 
-        var pnpResults =
+        var pnpResult =
                 VisionEstimation.estimateCamPosePNP(
                         cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
         // try fallback strategy if solvePNP fails for some reason
-        if (!pnpResults.isPresent)
+        if (!pnpResult.isPresent)
             return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
         var best =
                 new Pose3d()
-                        .plus(pnpResults.best) // field-to-camera
+                        .plus(pnpResult.best) // field-to-camera
                         .plus(robotToCamera.inverse()); // field-to-robot
 
         return Optional.of(

photon-lib/src/main/java/org/photonvision/PhotonUtils.java

@@ -201,7 +201,7 @@ public static Rotation2d getYawToPose(Pose2d robotPose, Pose2d targetPose) {
      *
      * @param robotPose Pose of the robot
      * @param targetPose Pose of the target
-     * @return
+     * @return distance to the pose
      */
     public static double getDistanceToPose(Pose2d robotPose, Pose2d targetPose) {
         return robotPose.getTranslation().getDistance(targetPose.getTranslation());

photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java

@@ -53,8 +53,8 @@
 import org.photonvision.estimation.RotTrlTransform3d;
 import org.photonvision.estimation.TargetModel;
 import org.photonvision.estimation.VisionEstimation;
-import org.photonvision.targeting.MultiTargetPNPResults;
-import org.photonvision.targeting.PNPResults;
+import org.photonvision.targeting.MultiTargetPNPResult;
+import org.photonvision.targeting.PNPResult;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
@@ -79,7 +79,8 @@ public class PhotonCameraSim implements AutoCloseable {
     private double minTargetAreaPercent;
     private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest;
 
-    private AprilTagFieldLayout tagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
+    private final AprilTagFieldLayout tagLayout =
+            AprilTagFields.kDefaultField.loadAprilTagLayoutField();
 
     // video stream simulation
     private final CvSource videoSimRaw;
@@ -418,7 +419,7 @@ public PhotonPipelineResult process(
             // projected target can't be detected, skip to next
             if (!(canSeeCorners(noisyTargetCorners) && areaPercent >= minTargetAreaPercent)) continue;
 
-            var pnpSim = new PNPResults();
+            var pnpSim = new PNPResult();
             if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP
                 pnpSim =
                         OpenCVHelp.solvePNP_SQUARE(
@@ -515,21 +516,21 @@ public PhotonPipelineResult process(
         } else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose);
 
         // calculate multitag results
-        var multitagResults = new MultiTargetPNPResults();
+        var multitagResult = new MultiTargetPNPResult();
         // TODO: Implement ATFL subscribing in backend
         // var tagLayout = cam.getAprilTagFieldLayout();
         var visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(detectableTgts, tagLayout);
         if (visibleLayoutTags.size() > 1) {
             List<Integer> usedIDs =
                     visibleLayoutTags.stream().map(t -> t.ID).sorted().collect(Collectors.toList());
-            var pnpResults =
+            var pnpResult =
                     VisionEstimation.estimateCamPosePNP(
                             prop.getIntrinsics(),
                             prop.getDistCoeffs(),
                             detectableTgts,
                             tagLayout,
                             TargetModel.kAprilTag16h5);
-            multitagResults = new MultiTargetPNPResults(pnpResults, usedIDs);
+            multitagResult = new MultiTargetPNPResult(pnpResult, usedIDs);
         }
 
         // sort target order
@@ -538,7 +539,7 @@ public PhotonPipelineResult process(
         }
 
         // put this simulated data to NT
-        return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResults);
+        return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResult);
     }
 
     /**