diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index a13723780..3810b2903 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -34,9 +34,9 @@ import org.photonvision.common.networking.NetworkManager; import org.photonvision.common.util.TestUtils; import org.photonvision.server.Server; import org.photonvision.vision.camera.FileVisionSource; -import org.photonvision.vision.camera.USBCameraSource; import org.photonvision.vision.pipeline.CVPipelineSettings; import org.photonvision.vision.pipeline.ReflectivePipelineSettings; +import org.photonvision.vision.processes.VisionModule; import org.photonvision.vision.processes.VisionModuleManager; import org.photonvision.vision.processes.VisionSource; import org.photonvision.vision.processes.VisionSourceManager; @@ -84,58 +84,44 @@ public class Main { return true; } - private static HashMap> gatherSources() { + private static void addTestModeSources() { var collectedSources = new HashMap>(); - if (!isTestMode) { - var camConfigs = ConfigManager.getInstance().getConfig().getCameraConfigurations(); - logger.info("Loaded " + camConfigs.size() + " configs from disk."); - var sources = VisionSourceManager.loadAllSources(camConfigs.values()); - for (var src : sources) { - var usbSrc = (USBCameraSource) src; - collectedSources.put(usbSrc, usbSrc.configuration.pipelineSettings); - logger.debug( - () -> - "Matched config for camera \"" - + src.getFrameProvider().getName() - + "\" and loaded " - + usbSrc.configuration.pipelineSettings.size() - + " pipelines"); - } - } else { - var camConf2019 = - new CameraConfiguration("WPI2019", TestUtils.getTestMode2019ImagePath().toString()); - camConf2019.FOV = TestUtils.WPI2019Image.FOV; - camConf2019.calibrations.add(TestUtils.get2019LifeCamCoeffs(true)); + var camConf2019 = + new CameraConfiguration("WPI2019", TestUtils.getTestMode2019ImagePath().toString()); + camConf2019.FOV = TestUtils.WPI2019Image.FOV; + camConf2019.calibrations.add(TestUtils.get2019LifeCamCoeffs(true)); - var pipeline2019 = new ReflectivePipelineSettings(); - pipeline2019.pipelineNickname = "CargoShip"; - pipeline2019.targetModel = TargetModel.get2019Target(); + var pipeline2019 = new ReflectivePipelineSettings(); + pipeline2019.pipelineNickname = "CargoShip"; + pipeline2019.targetModel = TargetModel.get2019Target(); - var psList2019 = new ArrayList(); - psList2019.add(pipeline2019); + var psList2019 = new ArrayList(); + psList2019.add(pipeline2019); - var fvs2019 = new FileVisionSource(camConf2019); + var fvs2019 = new FileVisionSource(camConf2019); - var camConf2020 = - new CameraConfiguration("WPI2020", TestUtils.getTestMode2020ImagePath().toString()); - camConf2020.FOV = TestUtils.WPI2020Image.FOV; - camConf2019.calibrations.add(TestUtils.get2019LifeCamCoeffs(true)); + var camConf2020 = + new CameraConfiguration("WPI2020", TestUtils.getTestMode2020ImagePath().toString()); + camConf2020.FOV = TestUtils.WPI2020Image.FOV; + camConf2019.calibrations.add(TestUtils.get2019LifeCamCoeffs(true)); - var pipeline2020 = new ReflectivePipelineSettings(); - pipeline2020.pipelineNickname = "OuterPort"; - pipeline2020.targetModel = TargetModel.get2020Target(); - camConf2019.calibrations.add(TestUtils.get2019LifeCamCoeffs(true)); + var pipeline2020 = new ReflectivePipelineSettings(); + pipeline2020.pipelineNickname = "OuterPort"; + pipeline2020.targetModel = TargetModel.get2020Target(); + camConf2020.calibrations.add(TestUtils.get2020LifeCamCoeffs(true)); - var psList2020 = new ArrayList(); - psList2020.add(pipeline2020); + var psList2020 = new ArrayList(); + psList2020.add(pipeline2020); - var fvs2020 = new FileVisionSource(camConf2020); + var fvs2020 = new FileVisionSource(camConf2020); - collectedSources.put(fvs2019, psList2019); - collectedSources.put(fvs2020, psList2020); - } - return collectedSources; + collectedSources.put(fvs2019, psList2019); + collectedSources.put(fvs2020, psList2020); + + // logger.info("Adding " + allSources.size() + " configs to VMM."); + VisionModuleManager.getInstance().addSources(collectedSources).forEach(VisionModule::start); + ConfigManager.getInstance().addCameraConfigurations(collectedSources); } public static void main(String[] args) { @@ -145,7 +131,7 @@ public class Main { logger.error("Failed to parse command-line options!", e); } - System.out.println("Running in " + (isRelease ? "release" : "development") + " mode!"); + logger.info("Running in " + (isRelease ? "release" : "development") + " mode!"); var logLevel = (isRelease || printDebugLogs) ? LogLevel.INFO : LogLevel.DEBUG; Logger.setLevel(LogGroup.Camera, logLevel); Logger.setLevel(LogGroup.WebServer, logLevel); @@ -169,22 +155,32 @@ public class Main { } ConfigManager.getInstance().load(); // init config manager + ConfigManager.getInstance().requestSave(); + NetworkManager.getInstance().initialize(false); NetworkTablesManager.getInstance() .setConfig(ConfigManager.getInstance().getConfig().getNetworkConfig()); - HashMap> allSources = gatherSources(); + // HashMap> allSources = gatherSources(); - logger.info("Adding " + allSources.size() + " configs to VMM."); - VisionModuleManager.getInstance().addSources(allSources); - ConfigManager.getInstance().addCameraConfigurations(allSources); + // logger.info("Adding " + allSources.size() + " configs to VMM."); + // VisionModuleManager.getInstance().addSources(allSources); + // ConfigManager.getInstance().addCameraConfigurations(allSources); + + if (!isTestMode) { + VisionSourceManager.getInstance() + .registerLoadedConfigs( + ConfigManager.getInstance().getConfig().getCameraConfigurations().values()); + VisionSourceManager.getInstance().registerTimedTask(); + } else { + addTestModeSources(); + } // Add hardware config to hardware manager HardwareManager.getInstance() .setConfig(ConfigManager.getInstance().getConfig().getHardwareConfig()); - VisionModuleManager.getInstance().startModules(); Server.main(DEFAULT_WEBPORT); } } diff --git a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java index a375a7e71..1d60f2f63 100644 --- a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -29,7 +29,6 @@ import org.photonvision.common.dataflow.DataChangeService; import org.photonvision.common.dataflow.events.OutgoingUIEvent; import org.photonvision.common.dataflow.networktables.NTDataPublisher; import org.photonvision.common.dataflow.websocket.UIDataPublisher; -import org.photonvision.common.hardware.HardwareManager; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.SerializationUtils; @@ -131,6 +130,8 @@ public class VisionModule { logger.info("Setting FOV of vendor camera to " + fov); visionSource.getSettables().setFOV(fov); } + + saveAndBroadcastAll(); } void setDriverMode(boolean isDriverMode) { @@ -312,7 +313,7 @@ public class VisionModule { ret.calibrations = calList; ret.isFovConfigurable = - !(HardwareManager.getInstance().getConfig().hasPresetFOV() + !(ConfigManager.getInstance().getConfig().getHardwareConfig().hasPresetFOV() && cameraQuirks.hasQuirk(CameraQuirk.PiCam)); return ret; diff --git a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java index 52f21ec38..4b567751f 100644 --- a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java +++ b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java @@ -52,20 +52,16 @@ public class VisionModuleManager { return visionModules.get(i); } - public void addSources(HashMap> visionSources) { + public List addSources( + HashMap> visionSources) { + var addedModules = new ArrayList(); for (var entry : visionSources.entrySet()) { var visionSource = entry.getKey(); var pipelineManager = new PipelineManager(entry.getValue()); var module = new VisionModule(pipelineManager, visionSource, visionModules.size()); visionModules.add(module); - // todo: logging - } - } - - public void startModules() { - for (var visionModule : visionModules) { - visionModule.start(); - // todo: logging + addedModules.add(module); } + return addedModules; } } diff --git a/photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java b/photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java index a6744b962..2d0138fc6 100644 --- a/photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java +++ b/photon-server/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java @@ -20,67 +20,150 @@ package org.photonvision.vision.processes; import edu.wpi.cscore.UsbCamera; import edu.wpi.cscore.UsbCameraInfo; import java.util.*; -import java.util.stream.Collectors; +import java.util.concurrent.CopyOnWriteArrayList; +import java.util.function.Supplier; import org.photonvision.common.configuration.CameraConfiguration; +import org.photonvision.common.configuration.ConfigManager; +import org.photonvision.common.dataflow.DataChangeService; +import org.photonvision.common.dataflow.events.OutgoingUIEvent; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.TimedTaskManager; import org.photonvision.vision.camera.CameraType; import org.photonvision.vision.camera.USBCameraSource; +import org.photonvision.vision.pipeline.CVPipelineSettings; public class VisionSourceManager { private static final Logger logger = new Logger(VisionSourceManager.class, LogGroup.Camera); private static final List deviceBlacklist = List.of("bcm2835-isp"); - /** - * Load vision sources based on currently connected hardware. - * - * @param loadedConfigs The {@link CameraConfiguration}s loaded from disk. - */ - public static List loadAllSources(Collection loadedConfigs) { - return loadAllSources( - loadedConfigs, filterAllowedDevices(Arrays.asList(UsbCamera.enumerateUsbCameras()))); + final List knownUsbCameras = new CopyOnWriteArrayList<>(); + final List unmatchedLoadedConfigs = new CopyOnWriteArrayList<>(); + + private static class SingletonHolder { + private static final VisionSourceManager INSTANCE = new VisionSourceManager(); + } + + public static VisionSourceManager getInstance() { + return SingletonHolder.INSTANCE; + } + + VisionSourceManager() {} + + public void registerTimedTask() { + TimedTaskManager.getInstance().addTask("VisionSourceManager", this::tryMatchUSBCams, 3000); + } + + public void registerLoadedConfigs(CameraConfiguration... configs) { + registerLoadedConfigs(Arrays.asList(configs)); } /** - * Load vision sources based on given cameras and configs. + * Register new camera configs loaded from disk. This will add them to the list of configs to try + * to match, and also automatically spawn new vision processes as necessary. * - * @param loadedConfigs The configs loaded from disk. - * @param detectedCamInfos The cameras to attempt connection to. + * @param configs The loaded camera configs. */ - public static List loadAllSources( - Collection loadedConfigs, List detectedCamInfos) { - var loadedUsbCamConfigs = - loadedConfigs.stream() - .filter(configuration -> configuration.cameraType == CameraType.UsbCamera) - .collect(Collectors.toList()); - var matchedCameras = matchUSBCameras(detectedCamInfos, loadedUsbCamConfigs); - - // turn the matched cameras into VisionSources - return loadVisionSourcesFromCamConfigs(matchedCameras); + public void registerLoadedConfigs(Collection configs) { + unmatchedLoadedConfigs.addAll(configs); } - private static List filterAllowedDevices(List allDevices) { - List filteredDevices = new ArrayList<>(); - for (var device : allDevices) { - var deviceInfoStr = - "\"" - + device.name - + "\" at \"" - + device.path - + "\" with USB ID \"" - + device.vendorId - + ":" - + device.productId - + "\""; - if (deviceBlacklist.contains(device.name)) { - logger.info("Skipping blacklisted device - " + deviceInfoStr); - } else { - filteredDevices.add(device); - logger.info("Adding local video device - " + deviceInfoStr); + protected Supplier> cameraInfoSupplier = + () -> List.of(UsbCamera.enumerateUsbCameras()); + + protected void tryMatchUSBCams() { + var visionSourceMap = tryMatchUSBCamImpl(); + if (visionSourceMap == null) return; + + logger.info("Adding " + visionSourceMap.size() + " configs to VMM."); + ConfigManager.getInstance().addCameraConfigurations(visionSourceMap); + var addedSources = VisionModuleManager.getInstance().addSources(visionSourceMap); + addedSources.forEach(VisionModule::start); + DataChangeService.getInstance() + .publishEvent( + new OutgoingUIEvent<>( + "fullsettings", ConfigManager.getInstance().getConfig().toHashMap())); + } + + protected HashMap> tryMatchUSBCamImpl() { + // Detect cameras using CSCore + List connectedCameras = + new ArrayList<>(filterAllowedDevices(cameraInfoSupplier.get())); + + // Remove all known devices + var notYetLoadedCams = new ArrayList(); + for (var connectedCam : connectedCameras) { + boolean cameraIsUnknown = true; + for (UsbCameraInfo knownCam : this.knownUsbCameras) { + if (usbCamEquals(knownCam, connectedCam)) { + cameraIsUnknown = false; + break; + } + } + if (cameraIsUnknown) { + notYetLoadedCams.add(connectedCam); } } - return filteredDevices; + + if (notYetLoadedCams.isEmpty()) return null; + + if (connectedCameras.isEmpty()) { + logger.warn( + "No USB cameras were detected! Check that all cameras are connected, and that the path is correct."); + return null; + } + logger.debug("Matching " + notYetLoadedCams.size() + " new cameras!"); + + // Sort out just the USB cams + var usbCamConfigs = new ArrayList<>(); + for (var config : unmatchedLoadedConfigs) { + if (config.cameraType == CameraType.UsbCamera) usbCamConfigs.add(config); + } + + // Debug prints + for (var info : notYetLoadedCams) { + logger.info("Adding local video device - \"" + info.name + "\" at \"" + info.path + "\""); + } + + if (!usbCamConfigs.isEmpty()) + logger.debug("Trying to match " + usbCamConfigs.size() + " unmatched configs..."); + + // Match camera configs to physical cameras + var matchedCameras = matchUSBCameras(notYetLoadedCams, unmatchedLoadedConfigs); + unmatchedLoadedConfigs.removeAll(matchedCameras); + if (!unmatchedLoadedConfigs.isEmpty()) + logger.warn( + () -> + "After matching, " + + unmatchedLoadedConfigs.size() + + " configs remained unmatched. Is your camera disconnected?"); + + // We add the matched cameras to the known camera list + for (var cam : notYetLoadedCams) { + if (this.knownUsbCameras.stream().noneMatch(it -> usbCamEquals(it, cam))) { + this.knownUsbCameras.add(cam); + } + } + if (matchedCameras.isEmpty()) return null; + + // Turn these camera configs into vision sources + var sources = loadVisionSourcesFromCamConfigs(matchedCameras); + + // These sources can be turned into USB cameras, which can be added to the config manager + var visionSourceMap = new HashMap>(); + for (var src : sources) { + var usbSrc = (USBCameraSource) src; + visionSourceMap.put(usbSrc, usbSrc.configuration.pipelineSettings); + logger.debug( + () -> + "Matched config for camera \"" + + src.getFrameProvider().getName() + + "\" and loaded " + + usbSrc.configuration.pipelineSettings.size() + + " pipelines"); + } + return visionSourceMap; } /** @@ -89,11 +172,12 @@ public class VisionSourceManager { * * @param detectedCamInfos Information about currently connected USB cameras. * @param loadedUsbCamConfigs The USB {@link CameraConfiguration}s loaded from disk. + * @return the matched configurations. */ - private static List matchUSBCameras( + private List matchUSBCameras( List detectedCamInfos, List loadedUsbCamConfigs) { - ArrayList detectedCameraList = new ArrayList<>(detectedCamInfos); - List cameraConfigurations = new ArrayList<>(); + var detectedCameraList = new ArrayList<>(detectedCamInfos); + ArrayList cameraConfigurations = new ArrayList<>(); // loop over all the configs loaded from disk for (CameraConfiguration config : loadedUsbCamConfigs) { @@ -160,6 +244,35 @@ public class VisionSourceManager { return cameraConfigurations; } + private List loadVisionSourcesFromCamConfigs(List camConfigs) { + List usbCameraSources = new ArrayList<>(); + camConfigs.forEach(configuration -> usbCameraSources.add(new USBCameraSource(configuration))); + return usbCameraSources; + } + + private List filterAllowedDevices(List allDevices) { + List filteredDevices = new ArrayList<>(); + for (var device : allDevices) { + if (deviceBlacklist.contains(device.name)) { + logger.trace( + "Skipping blacklisted device: \"" + device.name + "\" at \"" + device.path + "\""); + } else { + filteredDevices.add(device); + logger.trace( + "Adding local video device - \"" + device.name + "\" at \"" + device.path + "\""); + } + } + return filteredDevices; + } + + private boolean usbCamEquals(UsbCameraInfo a, UsbCameraInfo b) { + return a.path.equals(b.path) + && a.dev == b.dev + && a.name.equals(b.name) + && a.productId == b.productId + && a.vendorId == b.vendorId; + } + // Remove all non-ASCII characters private static String cameraNameToBaseName(String cameraName) { return cameraName.replaceAll("[^\\x00-\\x7F]", ""); @@ -170,13 +283,6 @@ public class VisionSourceManager { return baseName.replaceAll(" ", "_"); } - private static List loadVisionSourcesFromCamConfigs( - List camConfigs) { - List usbCameraSources = new ArrayList<>(); - camConfigs.forEach(configuration -> usbCameraSources.add(new USBCameraSource(configuration))); - return usbCameraSources; - } - /** * Check if a given config list contains the given unique name. * @@ -184,7 +290,7 @@ public class VisionSourceManager { * @param uniqueName The unique name. * @return If the list of configs contains the unique name. */ - private static boolean containsName( + private boolean containsName( final List configList, final String uniqueName) { return configList.stream() .anyMatch(configuration -> configuration.uniqueName.equals(uniqueName)); diff --git a/photon-server/src/test/java/org/photonvision/common/configuration/ConfigTest.java b/photon-server/src/test/java/org/photonvision/common/configuration/ConfigTest.java index 0ba5f1607..9e353c953 100644 --- a/photon-server/src/test/java/org/photonvision/common/configuration/ConfigTest.java +++ b/photon-server/src/test/java/org/photonvision/common/configuration/ConfigTest.java @@ -45,6 +45,7 @@ public class ConfigTest { TestUtils.loadLibraries(); configMgr = new ConfigManager(Path.of("testconfigdir")); configMgr.load(); + Logger.setLevel(LogGroup.General, LogLevel.TRACE); REFLECTIVE_PIPELINE_SETTINGS = new ReflectivePipelineSettings(); diff --git a/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java b/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java index 07225dc05..15e30989d 100644 --- a/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java @@ -114,12 +114,12 @@ public class VisionModuleManagerTest { TestUtils.WPI2019Image.FOV)), List.of()); - VisionModuleManager.getInstance().addSources(sources); + var modules = VisionModuleManager.getInstance().addSources(sources); var module0DataConsumer = new TestDataConsumer(); VisionModuleManager.getInstance().visionModules.get(0).addResultConsumer(module0DataConsumer); - VisionModuleManager.getInstance().startModules(); + modules.forEach(VisionModule::start); sleep(500); diff --git a/photon-server/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java b/photon-server/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java index e6241ef3c..d97b0214a 100644 --- a/photon-server/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java +++ b/photon-server/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java @@ -17,14 +17,15 @@ package org.photonvision.vision.processes; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + import edu.wpi.cscore.UsbCameraInfo; -import java.util.List; -import org.junit.jupiter.api.Assertions; +import java.util.ArrayList; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.util.TestUtils; -import org.photonvision.vision.camera.USBCameraSource; public class VisionSourceManagerTest { @BeforeEach @@ -32,39 +33,32 @@ public class VisionSourceManagerTest { TestUtils.loadLibraries(); } - final List usbCameraInfos = - List.of( - new UsbCameraInfo(0, "/this-is-a-real-path", "cameraByPath", new String[] {""}, 1, 1), - new UsbCameraInfo(2, "/this-is-a-fake-path1", "cameraById", new String[] {""}, 420, 1), - new UsbCameraInfo(1, "/this-is-a-real-path2", "cameraByPath", new String[] {""}, 1, 1), - new UsbCameraInfo(3, "/this-is-a-fake-path2", "cameraById", new String[] {""}, 420, 1), - new UsbCameraInfo(4, "/fake-path420", "notExisting", new String[] {""}, 420, 1), - new UsbCameraInfo(5, "/fake-path421", "notExisting", new String[] {""}, 420, 1)); - - final List camConfig = - List.of( - new CameraConfiguration("cameraByPath", "dank meme", "good name", "/this-is-a-real-path"), - new CameraConfiguration( - "cameraByPath", "dank meme2", "very original", "/this-is-a-real-path2"), - new CameraConfiguration("cameraById", "camera", "my camera", "2"), - new CameraConfiguration("cameraById", "camera2", "my camera", "3")); - - final List usbCameraSources = - List.of( - new USBCameraSource(camConfig.get(0)), - new USBCameraSource(camConfig.get(1)), - new USBCameraSource(camConfig.get(2)), - new USBCameraSource(camConfig.get(3)), - new USBCameraSource( - new CameraConfiguration("notExisting", "notExisting", "notExisting", "4")), - new USBCameraSource( - new CameraConfiguration("notExisting", "notExisting (1)", "notExisting (1)", "5"))); - @Test public void visionSourceTest() { - List i = VisionSourceManager.loadAllSources(camConfig, usbCameraInfos); - for (var source : i) { - Assertions.assertEquals(source, usbCameraSources.get(i.indexOf(source))); - } + var inst = new VisionSourceManager(); + var infoList = new ArrayList(); + inst.cameraInfoSupplier = () -> infoList; + + inst.tryMatchUSBCamImpl(); + var config = new CameraConfiguration("secondTestVideo", "dev/video1"); + UsbCameraInfo info1 = new UsbCameraInfo(0, "dev/video0", "testVideo", new String[0], 1, 2); + infoList.add(info1); + + inst.registerLoadedConfigs(config); + inst.tryMatchUSBCamImpl(); + inst.tryMatchUSBCamImpl(); + + assertTrue(inst.knownUsbCameras.contains(info1)); + assertEquals(1, inst.unmatchedLoadedConfigs.size()); + + UsbCameraInfo info2 = + new UsbCameraInfo(0, "dev/video1", "secondTestVideo", new String[0], 2, 1); + infoList.add(info2); + + inst.tryMatchUSBCamImpl(); + + assertTrue(inst.knownUsbCameras.contains(info2)); + assertEquals(2, inst.knownUsbCameras.size()); + assertEquals(0, inst.unmatchedLoadedConfigs.size()); } }