diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java index 4efbc5eb6..0075e9825 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleManager.java @@ -19,7 +19,6 @@ package org.photonvision.vision.processes; import java.util.*; import java.util.stream.Collectors; -import org.photonvision.common.configuration.CameraConfiguration; /** VisionModuleManager has many VisionModules, and provides camera configuration data to them. */ public class VisionModuleManager { @@ -54,9 +53,9 @@ public class VisionModuleManager { public List addSources(List visionSources) { var addedModules = new HashMap(); + assignCameraIndex(visionSources); for (var visionSource : visionSources) { var pipelineManager = new PipelineManager(visionSource.getCameraConfiguration()); - assignCameraIndex(visionSource.getCameraConfiguration()); var module = new VisionModule(pipelineManager, visionSource, visionModules.size()); visionModules.add(module); @@ -72,16 +71,30 @@ public class VisionModuleManager { return sortedModulesList; } - private void assignCameraIndex(CameraConfiguration config) { - var max = - visionModules.stream() - .mapToInt(it -> it.visionSource.getCameraConfiguration().streamIndex) - .max() - .orElse(-1); + private void assignCameraIndex(List config) { + // We won't necessarily have already added all of the cameras we need to at this point + // But by operating on the list, we have a fairly good idea of which we need to change + // but it's not guaranteed that we change the correct one + // The best we can do is try to avoid a case where the stream index runs away to infinity + // since we can only stream 5 cameras at once - // If the current stream index is reserved, increase by 1 - if (config.streamIndex <= max) { - config.streamIndex = max + 1; + for (var v : config) { + var listNoV = new ArrayList<>(config); + listNoV.remove(v); + if (listNoV.stream() + .anyMatch( + it -> + it.getCameraConfiguration().streamIndex + == v.getCameraConfiguration().streamIndex)) { + int idx = 0; + while (listNoV.stream() + .map(it -> it.getCameraConfiguration().streamIndex) + .collect(Collectors.toList()) + .contains(idx)) { + idx++; + } + v.getCameraConfiguration().streamIndex = idx; + } } } } diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java index 3464a4ac8..99ffe1d3b 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java @@ -17,10 +17,14 @@ package org.photonvision.vision.processes; +import static org.junit.jupiter.api.Assertions.assertTrue; + import edu.wpi.cscore.VideoMode; import edu.wpi.first.wpilibj.geometry.Rotation2d; +import java.util.Arrays; import java.util.HashMap; import java.util.List; +import java.util.stream.Collectors; import org.junit.jupiter.api.*; import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; @@ -131,6 +135,52 @@ public class VisionModuleManagerTest { printTestResults(module0DataConsumer.result); } + @Test + public void testMultipleStreamIndex() { + ConfigManager.getInstance().load(); + + var conf = new CameraConfiguration("Foo", "Bar"); + conf.streamIndex = 1; + var ffp = + new FileFrameProvider( + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false), + TestUtils.WPI2019Image.FOV); + var testSource = new TestSource(ffp, conf); + + var conf2 = new CameraConfiguration("Foo2", "Bar"); + conf2.streamIndex = 0; + var ffp2 = + new FileFrameProvider( + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false), + TestUtils.WPI2019Image.FOV); + var testSource2 = new TestSource(ffp2, conf2); + + var conf3 = new CameraConfiguration("Foo3", "Bar"); + conf3.streamIndex = 0; + var ffp3 = + new FileFrameProvider( + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false), + TestUtils.WPI2019Image.FOV); + var testSource3 = new TestSource(ffp3, conf3); + + var modules = + VisionModuleManager.getInstance().addSources(List.of(testSource, testSource2, testSource3)); + + System.out.println( + Arrays.toString( + modules.stream() + .map(it -> it.visionSource.getCameraConfiguration().streamIndex) + .collect(Collectors.toList()) + .toArray())); + var idxs = + modules.stream() + .map(it -> it.visionSource.getCameraConfiguration().streamIndex) + .collect(Collectors.toList()); + assertTrue(idxs.contains(0)); + assertTrue(idxs.contains(1)); + assertTrue(idxs.contains(2)); + } + private static void printTestResults(CVPipelineResult pipelineResult) { double fps = 1000 / pipelineResult.getLatencyMillis(); System.out.print(