Add NT controlled framerate limiter (#2257)

Adds a method to lower the speed of a pipeline over NT, primarily to
reduce power consumption.

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java

@@ -51,16 +51,24 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
     private final BooleanSupplier driverModeSupplier;
     private final Consumer<Boolean> driverModeConsumer;
 
+    NTDataChangeListener fpsLimitListener;
+    private final Consumer<Integer> fpsLimitConsumer;
+    private final Supplier<Integer> fpsLimitSupplier;
+
     public NTDataPublisher(
             String cameraNickname,
             Supplier<Integer> pipelineIndexSupplier,
             Consumer<Integer> pipelineIndexConsumer,
             BooleanSupplier driverModeSupplier,
-            Consumer<Boolean> driverModeConsumer) {
+            Consumer<Boolean> driverModeConsumer,
+            Supplier<Integer> fpsLimitSupplier,
+            Consumer<Integer> fpsLimitConsumer) {
         this.pipelineIndexSupplier = pipelineIndexSupplier;
         this.pipelineIndexConsumer = pipelineIndexConsumer;
         this.driverModeSupplier = driverModeSupplier;
         this.driverModeConsumer = driverModeConsumer;
+        this.fpsLimitSupplier = fpsLimitSupplier;
+        this.fpsLimitConsumer = fpsLimitConsumer;
 
         updateCameraNickname(cameraNickname);
         updateEntries();
@@ -103,6 +111,19 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
         logger.debug("Set driver mode to " + newDriverMode);
     }
 
+    private void onFPSLimitChange(NetworkTableEvent entryNotification) {
+        var newFPSLimit = (int) entryNotification.valueData.value.getInteger();
+        var originalFPSLimit = fpsLimitSupplier.get();
+
+        if (newFPSLimit == originalFPSLimit) {
+            logger.debug("FPS limit is already " + newFPSLimit);
+            return;
+        }
+
+        fpsLimitConsumer.accept(newFPSLimit);
+        logger.debug("Set FPS limit to " + newFPSLimit);
+    }
+
     private void removeEntries() {
         if (pipelineIndexListener != null) pipelineIndexListener.remove();
         if (driverModeListener != null) driverModeListener.remove();
@@ -112,6 +133,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
     private void updateEntries() {
         if (pipelineIndexListener != null) pipelineIndexListener.remove();
         if (driverModeListener != null) driverModeListener.remove();
+        if (fpsLimitListener != null) fpsLimitListener.remove();
 
         ts.updateEntries();
 
@@ -122,6 +144,10 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
         driverModeListener =
                 new NTDataChangeListener(
                         ts.subTable.getInstance(), ts.driverModeSubscriber, this::onDriverModeChange);
+
+        fpsLimitListener =
+                new NTDataChangeListener(
+                        ts.subTable.getInstance(), ts.fpsLimitSubscriber, this::onFPSLimitChange);
     }
 
     public void updateCameraNickname(String newCameraNickname) {
@@ -170,6 +196,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
 
         ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get());
         ts.driverModePublisher.set(driverModeSupplier.getAsBoolean());
+        ts.fpsLimitPublisher.set(fpsLimitSupplier.get());
         ts.latencyMillisEntry.set(acceptedResult.getLatencyMillis());
         ts.fpsEntry.set(acceptedResult.fps);
         ts.hasTargetEntry.set(acceptedResult.hasTargets());

photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UICameraConfiguration.java

@@ -54,6 +54,8 @@ public class UICameraConfiguration {
     public PVCameraInfo matchedCameraInfo;
     public boolean mismatch;
 
+    public int fpsLimit;
+
     // Status for if the underlying device is present and such
     public boolean isConnected;
     public boolean hasConnected;

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

@@ -87,6 +87,8 @@ public class VisionModule {
     private int inputStreamPort = -1;
     private int outputStreamPort = -1;
 
+    private int fpsLimit = -1;
+
     FileSaveFrameConsumer inputFrameSaver;
     FileSaveFrameConsumer outputFrameSaver;
 
@@ -134,7 +136,8 @@ public class VisionModule {
                         this.pipelineManager::getCurrentPipeline,
                         this::consumeResult,
                         this.cameraQuirks,
-                        getChangeSubscriber());
+                        getChangeSubscriber(),
+                        this::getFPSLimit);
         this.streamRunnable = new StreamRunnable(new OutputStreamPipeline());
         changeSubscriberHandle = DataChangeService.getInstance().addSubscriber(changeSubscriber);
 
@@ -148,7 +151,9 @@ public class VisionModule {
                         pipelineManager::getRequestedIndex,
                         this::setPipeline,
                         pipelineManager::getDriverMode,
-                        this::setDriverMode);
+                        this::setDriverMode,
+                        this::getFPSLimit,
+                        this::setFPSLimit);
         uiDataConsumer = new UIDataPublisher(visionSource.getSettables().getConfiguration().uniqueName);
         statusLEDsConsumer =
                 new StatusLEDConsumer(visionSource.getSettables().getConfiguration().uniqueName);
@@ -574,6 +579,8 @@ public class VisionModule {
 
         ret.mismatch = this.mismatch;
 
+        ret.fpsLimit = this.fpsLimit;
+
         // TODO refactor into helper method
         var temp = new HashMap<Integer, HashMap<String, Object>>();
         var videoModes = visionSource.getSettables().getAllVideoModes();
@@ -616,6 +623,28 @@ public class VisionModule {
         return ret;
     }
 
+    /**
+     * Set FPS limit for this vision module. This will cause our processing thread to sleep in order
+     * to increase our processing time to match the provided fps. If our processing time is longer
+     * than the frame period, the FPS limit will not be reached.
+     *
+     * @param fps
+     */
+    public void setFPSLimit(int fps) {
+        this.fpsLimit = fps;
+        saveAndBroadcastAll();
+    }
+
+    /**
+     * Get the current FPS limit for this vision module. This limit cannot be exceeded, but may be
+     * lower depending on processing time.
+     *
+     * @return the FPS limit
+     */
+    public int getFPSLimit() {
+        return fpsLimit;
+    }
+
     public CameraConfiguration getStateAsCameraConfig() {
         var config = visionSource.getSettables().getConfiguration();
         config.setPipelineSettings(pipelineManager.userPipelineSettings);

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

@@ -49,6 +49,7 @@ public class VisionRunner {
     private final VisionModuleChangeSubscriber changeSubscriber;
     private final List<Runnable> runnableList = new ArrayList<Runnable>();
     private final QuirkyCamera cameraQuirks;
+    private final Supplier<Integer> fpsLimitSupplier;
 
     private long loopCount;
 
@@ -65,12 +66,14 @@ public class VisionRunner {
             Supplier<CVPipeline> pipelineSupplier,
             Consumer<CVPipelineResult> pipelineResultConsumer,
             QuirkyCamera cameraQuirks,
-            VisionModuleChangeSubscriber changeSubscriber) {
+            VisionModuleChangeSubscriber changeSubscriber,
+            Supplier<Integer> fpsLimitSupplier) {
         this.frameSupplier = frameSupplier;
         this.pipelineSupplier = pipelineSupplier;
         this.pipelineResultConsumer = pipelineResultConsumer;
         this.cameraQuirks = cameraQuirks;
         this.changeSubscriber = changeSubscriber;
+        this.fpsLimitSupplier = fpsLimitSupplier;
 
         visionProcessThread = new Thread(this::update);
         visionProcessThread.setName("VisionRunner - " + frameSupplier.getName());
@@ -146,6 +149,7 @@ public class VisionRunner {
                                 UIPhotonConfiguration.programStateToUi(ConfigManager.getInstance().getConfig())));
 
         while (!Thread.interrupted()) {
+            long start = System.currentTimeMillis();
             changeSubscriber.processSettingChanges();
             synchronized (runnableList) {
                 for (var runnable : runnableList) {
@@ -187,25 +191,33 @@ public class VisionRunner {
                 // Still feed with blank frames just dont run any pipelines
 
                 pipelineResultConsumer.accept(new CVPipelineResult(0l, 0, 0, null, new Frame()));
-                continue;
-            }
 
-            // If the pipeline has changed while we are getting our frame we should scrap
-            // that frame it
-            // may result in incorrect frame settings like hsv values
-            if (pipeline == pipelineSupplier.get()) {
+            } else if (pipeline == pipelineSupplier.get()) {
+                // If the pipeline has changed while we are getting our frame we should scrap
+                // that frame it may result in incorrect frame settings like hsv values
+
                 // There's no guarantee the processing type change will occur this tick, so
-                // pipelines should
-                // check themselves
+                // pipelines should check themselves
                 try {
                     var pipelineResult = pipeline.run(frame, cameraQuirks);
                     pipelineResultConsumer.accept(pipelineResult);
                 } catch (Exception ex) {
                     logger.error("Exception on loop " + loopCount, ex);
                 }
+                loopCount++;
             }
+            int fpsLimit = fpsLimitSupplier.get();
+            if (fpsLimit > 0) {
+                long sleepTime = (long) (1000 / fpsLimit - (System.currentTimeMillis() - start));
 
-            loopCount++;
+                if (sleepTime > 0) {
+                    try {
+                        Thread.sleep(sleepTime);
+                    } catch (InterruptedException e) {
+                        return;
+                    }
+                }
+            }
         }
     }
 }