Set default pipeline idx in PipelineManager constructor (#1286)

Addresses #1285

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

@@ -61,7 +61,8 @@ public class PipelineManager {
     PipelineManager(
             DriverModePipelineSettings driverSettings,
             List<CVPipelineSettings> userPipelines,
-            String uniqueName) {
+            String uniqueName,
+            int defaultIndex) {
         this.userPipelineSettings = new ArrayList<>(userPipelines);
         // This is to respect the default res idx for vendor cameras
 
@@ -70,10 +71,19 @@ public class PipelineManager {
         if (userPipelines.isEmpty()) addPipeline(PipelineType.Reflective);
 
         calibration3dPipeline = new Calibrate3dPipeline(uniqueName);
+
+        // We know that at this stage, VisionRunner hasn't yet started so we're good to do this from
+        // this thread
+        this.setIndex(defaultIndex);
+        updatePipelineFromRequested();
     }
 
     public PipelineManager(CameraConfiguration config) {
-        this(config.driveModeSettings, config.pipelineSettings, config.uniqueName);
+        this(
+                config.driveModeSettings,
+                config.pipelineSettings,
+                config.uniqueName,
+                config.currentPipelineIndex);
     }
 
     /**
@@ -173,6 +183,14 @@ public class PipelineManager {
 
     private volatile int requestedIndex = 0;
 
+    /**
+     * Grab the currently requested pipeline index. The VisionRunner may not have changed over to this
+     * pipeline yet.
+     */
+    public int getRequestedIndex() {
+        return requestedIndex;
+    }
+
     /**
      * Internal method for setting the active pipeline. <br>
      * <br>

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

@@ -143,7 +143,7 @@ public class VisionModule {
         ntConsumer =
                 new NTDataPublisher(
                         visionSource.getSettables().getConfiguration().nickname,
-                        pipelineManager::getCurrentPipelineIndex,
+                        pipelineManager::getRequestedIndex,
                         this::setPipeline,
                         pipelineManager::getDriverMode,
                         this::setDriverMode);
@@ -156,6 +156,7 @@ public class VisionModule {
                 (result) ->
                         lastPipelineResultBestTarget = result.hasTargets() ? result.targets.get(0) : null);
 
+        // Sync VisionModule state with the first pipeline index
         setPipeline(visionSource.getSettables().getConfiguration().currentPipelineIndex);
 
         // Set vendor FOV
@@ -321,7 +322,7 @@ public class VisionModule {
 
     void changePipelineType(int newType) {
         pipelineManager.changePipelineType(newType);
-        setPipeline(pipelineManager.getCurrentPipelineIndex());
+        setPipeline(pipelineManager.getRequestedIndex());
         saveAndBroadcastAll();
     }
 
@@ -329,9 +330,7 @@ public class VisionModule {
         pipelineManager.setDriverMode(isDriverMode);
         setVisionLEDs(!isDriverMode);
         setPipeline(
-                isDriverMode
-                        ? PipelineManager.DRIVERMODE_INDEX
-                        : pipelineManager.getCurrentPipelineIndex());
+                isDriverMode ? PipelineManager.DRIVERMODE_INDEX : pipelineManager.getRequestedIndex());
         saveAndBroadcastAll();
     }
 
@@ -385,7 +384,7 @@ public class VisionModule {
         var ret = pipelineManager.calibration3dPipeline.tryCalibration();
         pipelineManager.setCalibrationMode(false);
 
-        setPipeline(pipelineManager.getCurrentPipelineIndex());
+        setPipeline(pipelineManager.getRequestedIndex());
 
         if (ret != null) {
             logger.debug("Saving calibration...");
@@ -447,7 +446,7 @@ public class VisionModule {
         setVisionLEDs(pipelineSettings.ledMode);
 
         visionSource.getSettables().getConfiguration().currentPipelineIndex =
-                pipelineManager.getCurrentPipelineIndex();
+                pipelineManager.getRequestedIndex();
 
         return true;
     }
@@ -511,7 +510,7 @@ public class VisionModule {
         ret.uniqueName = visionSource.getSettables().getConfiguration().uniqueName;
         ret.currentPipelineSettings =
                 SerializationUtils.objectToHashMap(pipelineManager.getCurrentPipelineSettings());
-        ret.currentPipelineIndex = pipelineManager.getCurrentPipelineIndex();
+        ret.currentPipelineIndex = pipelineManager.getRequestedIndex();
         ret.pipelineNicknames = pipelineManager.getPipelineNicknames();
         ret.cameraQuirks = visionSource.getSettables().getConfiguration().cameraQuirks;
 
@@ -553,7 +552,7 @@ public class VisionModule {
         var config = visionSource.getSettables().getConfiguration();
         config.setPipelineSettings(pipelineManager.userPipelineSettings);
         config.driveModeSettings = pipelineManager.driverModePipeline.getSettings();
-        config.currentPipelineIndex = Math.max(pipelineManager.getCurrentPipelineIndex(), -1);
+        config.currentPipelineIndex = Math.max(pipelineManager.getRequestedIndex(), -1);
 
         return config;
     }

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

@@ -88,7 +88,7 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
                         parentModule.saveAndBroadcastAll();
                         return;
                     case "deleteCurrPipeline":
-                        var indexToDelete = parentModule.pipelineManager.getCurrentPipelineIndex();
+                        var indexToDelete = parentModule.pipelineManager.getRequestedIndex();
                         logger.info("Deleting current pipe at index " + indexToDelete);
                         int newIndex = parentModule.pipelineManager.removePipeline(indexToDelete);
                         parentModule.setPipeline(newIndex);
@@ -96,7 +96,7 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
                         return;
                     case "changePipeline": // change active pipeline
                         var index = (Integer) newPropValue;
-                        if (index == parentModule.pipelineManager.getCurrentPipelineIndex()) {
+                        if (index == parentModule.pipelineManager.getRequestedIndex()) {
                             logger.debug("Skipping pipeline change, index " + index + " already active");
                             return;
                         }
@@ -181,6 +181,9 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
                         parentModule.changePipelineType((Integer) newPropValue);
                         parentModule.saveAndBroadcastAll();
                         return;
+                    case "isDriverMode":
+                        parentModule.setDriverMode((Boolean) newPropValue);
+                        return;
                 }
 
                 // special case for camera settables

photon-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java

@@ -30,7 +30,7 @@ public class PipelineManagerTest {
     public void testUniqueName() {
         TestUtils.loadLibraries();
         PipelineManager manager =
-                new PipelineManager(new DriverModePipelineSettings(), List.of(), "meme_name");
+                new PipelineManager(new DriverModePipelineSettings(), List.of(), "meme_name", -1);
         manager.addPipeline(PipelineType.Reflective, "Another");
 
         // We now have ["New Pipeline", "Another"]