WPI Image Test Mode (#81)

* Add camera config for calibration to test mode

* Add 3D capabilities to test mode

* Add resources to main for jar testmode

* Refactor path utilities for test mode

* Apply spotless

photon-server/src/main/java/org/photonvision/common/util/TestUtils.java

@@ -17,6 +17,7 @@
 
 package org.photonvision.common.util;
 
+import com.fasterxml.jackson.databind.ObjectMapper;
 import edu.wpi.cscore.CameraServerCvJNI;
 import java.awt.*;
 import java.io.File;
@@ -24,6 +25,7 @@ import java.io.IOException;
 import java.nio.file.Path;
 import org.opencv.core.Mat;
 import org.opencv.highgui.HighGui;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 
 public class TestUtils {
 
@@ -39,7 +41,9 @@ public class TestUtils {
         kCargoStraightDark48in(1.2192),
         kCargoStraightDark72in(1.8288),
         kCargoStraightDark72in_HighRes(1.8288),
-        kCargoStraightDark90in(2.286);
+        kCargoStraightDark90in(2.286),
+        kRocketPanelAngleDark48in(1.2192),
+        kRocketPanelAngleDark60in(1.524);
 
         public static double FOV = 68.5;
 
@@ -130,44 +134,82 @@ public class TestUtils {
         }
     }
 
-    private static Path getResourcesFolderPath() {
-        return Path.of("src", "test", "resources").toAbsolutePath();
+    private static Path getResourcesFolderPath(boolean testMode) {
+        return Path.of("src", (testMode ? "main" : "test"), "resources").toAbsolutePath();
     }
 
-    public static Path getTestImagesPath() {
-        return getResourcesFolderPath().resolve("testimages");
+    public static Path getTestMode2019ImagePath() {
+        return getResourcesFolderPath(true)
+                .resolve("testimages")
+                .resolve(WPI2019Image.kRocketPanelAngleDark60in.path);
     }
 
-    public static Path getCalibrationPath() {
-        return getResourcesFolderPath().resolve("calibration");
+    public static Path getTestMode2020ImagePath() {
+        return getResourcesFolderPath(true)
+                .resolve("testimages")
+                .resolve(WPI2020Image.kBlueGoal_108in_Center.path);
     }
 
-    public static Path getPowercellPath() {
-        return getTestImagesPath().resolve("polygons").resolve("powercells");
+    public static Path getTestImagesPath(boolean testMode) {
+        return getResourcesFolderPath(testMode).resolve("testimages");
     }
 
-    public static Path getWPIImagePath(WPI2020Image image) {
-        return getTestImagesPath().resolve(image.path);
+    public static Path getCalibrationPath(boolean testMode) {
+        return getResourcesFolderPath(testMode).resolve("calibration");
     }
 
-    public static Path getWPIImagePath(WPI2019Image image) {
-        return getTestImagesPath().resolve(image.path);
+    public static Path getPowercellPath(boolean testMode) {
+        return getTestImagesPath(testMode).resolve("polygons").resolve("powercells");
     }
 
-    public static Path getPolygonImagePath(PolygonTestImages image) {
-        return getTestImagesPath().resolve(image.path);
+    public static Path getWPIImagePath(WPI2020Image image, boolean testMode) {
+        return getTestImagesPath(testMode).resolve(image.path);
     }
 
-    public static Path getPowercellImagePath(PowercellTestImages image) {
-        return getPowercellPath().resolve(image.path);
+    public static Path getWPIImagePath(WPI2019Image image, boolean testMode) {
+        return getTestImagesPath(testMode).resolve(image.path);
+    }
+
+    public static Path getPolygonImagePath(PolygonTestImages image, boolean testMode) {
+        return getTestImagesPath(testMode).resolve(image.path);
+    }
+
+    public static Path getPowercellImagePath(PowercellTestImages image, boolean testMode) {
+        return getPowercellPath(testMode).resolve(image.path);
     }
 
     public static Path getDotBoardImagesPath() {
-        return getResourcesFolderPath().resolve("calibrationBoardImages");
+        return getResourcesFolderPath(false).resolve("calibrationBoardImages");
     }
 
     public static File getHardwareConfigJson() {
-        return getResourcesFolderPath().resolve("hardware").resolve("HardwareConfig.json").toFile();
+        return getResourcesFolderPath(false)
+                .resolve("hardware")
+                .resolve("HardwareConfig.json")
+                .toFile();
+    }
+
+    private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json";
+    private static final String LIFECAM_480P_CAL_FILE = "lifecam480p.json";
+
+    public static CameraCalibrationCoefficients getCoeffs(String filename, boolean testMode) {
+        try {
+            return new ObjectMapper()
+                    .readValue(
+                            (Path.of(getCalibrationPath(testMode).toString(), filename).toFile()),
+                            CameraCalibrationCoefficients.class);
+        } catch (IOException e) {
+            e.printStackTrace();
+            return null;
+        }
+    }
+
+    public static CameraCalibrationCoefficients get2019LifeCamCoeffs(boolean testMode) {
+        return getCoeffs(LIFECAM_240P_CAL_FILE, testMode);
+    }
+
+    public static CameraCalibrationCoefficients get2020LifeCamCoeffs(boolean testMode) {
+        return getCoeffs(LIFECAM_480P_CAL_FILE, testMode);
     }
 
     public static void loadLibraries() {