Merge branch 'mdurrani808-patch-2' of https://github.com/PhotonVision…

…/photonvision into mdurrani808-patch-2

photon-client/src/components/dashboard/tabs/PnPTab.vue

@@ -26,11 +26,8 @@ const interactiveCols = computed(
         { name: '2020 High Goal Outer', value: TargetModel.InfiniteRechargeHighGoalOuter },
         { name: '2020 Power Cell (7in)', value: TargetModel.CircularPowerCell7in },
         { name: '2022 Cargo Ball (9.5in)', value: TargetModel.RapidReactCircularCargoBall },
-        { name: '200mm AprilTag', value: TargetModel.Apriltag_200mm },
-        { name: '6in (16h5) Aruco', value: TargetModel.Aruco6in_16h5 },
-        { name: '6in (16h5) AprilTag', value: TargetModel.Apriltag6in_16h5 },
-        { name: '6.5in (36h11) Aruco', value: TargetModel.Aruco6p5in_36h11 },
-        { name: '6.5in (36h11) AprilTag', value: TargetModel.Apriltag6p5in_36h11 }
+        { name: '2023 AprilTag 6in (16h5)', value: TargetModel.AprilTag6in_16h5 },
+        { name: '2024 AprilTag 6.5in (36h11)', value: TargetModel.AprilTag6p5in_36h11 }
       ]"
       :select-cols="interactiveCols"
       @input="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ targetModel: value }, false)"

photon-client/src/types/PipelineTypes.ts

@@ -26,11 +26,8 @@ export enum TargetModel {
   InfiniteRechargeHighGoalOuter = 2,
   CircularPowerCell7in = 3,
   RapidReactCircularCargoBall = 4,
-  Apriltag_200mm = 5,
-  Aruco6in_16h5 = 6,
-  Apriltag6in_16h5 = 7,
-  Aruco6p5in_36h11 = 8,
-  Apriltag6p5in_36h11 = 9
+  AprilTag6in_16h5 = 5,
+  AprilTag6p5in_36h11 = 6
 }
 
 export interface PipelineSettings {
@@ -225,7 +222,7 @@ export type ConfigurableAprilTagPipelineSettings = Partial<
 export const DefaultAprilTagPipelineSettings: AprilTagPipelineSettings = {
   ...DefaultPipelineSettings,
   cameraGain: 75,
-  targetModel: TargetModel.Apriltag6in_16h5,
+  targetModel: TargetModel.AprilTag6in_16h5,
   ledMode: false,
   outputShowMultipleTargets: true,
   cameraExposure: 20,
@@ -268,7 +265,7 @@ export type ConfigurableArucoPipelineSettings = Partial<Omit<ArucoPipelineSettin
 export const DefaultArucoPipelineSettings: ArucoPipelineSettings = {
   ...DefaultPipelineSettings,
   outputShowMultipleTargets: true,
-  targetModel: TargetModel.Aruco6in_16h5,
+  targetModel: TargetModel.AprilTag6in_16h5,
   cameraExposure: -1,
   cameraAutoExposure: true,
   ledMode: false,

photon-core/src/main/java/org/photonvision/vision/frame/consumer/MJPGFrameConsumer.java

@@ -28,12 +28,18 @@
 import org.opencv.core.Rect;
 import org.opencv.imgproc.Imgproc;
 import org.photonvision.common.util.ColorHelper;
+import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.opencv.CVMat;
 
-public class MJPGFrameConsumer {
-    public static final Mat EMPTY_MAT = new Mat(60, 15 * 7, CvType.CV_8UC3);
+public class MJPGFrameConsumer implements AutoCloseable {
+    private static final double MAX_FRAMERATE = -1;
+    private static final long MAX_FRAME_PERIOD_NS = Math.round(1e9 / MAX_FRAMERATE);
+    private long lastFrameTimeNs;
+
+    private static final Mat EMPTY_MAT = new Mat(60, 15 * 7, CvType.CV_8UC3);
     private static final double EMPTY_FRAMERATE = 2;
-    private long lastEmptyTime;
+    private static final long EMPTY_FRAME_PERIOD_NS = Math.round(1e9 / EMPTY_FRAMERATE);
+    private long lastEmptyTimeNs;
 
     static {
         EMPTY_MAT.setTo(ColorHelper.colorToScalar(Color.BLACK));
@@ -168,11 +174,15 @@ public MJPGFrameConsumer(String name, int port) {
 
     public void accept(CVMat image) {
         if (image != null && !image.getMat().empty()) {
-            cvSource.putFrame(image.getMat());
+            long now = MathUtils.wpiNanoTime();
+            if (now - lastFrameTimeNs > MAX_FRAME_PERIOD_NS) {
+                lastFrameTimeNs = now;
+                cvSource.putFrame(image.getMat());
+            }
 
             // Make sure our disabled framerate limiting doesn't get confused
             isDisabled = false;
-            lastEmptyTime = 0;
+            lastEmptyTimeNs = 0;
         }
     }
 
@@ -182,9 +192,10 @@ public void disabledTick() {
             isDisabled = true;
         }
 
-        if (System.currentTimeMillis() - lastEmptyTime > 1000.0 / EMPTY_FRAMERATE) {
+        long now = MathUtils.wpiNanoTime();
+        if (now - lastEmptyTimeNs > EMPTY_FRAME_PERIOD_NS) {
+            lastEmptyTimeNs = now;
             cvSource.putFrame(EMPTY_MAT);
-            lastEmptyTime = System.currentTimeMillis();
         }
     }
 
@@ -233,6 +244,7 @@ private static String pixelFormatToString(VideoMode.PixelFormat pixelFormat) {
         }
     }
 
+    @Override
     public void close() {
         table.getEntry("connected").setBoolean(false);
         mjpegServer.close();

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

@@ -22,6 +22,7 @@
 import java.util.List;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
+import org.photonvision.estimation.TargetModel;
 import org.photonvision.estimation.VisionEstimation;
 import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
@@ -38,7 +39,8 @@ public class MultiTargetPNPPipe
 
     @Override
     protected MultiTargetPNPResults process(List<TrackedTarget> targetList) {
-        if (params.cameraCoefficients == null
+        if (params == null
+                || params.cameraCoefficients == null
                 || params.cameraCoefficients.getCameraIntrinsicsMat() == null
                 || params.cameraCoefficients.getDistCoeffsMat() == null) {
             if (!hasWarned) {
@@ -70,19 +72,24 @@ private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetL
                         params.cameraCoefficients.cameraIntrinsics.getAsWpilibMat(),
                         params.cameraCoefficients.distCoeffs.getAsWpilibMat(),
                         TrackedTarget.simpleFromTrackedTargets(targetList),
-                        params.atfl);
+                        params.atfl,
+                        params.targetModel);
 
         return new MultiTargetPNPResults(estimatedPose, tagIDsUsed);
     }
 
     public static class MultiTargetPNPPipeParams {
         private final CameraCalibrationCoefficients cameraCoefficients;
         private final AprilTagFieldLayout atfl;
+        private final TargetModel targetModel;
 
         public MultiTargetPNPPipeParams(
-                CameraCalibrationCoefficients cameraCoefficients, AprilTagFieldLayout atfl) {
+                CameraCalibrationCoefficients cameraCoefficients,
+                AprilTagFieldLayout atfl,
+                TargetModel targetModel) {
             this.cameraCoefficients = cameraCoefficients;
             this.atfl = atfl;
+            this.targetModel = targetModel;
         }
     }
 }

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

@@ -30,6 +30,7 @@
 import java.util.List;
 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.vision.apriltag.AprilTagFamily;
 import org.photonvision.vision.frame.Frame;
@@ -71,13 +72,13 @@ protected void setPipeParamsImpl() {
         settings.threads = Math.max(1, settings.threads);
 
         // for now, hard code tag width based on enum value
-        double tagWidth;
+        // 2023/other: best guess is 6in
+        double tagWidth = Units.inchesToMeters(6);
+        TargetModel tagModel = TargetModel.kAprilTag16h5;
         if (settings.tagFamily == AprilTagFamily.kTag36h11) {
-            // 2024 tag, guess 6.5in
+            // 2024 tag, 6.5in
             tagWidth = Units.inchesToMeters(6.5);
-        } else {
-            // 2023/other: best guess is 6in
-            tagWidth = Units.inchesToMeters(6);
+            tagModel = TargetModel.kAprilTag36h11;
         }
 
         var config = new AprilTagDetector.Config();
@@ -104,7 +105,7 @@ protected void setPipeParamsImpl() {
                 // TODO global state ew
                 var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
                 multiTagPNPPipe.setParams(
-                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl));
+                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl, tagModel));
             }
         }
     }

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

@@ -41,7 +41,7 @@ public AprilTagPipelineSettings() {
         super();
         pipelineType = PipelineType.AprilTag;
         outputShowMultipleTargets = true;
-        targetModel = TargetModel.k6in_16h5;
+        targetModel = TargetModel.kAprilTag6in_16h5;
         cameraExposure = 20;
         cameraAutoExposure = false;
         ledMode = false;

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

@@ -46,6 +46,7 @@
 import org.opencv.objdetect.Objdetect;
 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.vision.aruco.ArucoDetectionResult;
 import org.photonvision.vision.frame.Frame;
@@ -79,9 +80,16 @@ protected void setPipeParamsImpl() {
         var params = new ArucoDetectionPipeParams();
         // sanitize and record settings
 
+        // for now, hard code tag width based on enum value
+        // 2023/other: best guess is 6in
+        double tagWidth = Units.inchesToMeters(6);
+        TargetModel tagModel = TargetModel.kAprilTag16h5;
         switch (settings.tagFamily) {
             case kTag36h11:
+                // 2024 tag, 6.5in
                 params.tagFamily = Objdetect.DICT_APRILTAG_36h11;
+                tagWidth = Units.inchesToMeters(6.5);
+                tagModel = TargetModel.kAprilTag36h11;
                 break;
             case kTag25h9:
                 params.tagFamily = Objdetect.DICT_APRILTAG_25h9;
@@ -113,14 +121,13 @@ protected void setPipeParamsImpl() {
             var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat();
             if (cameraMatrix != null && cameraMatrix.rows() > 0) {
                 var estimatorParams =
-                        new ArucoPoseEstimatorPipeParams(
-                                frameStaticProperties.cameraCalibration, Units.inchesToMeters(6));
+                        new ArucoPoseEstimatorPipeParams(frameStaticProperties.cameraCalibration, tagWidth);
                 singleTagPoseEstimatorPipe.setParams(estimatorParams);
 
                 // TODO global state ew
                 var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
                 multiTagPNPPipe.setParams(
-                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl));
+                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl, tagModel));
             }
         }
     }

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

@@ -46,7 +46,7 @@ public ArucoPipelineSettings() {
         super();
         pipelineType = PipelineType.Aruco;
         outputShowMultipleTargets = true;
-        targetModel = TargetModel.k6in_16h5;
+        targetModel = TargetModel.kAprilTag6in_16h5;
         cameraExposure = -1;
         cameraAutoExposure = true;
         ledMode = false;

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

@@ -107,7 +107,7 @@ protected void setPipeParams(
                 new Draw3dArucoPipe.Draw3dArucoParams(
                         settings.outputShouldDraw,
                         frameStaticProperties.cameraCalibration,
-                        TargetModel.k6in_16h5,
+                        TargetModel.kAprilTag6in_16h5,
                         settings.streamingFrameDivisor);
         draw3dArucoPipe.setParams(draw3dArucoParams);
 

photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java

@@ -17,6 +17,7 @@
 
 package org.photonvision.vision.processes;
 
+import edu.wpi.first.cscore.CameraServerJNI;
 import edu.wpi.first.cscore.VideoException;
 import edu.wpi.first.math.util.Units;
 import io.javalin.websocket.WsContext;
@@ -44,15 +45,14 @@
 import org.photonvision.vision.camera.USBCameraSource;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.consumer.FileSaveFrameConsumer;
+import org.photonvision.vision.frame.consumer.MJPGFrameConsumer;
 import org.photonvision.vision.pipeline.AdvancedPipelineSettings;
 import org.photonvision.vision.pipeline.OutputStreamPipeline;
 import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
 import org.photonvision.vision.pipeline.UICalibrationData;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TargetModel;
 import org.photonvision.vision.target.TrackedTarget;
-import org.photonvision.vision.videoStream.SocketVideoStream;
-import org.photonvision.vision.videoStream.SocketVideoStreamManager;
 
 /**
  * This is the God Class
@@ -83,8 +83,8 @@ public class VisionModule {
     FileSaveFrameConsumer inputFrameSaver;
     FileSaveFrameConsumer outputFrameSaver;
 
-    SocketVideoStream inputVideoStreamer;
-    SocketVideoStream outputVideoStreamer;
+    MJPGFrameConsumer inputVideoStreamer;
+    MJPGFrameConsumer outputVideoStreamer;
 
     public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, int index) {
         logger =
@@ -169,11 +169,6 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource,
         saveAndBroadcastAll();
     }
 
-    private void destroyStreams() {
-        SocketVideoStreamManager.getInstance().removeStream(inputVideoStreamer);
-        SocketVideoStreamManager.getInstance().removeStream(outputVideoStreamer);
-    }
-
     private void createStreams() {
         var camStreamIdx = visionSource.getSettables().getConfiguration().streamIndex;
         // If idx = 0, we want (1181, 1182)
@@ -186,10 +181,13 @@ private void createStreams() {
                 new FileSaveFrameConsumer(
                         visionSource.getSettables().getConfiguration().nickname, "output");
 
-        inputVideoStreamer = new SocketVideoStream(this.inputStreamPort);
-        outputVideoStreamer = new SocketVideoStream(this.outputStreamPort);
-        SocketVideoStreamManager.getInstance().addStream(inputVideoStreamer);
-        SocketVideoStreamManager.getInstance().addStream(outputVideoStreamer);
+        String camHostname = CameraServerJNI.getHostname();
+        inputVideoStreamer =
+                new MJPGFrameConsumer(
+                        camHostname + "_Port_" + inputStreamPort + "_Input_MJPEG_Server", inputStreamPort);
+        outputVideoStreamer =
+                new MJPGFrameConsumer(
+                        camHostname + "_Port_" + outputStreamPort + "_Output_MJPEG_Server", outputStreamPort);
     }
 
     private void recreateStreamResultConsumers() {
@@ -483,16 +481,6 @@ public void setCameraNickname(String newName) {
         inputFrameSaver.updateCameraNickname(newName);
         outputFrameSaver.updateCameraNickname(newName);
 
-        // Rename streams
-        streamResultConsumers.clear();
-
-        // Teardown and recreate streams
-        destroyStreams();
-        createStreams();
-
-        // Rebuild streamers
-        recreateStreamResultConsumers();
-
         // Push new data to the UI
         saveAndBroadcastAll();
     }

photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java

@@ -107,30 +107,18 @@ public enum TargetModel implements Releasable {
                             -Units.inchesToMeters(9.5) / 2,
                             -Units.inchesToMeters(9.5) / 2)),
             0),
-    k200mmAprilTag( // Corners of the tag's inner black square (excluding white border)
-            List.of(
-                    new Point3(Units.inchesToMeters(3.25), Units.inchesToMeters(3.25), 0),
-                    new Point3(-Units.inchesToMeters(3.25), Units.inchesToMeters(3.25), 0),
-                    new Point3(-Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0),
-                    new Point3(Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0)),
-            Units.inchesToMeters(3.25 * 2)),
-    k6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners
-            // do not
+    // 2023 AprilTag, with 6 inch marker width (inner black square).
+    kAprilTag6in_16h5(
+            // Corners of the tag's inner black square (excluding white border)
             List.of(
                     new Point3(Units.inchesToMeters(3), Units.inchesToMeters(3), 0),
                     new Point3(-Units.inchesToMeters(3), Units.inchesToMeters(3), 0),
                     new Point3(-Units.inchesToMeters(3), -Units.inchesToMeters(3), 0),
                     new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)),
             Units.inchesToMeters(3 * 2)),
-    // 2024 FRC tag. 6.5in inner tag, 8.125 overall
-    kAruco6p5in_36h11(
-            List.of(
-                    new Point3(Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0),
-                    new Point3(Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0),
-                    new Point3(-Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0),
-                    new Point3(Units.inchesToMeters(6.5 / 2.0), -Units.inchesToMeters(6.5 / 2.0), 0)),
-            Units.inchesToMeters(6.5)),
-    k6p5in_36h11(
+    // 2024 AprilTag, with 6.5 inch marker width (inner black square).
+    kAprilTag6p5in_36h11(
+            // Corners of the tag's inner black square (excluding white border)
             List.of(
                     new Point3(-Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0),
                     new Point3(Units.inchesToMeters(6.5 / 2.0), Units.inchesToMeters(6.5 / 2.0), 0),