Add checking of current and requested video_mode (#859)

* (fix): Add checking of current to requested mode

* Create videoModeEquals

* Update OpenCvUtils.java

---------

Co-authored-by: Chirag Lamsal <chirag.lamsal@gmail.com>

photon-core/src/main/java/org/photonvision/common/util/vision/OpenCvUtils.java

@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.util.vision;
+
+import edu.wpi.first.cscore.VideoMode;
+
+public class OpenCvUtils {
+    private OpenCvUtils() {}
+
+    public static boolean videoModeEquals(VideoMode a, VideoMode b) {
+        // WPILib doesn't provide an equals(), so implement our own here
+        if (a.pixelFormat != b.pixelFormat) return false;
+        if (a.width != b.width) return false;
+        if (a.height != b.height) return false;
+        if (a.fps != b.fps) return false;
+        return true;
+    }
+}

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

@@ -22,6 +22,7 @@ import java.util.HashMap;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
+import org.photonvision.common.util.vision.OpenCvUtils;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.frame.FrameStaticProperties;
 
@@ -63,6 +64,11 @@ public abstract class VisionSourceSettables {
     }
 
     public void setVideoMode(VideoMode mode) {
+        if (OpenCvUtils.videoModeEquals(mode, getCurrentVideoMode())) {
+            logger.info("Requested video mode is already the current video mode");
+            return;
+        }
+
         logger.info(
                 "Setting video mode to "
                         + "FPS: "