Add NT controlled framerate limiter (#2257)

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

docs/source/docs/programming/photonlib/fps-limiter.md

@@ -0,0 +1,37 @@
+# FPS Limiter
+
+:::{warning}
+When using the FPS limiter, it's important to disable it before a match begins.
+:::
+
+The FPS limiter can be used to lower the frames processed per second for a given camera. This is intended to be used for power-saving, particularly in the case of high FPS cameras with powerful coprocessors. The value passed to the function will indicate the frames per second that should be processed. A value of -1 should be passed to indicate that the FPS limiter should not restrict processing; this is the default behavior.
+
+```{eval-rst}
+.. tab-set-code::
+   .. code-block:: java
+
+      int limit = camera.getFPSLimit();
+
+      camera.setFPSLimit(10);
+
+      // This removes any previously set FPS limit.
+      camera.setFPSLimit(-1);
+
+   .. code-block:: c++
+
+        int limit = camera.GetFPSLimit();
+
+        camera.SetFPSLimit(10);
+
+        // This removes any previously set FPS limit.
+        camera.SetFPSLimit(-1);
+
+   .. code-block:: python
+
+        limit = camera.getFPSLimit()
+
+        camera.setFPSLimit(10)
+
+        # This removes any previously set FPS limit.
+        camera.setFPSLimit(-1)
+```

docs/source/docs/programming/photonlib/index.md

@@ -9,4 +9,5 @@ using-target-data
 robot-pose-estimator
 driver-mode-pipeline-index
 controlling-led
+fps-limiter
 ```

photon-client/src/stores/settings/CameraSettingsStore.ts

@@ -91,6 +91,9 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
     maxWhiteBalanceTemp(): number {
       return this.currentCameraSettings.maxWhiteBalanceTemp;
     },
+    fpsLimit(): number {
+      return this.currentCameraSettings.fpsLimit;
+    },
     isConnected(): boolean {
       return this.currentCameraSettings.isConnected;
     },
@@ -141,6 +144,7 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
           minWhiteBalanceTemp: d.minWhiteBalanceTemp,
           maxWhiteBalanceTemp: d.maxWhiteBalanceTemp,
           matchedCameraInfo: d.matchedCameraInfo,
+          fpsLimit: d.fpsLimit,
           isConnected: d.isConnected,
           hasConnected: d.hasConnected,
           mismatch: d.mismatch

photon-client/src/types/SettingTypes.ts

@@ -270,6 +270,8 @@ export interface UiCameraConfiguration {
   minWhiteBalanceTemp: number;
   maxWhiteBalanceTemp: number;
 
+  fpsLimit: number;
+
   matchedCameraInfo: PVCameraInfo;
   isConnected: boolean;
   hasConnected: boolean;
@@ -439,6 +441,7 @@ export const PlaceholderCameraSettings: UiCameraConfiguration = reactive({
     PVCSICameraInfo: undefined,
     PVUsbCameraInfo: undefined
   },
+  fpsLimit: -1,
   isConnected: true,
   hasConnected: true,
   mismatch: false

photon-client/src/types/WebsocketDataTypes.ts

@@ -67,6 +67,7 @@ export interface WebsocketCameraSettingsUpdate {
   minWhiteBalanceTemp: number;
   maxWhiteBalanceTemp: number;
   matchedCameraInfo: PVCameraInfo;
+  fpsLimit: number;
   isConnected: boolean;
   hasConnected: boolean;
   mismatch: boolean;

photon-client/src/views/DashboardView.vue

@@ -77,6 +77,10 @@ const conflictingCameraShown = computed<boolean>(() => {
   return useSettingsStore().general.conflictingCameras.length > 0;
 });
 
+const fpsLimitWarningShown = computed<boolean>(() => {
+  return Object.values(useCameraSettingsStore().cameras).some((c) => c.fpsLimit > 0);
+});
+
 const showCameraSetupDialog = ref(useCameraSettingsStore().needsCameraConfiguration);
 </script>
 
@@ -106,6 +110,19 @@ const showCameraSetupDialog = ref(useCameraSettingsStore().needsCameraConfigurat
         Conflicting hostname detected! Please change the hostname in the <a href="#/settings">Settings tab</a>!
       </span>
     </v-alert>
+    <v-alert
+      v-if="fpsLimitWarningShown"
+      class="mb-3"
+      color="error"
+      density="compact"
+      icon="mdi-alert-circle-outline"
+      :variant="theme.global.name.value === 'LightTheme' ? 'elevated' : 'tonal'"
+    >
+      <span
+        >One or more cameras have an FPS limit set! This may cause performance issues. Check your logs for more
+        information.
+      </span>
+    </v-alert>
     <v-alert
       v-if="conflictingCameraShown"
       class="mb-3"

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;
+                    }
+                }
+            }
         }
     }
 }

photon-lib/py/photonlibpy/networktables/NTTopicSet.py

@@ -47,6 +47,13 @@ class NTTopicSet:
 
         self.driverModeSubscriber.getTopic().publish().setDefault(False)
 
+        self.fpsLimitPublisher = self.subTable.getIntegerTopic("fpsLimit").publish()
+        self.fpsLimitSubscriber = self.subTable.getIntegerTopic(
+            "fpsLimitRequest"
+        ).subscribe(-1)
+
+        self.fpsLimitSubscriber.getTopic().publish().setDefault(-1)
+
         self.latencyMillisEntry = self.subTable.getDoubleTopic(
             "latencyMillis"
         ).publish()

photon-lib/py/photonlibpy/photonCamera.py

@@ -74,6 +74,12 @@ class PhotonCamera:
         self._driverModeSubscriber = self._cameraTable.getBooleanTopic(
             "driverMode"
         ).subscribe(False)
+        self._fpsLimitPublisher = self._cameraTable.getIntegerTopic(
+            "fpsLimitRequest"
+        ).publish()
+        self._fpsLimitSubscriber = self._cameraTable.getIntegerTopic(
+            "fpsLimit"
+        ).subscribe(-1)
         self._inputSaveImgEntry = self._cameraTable.getIntegerTopic(
             "inputSaveImgCmd"
         ).getEntry(0)
@@ -190,6 +196,22 @@ class PhotonCamera:
 
         self._driverModePublisher.set(driverMode)
 
+    def getFPSLimit(self) -> int:
+        """Returns the current FPS limit set on the camera.
+
+        :returns: The current FPS limit.
+        """
+
+        return self._fpsLimitSubscriber.get()
+
+    def setFPSLimit(self, fpsLimit: int) -> None:
+        """Sets the FPS limit on the camera.
+
+        :param fpsLimit: The FPS limit to set. Set to -1 for unlimited FPS.
+        """
+
+        self._fpsLimitPublisher.set(fpsLimit)
+
     def takeInputSnapshot(self) -> None:
         """Request the camera to save a new image file from the input camera stream with overlays. Images
         take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk

photon-lib/src/main/java/org/photonvision/PhotonCamera.java

@@ -66,6 +66,8 @@ public class PhotonCamera implements AutoCloseable {
     PacketSubscriber<PhotonPipelineResult> resultSubscriber;
     BooleanPublisher driverModePublisher;
     BooleanSubscriber driverModeSubscriber;
+    IntegerPublisher fpsLimitPublisher;
+    IntegerSubscriber fpsLimitSubscriber;
     StringSubscriber versionEntry;
     IntegerEntry inputSaveImgEntry, outputSaveImgEntry;
     IntegerPublisher pipelineIndexRequest, ledModeRequest;
@@ -81,6 +83,8 @@ public class PhotonCamera implements AutoCloseable {
         resultSubscriber.close();
         driverModePublisher.close();
         driverModeSubscriber.close();
+        fpsLimitPublisher.close();
+        fpsLimitSubscriber.close();
         versionEntry.close();
         inputSaveImgEntry.close();
         outputSaveImgEntry.close();
@@ -144,6 +148,8 @@ public class PhotonCamera implements AutoCloseable {
         resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.photonStruct);
         driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
         driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false);
+        fpsLimitPublisher = cameraTable.getIntegerTopic("fpsLimitRequest").publish();
+        fpsLimitSubscriber = cameraTable.getIntegerTopic("fpsLimit").subscribe(-1);
         inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
         outputSaveImgEntry = cameraTable.getIntegerTopic("outputSaveImgCmd").getEntry(0);
         pipelineIndexRequest = cameraTable.getIntegerTopic("pipelineIndexRequest").publish();
@@ -373,6 +379,24 @@ public class PhotonCamera implements AutoCloseable {
         driverModePublisher.set(driverMode);
     }
 
+    /**
+     * Gets the FPS limit set on the camera.
+     *
+     * @return The current FPS limit.
+     */
+    public int getFPSLimit() {
+        return (int) fpsLimitSubscriber.get();
+    }
+
+    /**
+     * Sets the FPS limit on the camera.
+     *
+     * @param fps The FPS limit to set. Set to -1 for unlimited FPS.
+     */
+    public void setFPSLimit(int fps) {
+        fpsLimitPublisher.set(fps);
+    }
+
     /**
      * Request the camera to save a new image file from the input camera stream with overlays. Images
      * take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk

photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp

@@ -196,6 +196,9 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
           rootTable->GetBooleanTopic("driverMode").Subscribe(false)),
       driverModePublisher(
           rootTable->GetBooleanTopic("driverModeRequest").Publish()),
+      fpsLimitSubscriber(rootTable->GetIntegerTopic("fpsLimit").Subscribe(-1)),
+      fpsLimitPublisher(
+          rootTable->GetIntegerTopic("fpsLimitRequest").Publish()),
       heartbeatSubscriber(
           rootTable->GetIntegerTopic("heartbeat").Subscribe(-1)),
       topicNameSubscriber(instance, PHOTON_PREFIX, {.topicsOnly = true}),
@@ -322,6 +325,14 @@ void PhotonCamera::SetDriverMode(bool driverMode) {
   driverModePublisher.Set(driverMode);
 }
 
+bool PhotonCamera::GetDriverMode() const { return driverModeSubscriber.Get(); }
+
+int PhotonCamera::GetFPSLimit() const { return fpsLimitSubscriber.Get(); }
+
+void PhotonCamera::SetFPSLimit(int fpsLimit) {
+  fpsLimitPublisher.Set(fpsLimit);
+}
+
 void PhotonCamera::TakeInputSnapshot() {
   inputSaveImgEntry.Set(inputSaveImgSubscriber.Get() + 1);
 }
@@ -330,8 +341,6 @@ void PhotonCamera::TakeOutputSnapshot() {
   outputSaveImgEntry.Set(outputSaveImgSubscriber.Get() + 1);
 }
 
-bool PhotonCamera::GetDriverMode() const { return driverModeSubscriber.Get(); }
-
 void PhotonCamera::SetPipelineIndex(int index) { pipelineIndexPub.Set(index); }
 
 int PhotonCamera::GetPipelineIndex() const {

photon-lib/src/main/native/include/photon/PhotonCamera.h

@@ -103,6 +103,16 @@ class PhotonCamera {
    */
   bool GetDriverMode() const;
 
+  /**
+   * @param fpsLimit The FPS limit to set. Use -1 for unlimited FPS.
+   */
+  void SetFPSLimit(int fpsLimit);
+
+  /**
+   * @return The FPS limit set on the camera, or -1 if no limit is set.
+   */
+  int GetFPSLimit() const;
+
   /**
    * Request the camera to save a new image file from the input
    * camera stream with overlays.
@@ -210,6 +220,9 @@ class PhotonCamera {
 
   nt::BooleanSubscriber driverModeSubscriber;
   nt::BooleanPublisher driverModePublisher;
+  nt::IntegerSubscriber fpsLimitSubscriber;
+  nt::IntegerPublisher fpsLimitPublisher;
+
   nt::IntegerSubscriber ledModeSubscriber;
 
   nt::IntegerSubscriber heartbeatSubscriber;

photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java

@@ -53,6 +53,9 @@ public class NTTopicSet {
     public BooleanPublisher driverModePublisher;
     public BooleanSubscriber driverModeSubscriber;
 
+    public IntegerPublisher fpsLimitPublisher;
+    public IntegerSubscriber fpsLimitSubscriber;
+
     public DoublePublisher latencyMillisEntry;
     public DoublePublisher fpsEntry;
     public BooleanPublisher hasTargetEntry;
@@ -100,6 +103,11 @@ public class NTTopicSet {
         // Fun little hack to make the request show up
         driverModeSubscriber.getTopic().publish().setDefault(false);
 
+        fpsLimitPublisher = subTable.getIntegerTopic("fpsLimit").publish();
+        fpsLimitSubscriber = subTable.getIntegerTopic("fpsLimitRequest").subscribe(-1);
+
+        fpsLimitSubscriber.getTopic().publish().setDefault(-1);
+
         latencyMillisEntry = subTable.getDoubleTopic("latencyMillis").publish();
         fpsEntry = subTable.getDoubleTopic("fps").publish();
         hasTargetEntry = subTable.getBooleanTopic("hasTarget").publish();
@@ -129,6 +137,9 @@ public class NTTopicSet {
         if (driverModePublisher != null) driverModePublisher.close();
         if (driverModeSubscriber != null) driverModeSubscriber.close();
 
+        if (fpsLimitPublisher != null) fpsLimitPublisher.close();
+        if (fpsLimitSubscriber != null) fpsLimitSubscriber.close();
+
         if (latencyMillisEntry != null) latencyMillisEntry.close();
         if (fpsEntry != null) fpsEntry.close();
         if (hasTargetEntry != null) hasTargetEntry.close();