Merge branch 'master' into 2023-12-28_pi-image-modifier

photon-client/src/components/dashboard/tabs/TargetsTab.vue

@@ -3,12 +3,32 @@ import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { PipelineType } from "@/types/PipelineTypes";
 import { useStateStore } from "@/stores/StateStore";
 
+const wrapToPi = (delta: number): number => {
+  let ret = delta;
+  while (ret < -Math.PI) ret += Math.PI * 2;
+  while (ret > Math.PI) ret -= Math.PI * 2;
+  return ret;
+};
+
 const calculateStdDev = (values: number[]): number => {
   if (values.length < 2) return 0;
 
-  const mean = values.reduce((sum, number) => sum + number, 0) / values.length;
+  // Use mean of cosine/sine components to handle angle wrapping
+  const cosines = values.map((it) => Math.cos(it));
+  const sines = values.map((it) => Math.sin(it));
+  const cosmean = cosines.reduce((sum, number) => sum + number, 0) / values.length;
+  const sinmean = sines.reduce((sum, number) => sum + number, 0) / values.length;
+
+  // Borrowed from WPILib's Rotation2d
+  const hypot = Math.hypot(cosmean, sinmean);
+  let mean;
+  if (hypot > 1e-6) {
+    mean = Math.atan2(sinmean / hypot, cosmean / hypot);
+  } else {
+    mean = 0;
+  }
 
-  return Math.sqrt(values.map((x) => Math.pow(x - mean, 2)).reduce((a, b) => a + b) / values.length);
+  return Math.sqrt(values.map((x) => Math.pow(wrapToPi(x - mean), 2)).reduce((a, b) => a + b) / values.length);
 };
 const resetCurrentBuffer = () => {
   // Need to clear the array in place
@@ -119,23 +139,23 @@ const resetCurrentBuffer = () => {
             <td>
               {{
                 (
-                  useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x *
+                  (useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x || 0) *
                   (180.0 / Math.PI)
                 ).toFixed(2)
               }}&deg;
             </td>
             <td>
               {{
                 (
-                  useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y *
+                  (useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y || 0) *
                   (180.0 / Math.PI)
                 ).toFixed(2)
               }}&deg;
             </td>
             <td>
               {{
                 (
-                  useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z *
+                  (useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z || 0) *
                   (180.0 / Math.PI)
                 ).toFixed(2)
               }}&deg;
@@ -146,8 +166,8 @@ const resetCurrentBuffer = () => {
       </v-row>
       <v-row class="pb-4 white--text" style="display: flex; flex-direction: column">
         <v-card-subtitle class="ma-0 pa-0 pb-4 pr-4" style="font-size: 16px"
-          >Multi-tag pose standard deviation over the last {{ useStateStore().currentMultitagBuffer.length }}/100
-          samples
+          >Multi-tag pose standard deviation over the last
+          {{ useStateStore().currentMultitagBuffer?.length || "NaN" }}/100 samples
         </v-card-subtitle>
         <v-btn color="secondary" class="mb-4 mt-1" style="width: min-content" depressed @click="resetCurrentBuffer"
           >Reset Samples</v-btn
@@ -164,37 +184,37 @@ const resetCurrentBuffer = () => {
           <tbody v-show="useStateStore().currentPipelineResults?.multitagResult">
             <td>
               {{
-                calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.x)).toFixed(5)
+                calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.x) || []).toFixed(5)
               }}&nbsp;m
             </td>
             <td>
               {{
-                calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.y)).toFixed(5)
+                calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.y) || []).toFixed(5)
               }}&nbsp;m
             </td>
             <td>
               {{
-                calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.z)).toFixed(5)
+                calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.z) || []).toFixed(5)
               }}&nbsp;m
             </td>
             <td>
               {{
                 calculateStdDev(
-                  useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_x * (180.0 / Math.PI))
+                  useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_x * (180.0 / Math.PI)) || []
                 ).toFixed(5)
               }}&deg;
             </td>
             <td>
               {{
                 calculateStdDev(
-                  useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_y * (180.0 / Math.PI))
+                  useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_y * (180.0 / Math.PI)) || []
                 ).toFixed(5)
               }}&deg;
             </td>
             <td>
               {{
                 calculateStdDev(
-                  useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_z * (180.0 / Math.PI))
+                  useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_z * (180.0 / Math.PI)) || []
                 ).toFixed(5)
               }}&deg;
             </td>

photon-core/src/main/java/org/photonvision/common/ProgramStatus.java

@@ -20,6 +20,5 @@
 public enum ProgramStatus {
     UHOH,
     RUNNING,
-    RUNNING_NT,
-    RUNNING_NT_TARGET
+    RUNNING_NT
 }

photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java

@@ -343,6 +343,29 @@ private void addFile(PreparedStatement ps, String key, String value) throws SQLE
         ps.setString(2, value);
     }
 
+    // NOTE to Future Developers:
+    // These booleans form a mechanism to prevent saveGlobal() and
+    // saveOneFile() from stepping on each other's toes. Both write
+    // to the database on disk, and both write to the same keys, but
+    // they use different sources. Generally, if the user has done something
+    // to trigger saveOneFile() to get called, it implies they want that
+    // configuration, and not whatever is in RAM right now (which is what
+    // saveGlobal() uses to write). Therefor, once saveOneFile() is invoked,
+    // we record which entry was overwritten in the database and prevent
+    // overwriting it when saveGlobal() is invoked (likely by the shutdown
+    // that should almost almost almost happen right after saveOneFile() is
+    // invoked).
+    //
+    // In the future, this may not be needed. A better architecture would involve
+    // manipulating the RAM representation of configuration when new .json files
+    // are uploaded in the UI, and eliminate all other usages of saveOneFile().
+    // But, seeing as it's Dec 28 and kickoff is nigh, we put this here and moved on.
+    // Thank you for coming to my TED talk.
+    private boolean skipSavingHWCfg = false;
+    private boolean skipSavingHWSet = false;
+    private boolean skipSavingNWCfg = false;
+    private boolean skipSavingAPRTG = false;
+
     private void saveGlobal(Connection conn) {
         PreparedStatement statement1 = null;
         PreparedStatement statement2 = null;
@@ -351,28 +374,34 @@ private void saveGlobal(Connection conn) {
             // Replace this camera's row with the new settings
             var sqlString = "REPLACE INTO global (filename, contents) VALUES " + "(?,?);";
 
-            statement1 = conn.prepareStatement(sqlString);
-            addFile(
-                    statement1,
-                    TableKeys.HARDWARE_SETTINGS,
-                    JacksonUtils.serializeToString(config.getHardwareSettings()));
-            statement1.executeUpdate();
+            if (!skipSavingHWSet) {
+                statement1 = conn.prepareStatement(sqlString);
+                addFile(
+                        statement1,
+                        TableKeys.HARDWARE_SETTINGS,
+                        JacksonUtils.serializeToString(config.getHardwareSettings()));
+                statement1.executeUpdate();
+            }
 
-            statement2 = conn.prepareStatement(sqlString);
-            addFile(
-                    statement2,
-                    TableKeys.NETWORK_CONFIG,
-                    JacksonUtils.serializeToString(config.getNetworkConfig()));
-            statement2.executeUpdate();
-            statement2.close();
-
-            statement3 = conn.prepareStatement(sqlString);
-            addFile(
-                    statement3,
-                    TableKeys.HARDWARE_CONFIG,
-                    JacksonUtils.serializeToString(config.getHardwareConfig()));
-            statement3.executeUpdate();
-            statement3.close();
+            if (!skipSavingNWCfg) {
+                statement2 = conn.prepareStatement(sqlString);
+                addFile(
+                        statement2,
+                        TableKeys.NETWORK_CONFIG,
+                        JacksonUtils.serializeToString(config.getNetworkConfig()));
+                statement2.executeUpdate();
+                statement2.close();
+            }
+
+            if (!skipSavingHWCfg) {
+                statement3 = conn.prepareStatement(sqlString);
+                addFile(
+                        statement3,
+                        TableKeys.HARDWARE_CONFIG,
+                        JacksonUtils.serializeToString(config.getHardwareConfig()));
+                statement3.executeUpdate();
+                statement3.close();
+            }
 
         } catch (SQLException | IOException e) {
             logger.error("Err saving global", e);
@@ -431,21 +460,25 @@ private boolean saveOneFile(String fname, Path path) {
 
     @Override
     public boolean saveUploadedHardwareConfig(Path uploadPath) {
+        skipSavingHWCfg = true;
         return saveOneFile(TableKeys.HARDWARE_CONFIG, uploadPath);
     }
 
     @Override
     public boolean saveUploadedHardwareSettings(Path uploadPath) {
+        skipSavingHWSet = true;
         return saveOneFile(TableKeys.HARDWARE_SETTINGS, uploadPath);
     }
 
     @Override
     public boolean saveUploadedNetworkConfig(Path uploadPath) {
+        skipSavingNWCfg = true;
         return saveOneFile(TableKeys.NETWORK_CONFIG, uploadPath);
     }
 
     @Override
     public boolean saveUploadedAprilTagFieldLayout(Path uploadPath) {
+        skipSavingAPRTG = true;
         return saveOneFile(TableKeys.ATFL_CONFIG_FILE, uploadPath);
     }
 

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

@@ -32,6 +32,7 @@
 import org.photonvision.common.configuration.NetworkConfig;
 import org.photonvision.common.dataflow.DataChangeService;
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
+import org.photonvision.common.hardware.HardwareManager;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.scripting.ScriptEventType;
@@ -91,6 +92,7 @@ public void accept(NetworkTableEvent event) {
                                 event.connInfo.remote_port,
                                 event.connInfo.protocol_version);
                 logger.error(msg);
+                HardwareManager.getInstance().setNTConnected(false);
 
                 hasReportedConnectionFailure = true;
                 getInstance().broadcastConnectedStatus();
@@ -102,6 +104,7 @@ public void accept(NetworkTableEvent event) {
                                 event.connInfo.remote_port,
                                 event.connInfo.protocol_version);
                 logger.info(msg);
+                HardwareManager.getInstance().setNTConnected(true);
 
                 hasReportedConnectionFailure = false;
                 ScriptManager.queueEvent(ScriptEventType.kNTConnected);

photon-core/src/main/java/org/photonvision/common/dataflow/statusLEDs/StatusLEDConsumer.java

@@ -0,0 +1,35 @@
+/*
+ * 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.dataflow.statusLEDs;
+
+import org.photonvision.common.dataflow.CVPipelineResultConsumer;
+import org.photonvision.common.hardware.HardwareManager;
+import org.photonvision.vision.pipeline.result.CVPipelineResult;
+
+public class StatusLEDConsumer implements CVPipelineResultConsumer {
+    private final int index;
+
+    public StatusLEDConsumer(int index) {
+        this.index = index;
+    }
+
+    @Override
+    public void accept(CVPipelineResult t) {
+        HardwareManager.getInstance().setTargetsVisibleStatus(this.index, t.hasTargets());
+    }
+}