Add test mode from path (#440)

adds --path to --test-mode

photon-server/src/main/java/org/photonvision/Main.java

@@ -18,7 +18,13 @@
 package org.photonvision;
 
 import edu.wpi.first.cscore.CameraServerCvJNI;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Path;
 import java.util.ArrayList;
+import java.util.List;
+import java.util.Objects;
+import java.util.stream.Collectors;
 import org.apache.commons.cli.*;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.configuration.ConfigManager;
@@ -54,6 +60,7 @@ public class Main {
     private static final boolean isRelease = PhotonVersion.isRelease;
 
     private static boolean isTestMode;
+    private static Path testModeFolder = null;
     private static boolean printDebugLogs;
 
     private static boolean handleArgs(String[] args) throws ParseException {
@@ -66,6 +73,8 @@ public class Main {
                 false,
                 "Run in test mode with 2019 and 2020 WPI field images in place of cameras");
 
+        options.addOption("p", "path", true, "Point test mode to a specific folder");
+
         CommandLineParser parser = new DefaultParser();
         CommandLine cmd = parser.parse(options, args);
 
@@ -82,11 +91,70 @@ public class Main {
             if (cmd.hasOption("test-mode")) {
                 isTestMode = true;
                 logger.info("Running in test mode - Cameras will not be used");
+
+                if (cmd.hasOption("path")) {
+                    Path p = Path.of(cmd.getOptionValue("path"));
+                    logger.info("Loading from Path " + p.toAbsolutePath().toString());
+                    testModeFolder = p;
+                }
             }
         }
         return true;
     }
 
+    private static void addTestModeFromFolder() {
+        ConfigManager.getInstance().load();
+
+        try {
+            var reflective = new ReflectivePipelineSettings();
+            var shape = new ColoredShapePipelineSettings();
+            List<VisionSource> collectedSources =
+                    Files.list(testModeFolder)
+                            .filter(p -> p.toFile().isFile())
+                            .map(
+                                    p -> {
+                                        try {
+                                            var camConf =
+                                                    ConfigManager.getInstance()
+                                                            .getConfig()
+                                                            .getCameraConfigurations()
+                                                            .get(p.getFileName().toString());
+                                            if (camConf == null) {
+                                                camConf =
+                                                        new CameraConfiguration(
+                                                                p.getFileName().toString(), p.toAbsolutePath().toString());
+                                                camConf.FOV = TestUtils.WPI2019Image.FOV; // Good guess?
+
+                                                var pipeSettings = new ReflectivePipelineSettings();
+                                                pipeSettings.pipelineNickname = p.getFileName().toString();
+                                                pipeSettings.outputShowMultipleTargets = true;
+                                                pipeSettings.inputShouldShow = true;
+                                                pipeSettings.outputShouldShow = true;
+
+                                                var psList = new ArrayList<CVPipelineSettings>();
+                                                psList.add(reflective);
+                                                psList.add(shape);
+                                                camConf.pipelineSettings = psList;
+                                            }
+
+                                            return new FileVisionSource(camConf);
+                                        } catch (Exception e) {
+                                            logger.error("Couldn't load image " + p.getFileName().toString());
+                                            return null;
+                                        }
+                                    })
+                            .filter(Objects::nonNull)
+                            .collect(Collectors.toList());
+
+            ConfigManager.getInstance().unloadCameraConfigs();
+            VisionModuleManager.getInstance().addSources(collectedSources).forEach(VisionModule::start);
+            ConfigManager.getInstance().addCameraConfigurations(collectedSources);
+        } catch (IOException e) {
+            logger.error("Path does not exist!");
+            System.exit(1);
+        }
+    }
+
     private static void addTestModeSources() {
         ConfigManager.getInstance().load();
 
@@ -182,13 +250,16 @@ public class Main {
         collectedSources.add(fvs2020);
         collectedSources.add(fvs2019);
 
+        ConfigManager.getInstance().unloadCameraConfigs();
         VisionModuleManager.getInstance().addSources(collectedSources).forEach(VisionModule::start);
         ConfigManager.getInstance().addCameraConfigurations(collectedSources);
     }
 
     public static void main(String[] args) {
         try {
-            if (!handleArgs(args)) return;
+            if (!handleArgs(args)) {
+                System.exit(0);
+            }
         } catch (ParseException e) {
             logger.error("Failed to parse command-line options!", e);
         }
@@ -235,9 +306,14 @@ public class Main {
             VisionSourceManager.getInstance()
                     .registerLoadedConfigs(
                             ConfigManager.getInstance().getConfig().getCameraConfigurations().values());
+
             VisionSourceManager.getInstance().registerTimedTask();
         } else {
-            addTestModeSources();
+            if (testModeFolder == null) {
+                addTestModeSources();
+            } else {
+                addTestModeFromFolder();
+            }
         }
 
         Server.main(DEFAULT_WEBPORT);