Bug Fix Grab Bag (#688)

* Reordered ov video modes to be lowest-to-highest res

* Save off sensor model on init. Guard against low, crashy exposures.

* Pulled in matt's fixups from https://github.com/PhotonVision/photon-libcamera-gl-driver/suites/10144555465/artifacts/495489276

* Further autoexposure tweaks for picam v1

* Allow undercores in camera rename

* Additional guarding against output images being empty

* lock out auto-exposure on ov9281's

* Guarding stream pipelines against empty frames from cameras. Rearranged driver stream to resize first, then draw crosshairs (matchces with other pipelines now).

* NT Priority fixup - if client is sending commands on NT, its nt value should win over anything done from the UI

* Synchronous pipline adjustmet fix, method cleanup

* lint

* circle pipe and data publish bugfixes

* lint

* Pulled in matt's latest .so and re-enabled auto exposure on 9281's

.github/workflows/main.yml

@@ -156,10 +156,11 @@ jobs:
           pip install sphinx sphinx_rtd_theme sphinx-tabs sphinxext-opengraph doc8
           pip install -r requirements.txt
 
-      - name: Check the docs
-        run: |
-          make linkcheck
-          make lint
+      # Don't check the docs. If a PR was merged to the docs repo, it ought to pass CI. No need to re-check here.
+      # - name: Check the docs
+      #   run: |
+      #     make linkcheck
+      #     make lint
 
       - name: Build the docs
         run: |
@@ -375,30 +376,57 @@ jobs:
           ./gradlew photon-server:shadowJar --max-workers 2
         if: ${{ (matrix.arch-override == 'none') }}
 
-      # The image will only pull the Pi32 JAR in
-      - name: Generate image
-        if: ${{ github.event_name != 'pull_request' && (matrix.artifact-name) == 'LinuxArm32' }}
-        run: |
-          chmod +x scripts/generatePiImage.sh
-          ./scripts/generatePiImage.sh
-
       # Upload final fat jar as artifact.
       - uses: actions/upload-artifact@v3
         with:
           name: jar-${{ matrix.artifact-name }}
           path: photon-server/build/libs
 
-      # Upload image as well
+
+  photon-image-generator:
+    needs: [photon-build-package]
+    if: ${{ github.event_name != 'pull_request' }}
+    strategy:
+        fail-fast: false
+        matrix:
+          include:
+            - os: ubuntu-latest
+              artifact-name: LinuxArm64
+              image_suffix: RaspberryPi
+              image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.1.0-beta-4_arm64
+            - os: ubuntu-latest
+              artifact-name: LinuxArm64
+              image_suffix: limelight
+              image_url: https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.1.0-beta-5_limelight-arm64
+
+    runs-on: ${{ matrix.os }}
+    name: "Build image - ${{ matrix.image_url }}"
+
+    steps:
+      # Checkout code.
+      - name: Checkout code
+        uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
+
+      - uses: actions/download-artifact@v2
+        with:
+          name: jar-${{ matrix.artifact-name }}
+
+      - name: Generate image
+        run: |
+          chmod +x scripts/generatePiImage.sh
+          ./scripts/generatePiImage.sh ${{ matrix.image_url }} ${{ matrix.image_suffix }}
 
       - uses: actions/upload-artifact@v3
-        if: ${{ github.event_name != 'pull_request' && (matrix.artifact-name) == 'LinuxArm32' }}
+        name: Upload image
         with:
-          name: image-${{ matrix.artifact-name }}
+          name: image-${{ matrix.image_suffix }}
           path: photonvision*.xz
 
 
   photon-release:
-    needs: [photon-build-package]
+    needs: [photon-build-package, photon-image-generator]
     runs-on: ubuntu-22.04
     steps:
       # Download literally every single artifact. This also downloads client and docs,

build.gradle

@@ -4,7 +4,7 @@ plugins {
     id "com.github.node-gradle.node" version "3.1.1" apply false
     id "edu.wpi.first.GradleJni" version "1.0.0"
     id "edu.wpi.first.GradleVsCode" version "1.1.0"
-    id "edu.wpi.first.NativeUtils" version "2023.10.0" apply false
+    id "edu.wpi.first.NativeUtils" version "2023.11.1" apply false
     id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2"
     id "org.hidetake.ssh" version "2.10.1"
     id 'edu.wpi.first.WpilibTools' version '1.0.0'
@@ -32,18 +32,14 @@ ext {
     pubVersion = versionString
     isDev = pubVersion.startsWith("dev")
 
-    if(project.hasProperty('pionly')) {
-        jniPlatforms = ['linuxarm32']
-    } else if(project.hasProperty('winonly')) {
-        jniPlatforms = ['windowsx86-64']
-    } else if(project.hasProperty('aarch64only')) {
-        jniPlatforms = ['linuxaarch64bionic']
-    } else {
-        jniPlatforms = ['linuxarm64', 'linuxarm32', 'linuxx86-64', 'osxuniversal', 'windowsx86-64']
-    }
-
-    println("Building for archs " + jniPlatforms)
-
+    // A list, for legacy reasons, with only the current platform contained
+    String nativeName = wpilibTools.platformMapper.currentPlatform.platformName;
+    if (nativeName == "linuxx64") nativeName = "linuxx86-64";
+    if (nativeName == "winx64") nativeName = "windowsx86-64";
+    if (nativeName == "macx64") nativeName = "osxx86-64";
+    if (nativeName == "macarm64") nativeName = "osxarm64";
+    jniPlatform = nativeName
+    println("Building for platform " + jniPlatform)
 }
 
 wpilibTools.deps.wpilibVersion = wpilibVersion

photon-client/public/thinclient.html

@@ -304,7 +304,7 @@
             document.getElementById("host").value = host_in;
         }
 
-        if(port_in != "" & host_in != ""){
+        if(port_in != "" && host_in != ""){
             streamStartRequest(); //we got valid inputs, auto-start the stream
         }
 

photon-client/src/components/pipeline/CameraAndPipelineSelect.vue

@@ -257,7 +257,7 @@ export default {
   },
   data: () => {
     return {
-      re: RegExp("^[A-Za-z0-9 \\-)(]*[A-Za-z0-9][A-Za-z0-9 \\-)(.]*$"),
+      re: RegExp("^[A-Za-z0-9_ \\-)(]*[A-Za-z0-9][A-Za-z0-9_ \\-)(.]*$"),
       isCameraNameEdit: false,
       newCameraName: "",
       cameraNameError: "",

photon-client/src/views/LogsView.vue

@@ -12,12 +12,21 @@
         color="secondary"
         style="margin-left: auto;"
         depressed
-        @click="download('photonlog.log', rawLogs.map(it => it.message).join('\n'))"
+        @click="$refs.exportLogFile.click()"
       >
         <v-icon left>
           mdi-download
         </v-icon>
         Download Log
+
+        <!-- Special hidden link that gets 'clicked' when the user exports journalctl logs -->
+        <a
+          ref="exportLogFile"
+          style="color: black; text-decoration: none; display: none"
+          :href="'http://' + this.$address + '/api/settings/photonvision-journalctl.txt'"
+          download="photonvision-journalctl.txt"
+        />
+
       </v-btn>
     </v-card-title>
     <div class="pr-6 pl-6">

photon-client/src/views/PipelineViews/AprilTagTab.vue

@@ -1,196 +1,166 @@
 <template>
   <div>
     <v-select
-      v-model="selectedFamily"
-      dark
-      color="accent"
-      item-color="secondary"
-      label="Select target family"
-      :items="familyList"
-      @input="handlePipelineUpdate('tagFamily', familyList.indexOf(selectedFamily))"
-    />
-    <v-select
-      v-model="selectedModel"
-      dark
-      color="accent"
-      item-color="secondary"
-      label="Select a target model"
-      :items="targetList"
-      item-text="name"
-      item-value="data"
-      @input="handlePipelineUpdate('targetModel', targetList.indexOf(selectedModel) + 6)"
+        v-model="selectedFamily"
+        dark
+        color="accent"
+        item-color="secondary"
+        label="Select target family"
+        :items="familyList"
+        @input="handlePipelineUpdate('tagFamily', familyList.indexOf(selectedFamily))"
     />
     <CVslider
-      v-model="decimate"
-      class="pt-2"
-      slider-cols="8"
-      name="Decimate"
-      min="1"
-      max="8"
-      step="1.0"
-      tooltip="Increases FPS at the expense of range by reducing image resolution initially"
-      @input="handlePipelineData('decimate')"
+        v-model="decimate"
+        class="pt-2"
+        slider-cols="8"
+        name="Decimate"
+        min="1"
+        max="8"
+        step="1.0"
+        tooltip="Increases FPS at the expense of range by reducing image resolution initially"
+        @input="handlePipelineData('decimate')"
     />
     <CVslider
-      v-model="blur"
-      class="pt-2"
-      slider-cols="8"
-      name="Blur"
-      min="0"
-      max="5"
-      step=".01"
-      tooltip="Gaussian blur added to the image, high FPS cost for slightly decreased noise"
-      @input="handlePipelineData('blur')"
+        v-model="blur"
+        class="pt-2"
+        slider-cols="8"
+        name="Blur"
+        min="0"
+        max="5"
+        step=".01"
+        tooltip="Gaussian blur added to the image, high FPS cost for slightly decreased noise"
+        @input="handlePipelineData('blur')"
     />
     <CVslider
-      v-model="threads"
-      class="pt-2"
-      slider-cols="8"
-      name="Threads"
-      min="1"
-      max="8"
-      step="1"
-      tooltip="Number of threads spawned by the AprilTag detector"
-      @input="handlePipelineData('threads')"
+        v-model="threads"
+        class="pt-2"
+        slider-cols="8"
+        name="Threads"
+        min="1"
+        max="8"
+        step="1"
+        tooltip="Number of threads spawned by the AprilTag detector"
+        @input="handlePipelineData('threads')"
     />
     <CVswitch
-      v-model="refineEdges"
-      class="pt-2"
-      slider-cols="8"
-      name="Refine Edges"
-      tooltip="Further refines the apriltag corner position initial estimate, suggested left on"
-      @input="handlePipelineData('refineEdges')"
+        v-model="refineEdges"
+        class="pt-2"
+        slider-cols="8"
+        name="Refine Edges"
+        tooltip="Further refines the apriltag corner position initial estimate, suggested left on"
+        @input="handlePipelineData('refineEdges')"
     />
     <CVslider
-      v-model="hammingDist"
-      class="pt-2 pb-4"
-      slider-cols="8"
-      name="Max error bits"
-      min="0"
-      max="10"
-      step="1"
-      tooltip="Maximum number of error bits to correct; potential tags with more will be thrown out. For smaller tags (like 16h5), set this as low as possible."
-      @input="handlePipelineData('hammingDist')"
+        v-model="decisionMargin"
+        class="pt-2 pb-4"
+        slider-cols="8"
+        name="Decision Margin Cutoff"
+        min="0"
+        max="250"
+        step="1"
+        tooltip="Tags with a 'margin' (decoding quality score) less than this wil be rejected. Increase this to reduce the number of false positive detections"
+        @input="handlePipelineData('decisionMargin')"
     />
     <CVslider
-      v-model="decisionMargin"
-      class="pt-2 pb-4"
-      slider-cols="8"
-      name="Decision Margin Cutoff"
-      min="0"
-      max="250"
-      step="1"
-      tooltip="Tags with a 'margin' (decoding quality score) less than this wil be rejected. Increase this to reduce the number of false positive detections"
-      @input="handlePipelineData('decisionMargin')"
-    />
-    <CVslider
-      v-model="numIterations"
-      class="pt-2 pb-4"
-      slider-cols="8"
-      name="Pose Estimation Iterations"
-      min="0"
-      max="500"
-      step="1"
-      tooltip="Number of iterations the pose estimation algorithm will run, 50-100 is a good starting point"
-      @input="handlePipelineData('numIterations')"
+        v-model="numIterations"
+        class="pt-2 pb-4"
+        slider-cols="8"
+        name="Pose Estimation Iterations"
+        min="0"
+        max="500"
+        step="1"
+        tooltip="Number of iterations the pose estimation algorithm will run, 50-100 is a good starting point"
+        @input="handlePipelineData('numIterations')"
     />
   </div>
 </template>
 
 <script>
-    import CVslider from '../../components/common/cv-slider'
-    import CVswitch from '../../components/common/cv-switch'
+import CVslider from '../../components/common/cv-slider'
+import CVswitch from '../../components/common/cv-switch'
 
-    export default {
-        name: "AprilTag",
-        components: {
-          CVslider,
-          CVswitch,
-        },
-        data() {
-            return {
-              familyList: ["tag36h11", "tag25h9", "tag16h5"],
-              // Selected model is offset (ew) by 6 from the photon ordinal, as we only wanna show the 36h11 and 16h5 options
-              targetList: ['6.5in (36h11) AprilTag', '6in (16h5) AprilTag'], //Keep in sync with TargetModel.java
-            }
-        },
-        computed: {
-          selectedModel: {
-              get() {
-                  let ret = this.$store.getters.currentPipelineSettings.targetModel - 6
-                  return this.targetList[ret];
-              },
-              set(val) {
-                  this.$store.commit("mutatePipeline", {"targetModel": this.targetList.indexOf(val) + 6})
-              }
-          },
-          selectedFamily: {
-            get() {
-                let ret = this.$store.getters.currentPipelineSettings.tagFamily
-                return this.familyList[ret];
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"tagFamily": this.familyList.indexOf(val)})
-            }
-          },
-          decimate: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.decimate
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"decimate": val});
-            }
-          },
-          hammingDist: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.hammingDist
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"hammingDist": val});
-            }
-          },
-          decisionMargin: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.decisionMargin
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"decisionMargin": val});
-            }
-          },
-          numIterations: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.numIterations
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"numIterations": val});
-            }
-          },
-          blur: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.blur
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"blur": val});
-            }
-          },
-          threads: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.threads
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"threads": val});
-            }
-          },
-          refineEdges: {
-            get() {
-                return this.$store.getters.currentPipelineSettings.refineEdges
-            },
-            set(val) {
-                this.$store.commit("mutatePipeline", {"refineEdges": val});
-            }
-          },
-        },
-        methods: {
-        }
+export default {
+  name: "AprilTag",
+  components: {
+    CVslider,
+    CVswitch,
+  },
+  data() {
+    return {
+      familyList: ["tag36h11", "tag25h9", "tag16h5"],
+      // Selected model is offset (ew) by 6 from the photon ordinal, as we only wanna show the 36h11 and 16h5 options
+      targetList: ['6in (16h5) AprilTag'], //Keep in sync with TargetModel.java
     }
+  },
+  computed: {
+    selectedModel: {
+      get() {
+        let ret = this.$store.getters.currentPipelineSettings.targetModel - 6
+        return this.targetList[ret];
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"targetModel": this.targetList.indexOf(val) + 6})
+      }
+    },
+    selectedFamily: {
+      get() {
+        let ret = this.$store.getters.currentPipelineSettings.tagFamily
+        return this.familyList[ret];
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"tagFamily": this.familyList.indexOf(val)})
+      }
+    },
+    decimate: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.decimate
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"decimate": val});
+      }
+    },
+    decisionMargin: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.decisionMargin
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"decisionMargin": val});
+      }
+    },
+    numIterations: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.numIterations
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"numIterations": val});
+      }
+    },
+    blur: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.blur
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"blur": val});
+      }
+    },
+    threads: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.threads
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"threads": val});
+      }
+    },
+    refineEdges: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.refineEdges
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"refineEdges": val});
+      }
+    },
+  },
+  methods: {
+  }
+}
 </script>

photon-client/src/views/SettingsView.vue

@@ -37,7 +37,8 @@
     import Networking from './SettingsViews/Networking'
     import Lighting from "./SettingsViews/Lighting";
     import cvImage from '../components/common/cv-image'
-    import General from "./SettingsViews/General";
+    import Stats from "./SettingsViews/Stats";
+    import DeviceControl from "./SettingsViews/DeviceControl";
 
     export default {
         name: 'SettingsTab',
@@ -69,7 +70,7 @@
             },
             tabList: {
                 get() {
-                    return [General, Networking].concat(this.$store.state.settings.lighting.supported ? Lighting : []);
+                    return [Stats, DeviceControl, Networking].concat(this.$store.state.settings.lighting.supported ? Lighting : []);
                 }
             }
         },

photon-client/src/views/SettingsViews/DeviceControl.vue

@@ -0,0 +1,289 @@
+<template>
+  <div>
+    <v-row>
+      <v-col cols="12" lg="4" md="6">
+        <v-btn color="red" @click="restartProgram()">
+          <v-icon left>
+            mdi-restart
+          </v-icon>
+          Restart PhotonVision
+        </v-btn>
+      </v-col>
+      <v-col cols="12" lg="4" md="6">
+        <v-btn color="red" @click="restartDevice()">
+          <v-icon left>
+            mdi-restart-alert
+          </v-icon>
+          Restart Device
+        </v-btn>
+      </v-col>
+      <v-col cols="12" lg="4">
+        <v-btn color="secondary" @click="$refs.offlineUpdate.click()">
+          <v-icon left>
+            mdi-update
+          </v-icon>
+          Offline Update
+        </v-btn>
+      </v-col>
+    </v-row>
+    <v-divider />
+    <v-row>
+      <v-col cols="12" sm="6">
+        <v-btn color="secondary" @click="$refs.exportSettings.click()">
+          <v-icon left>
+            mdi-download
+          </v-icon>
+          Export Settings
+        </v-btn>
+      </v-col>
+      <v-col cols="12" sm="6">
+        <v-btn color="secondary" @click="$refs.importSettings.click()">
+          <v-icon left>
+            mdi-upload
+          </v-icon>
+          Import Settings
+        </v-btn>
+      </v-col>
+
+      <v-col cols="12" sm="6">
+        <v-btn color="secondary" @click="$refs.exportLogFile.click()">
+          <v-icon left>
+            mdi-file
+          </v-icon>
+          Export current log
+
+          <!-- Special hidden link that gets 'clicked' when the user exports journalctl logs -->
+          <a
+            ref="exportLogFile"
+            style="color: black; text-decoration: none; display: none"
+            :href="
+              'http://' +
+                this.$address +
+                '/api/settings/photonvision-journalctl.txt'
+            "
+            download="photonvision-journalctl.txt"
+          />
+        </v-btn>
+      </v-col>
+
+      <v-col cols="12" sm="6">
+        <v-btn color="secondary" @click="showLogs()">
+          <v-icon left>
+            mdi-bug
+          </v-icon>
+          Show log viewer
+        </v-btn>
+      </v-col>
+    </v-row>
+    <v-snackbar v-model="snack" top :color="snackbar.color" timeout="-1">
+      <span>{{ snackbar.text }}</span>
+    </v-snackbar>
+
+    <!-- Special hidden upload input that gets 'clicked' when the user imports settings -->
+    <input
+      ref="importSettings"
+      type="file"
+      accept=".zip, .json"
+      style="display: none;"
+      @change="readImportedSettings"
+    />
+    <!-- Special hidden link that gets 'clicked' when the user exports settings -->
+    <a
+      ref="exportSettings"
+      style="color: black; text-decoration: none; display: none"
+      :href="
+        'http://' + this.$address + '/api/settings/photonvision_config.zip'
+      "
+      download="photonvision-settings.zip"
+    />
+
+    <!-- Special hidden new jar upload input that gets 'clicked' when the user posts a new .jar -->
+    <input
+      ref="offlineUpdate"
+      type="file"
+      accept=".jar"
+      style="display: none;"
+      @change="doOfflineUpdate"
+    />
+  </div>
+</template>
+
+<script>
+export default {
+  name: "Device Control",
+  data() {
+    return {
+      snack: false,
+      uploadPercentage: 0.0,
+      snackbar: {
+        color: "success",
+        text: "",
+      },
+    };
+  },
+  computed: {
+    settings() {
+      return this.$store.state.settings.general;
+    },
+    version() {
+      return `${this.settings.version}`;
+    },
+    hwModel() {
+      if (this.settings.hardwareModel !== "") {
+        return `${this.settings.hardwareModel}`;
+      } else {
+        return `Unknown`;
+      }
+    },
+    platform() {
+      return `${this.settings.hardwarePlatform}`;
+    },
+    gpuAccel() {
+      return `${this.settings.gpuAcceleration ? "Enabled" : "Unsupported"} ${
+        this.settings.gpuAcceleration
+          ? "(" + this.settings.gpuAcceleration + ")"
+          : ""
+      }`;
+    },
+    metrics() {
+      // console.log(this.$store.state.metrics);
+      return this.$store.state.metrics;
+    },
+  },
+  methods: {
+    restartProgram() {
+      this.axios.post("http://" + this.$address + "/api/restartProgram", {});
+    },
+    restartDevice() {
+      this.axios.post("http://" + this.$address + "/api/restartDevice", {});
+    },
+    readImportedSettings(event) {
+      let formData = new FormData();
+      formData.append("zipData", event.target.files[0]);
+      this.axios
+        .post("http://" + this.$address + "/api/settings/import", formData, {
+          headers: { "Content-Type": "multipart/form-data" },
+        })
+        .then(() => {
+          this.snackbar = {
+            color: "success",
+            text:
+              "Settings imported successfully! PhotonVision will restart in the background...",
+          };
+          this.snack = true;
+        })
+        .catch((err) => {
+          if (err.response) {
+            this.snackbar = {
+              color: "error",
+              text:
+                "Error while uploading settings file! Could not process provided file.",
+            };
+          } else if (err.request) {
+            this.snackbar = {
+              color: "error",
+              text:
+                "Error while uploading settings file! No respond to upload attempt.",
+            };
+          } else {
+            this.snackbar = {
+              color: "error",
+              text: "Error while uploading settings file!",
+            };
+          }
+          this.snack = true;
+        });
+    },
+    doOfflineUpdate(event) {
+      this.snackbar = {
+        color: "secondary",
+        text: "New Software Upload in Process...",
+      };
+      this.snack = true;
+
+      let formData = new FormData();
+      formData.append("jarData", event.target.files[0]);
+      this.axios
+        .post(
+          "http://" + this.$address + "/api/settings/offlineUpdate",
+          formData,
+          {
+            headers: { "Content-Type": "multipart/form-data" },
+            onUploadProgress: function(progressEvent) {
+              this.uploadPercentage = parseInt(
+                Math.round((progressEvent.loaded / progressEvent.total) * 100)
+              );
+              if (this.uploadPercentage < 99.5) {
+                this.snackbar.text =
+                  "New Software Upload in Process, " +
+                  this.uploadPercentage +
+                  "% complete";
+              } else {
+                this.snackbar.text = "Installing uploaded software...";
+              }
+            }.bind(this),
+          }
+        )
+        .then(() => {
+          this.snackbar = {
+            color: "success",
+            text:
+              "New .jar copied successfully! PhotonVision will restart in the background...",
+          };
+          this.snack = true;
+        })
+        .catch((err) => {
+          if (err.response) {
+            this.snackbar = {
+              color: "error",
+              text:
+                "Error while uploading new .jar file! Could not process provided file.",
+            };
+          } else if (err.request) {
+            this.snackbar = {
+              color: "error",
+              text:
+                "Error while uploading new .jar file! No respond to upload attempt.",
+            };
+          } else {
+            this.snackbar = {
+              color: "error",
+              text: "Error while uploading new .jar file!",
+            };
+          }
+          this.snack = true;
+        });
+    },
+    showLogs(event) {
+      event;
+      this.$store.state.logsOverlay = true;
+    },
+  },
+};
+</script>
+
+<style lang="css" scoped>
+.v-btn {
+  width: 100%;
+}
+
+.infoTable {
+  border: 1px solid;
+  border-collapse: separate;
+  border-spacing: 0px;
+  border-radius: 5px;
+  text-align: left;
+  margin-bottom: 10px;
+  width: 100%;
+  display: block;
+  overflow-x: auto;
+}
+
+.infoElem {
+  padding-right: 15px;
+  padding-bottom: 1px;
+  padding-top: 1px;
+  padding-left: 10px;
+  border-right: 1px solid;
+}
+</style>

photon-client/src/views/SettingsViews/Stats.vue

@@ -117,85 +117,11 @@
       </table>
     </v-row>
 
-    <v-row>
-      <v-col
-        cols="12"
-        sm="6"
-        md="4"
-      >
-        <v-btn
-          color="secondary"
-          @click="$refs.exportSettings.click()"
-        >
-          <v-icon left>
-            mdi-download
-          </v-icon>
-          Export Settings
-        </v-btn>
-      </v-col>
-      <v-col
-        cols="12"
-        sm="6"
-        md="4"
-      >
-        <v-btn
-          color="secondary"
-          @click="$refs.importSettings.click()"
-        >
-          <v-icon left>
-            mdi-upload
-          </v-icon>
-          Import Settings
-        </v-btn>
-      </v-col>
-      <v-col
-        cols="12"
-        md="4"
-      >
-        <v-btn
-          color="secondary"
-          @click="$refs.offlineUpdate.click()"
-        >
-          <v-icon left>
-            mdi-update
-          </v-icon>
-          Offline Update
-        </v-btn>
-      </v-col>
-      <v-col
-        cols="12"
-        lg="6"
-      >
-        <v-btn
-          color="red"
-          @click="restartProgram()"
-        >
-          <v-icon left>
-            mdi-restart
-          </v-icon>
-          Restart PhotonVision
-        </v-btn>
-      </v-col>
-      <v-col
-        cols="12"
-        lg="6"
-      >
-        <v-btn
-          color="red"
-          @click="restartDevice()"
-        >
-          <v-icon left>
-            mdi-restart-alert
-          </v-icon>
-          Restart Device
-        </v-btn>
-      </v-col>
-    </v-row>
     <v-snackbar
       v-model="snack"
       top
       :color="snackbar.color"
-      timeout="0"
+      timeout="-1"
     >
       <span>{{ snackbar.text }}</span>
     </v-snackbar>
@@ -230,7 +156,7 @@
 
 <script>
 export default {
-    name: 'General',
+    name: 'Stats',
     data() {
         return {
             snack: false,
@@ -262,8 +188,8 @@ export default {
           return  `${this.settings.gpuAcceleration ? "Enabled" : "Unsupported"} ${this.settings.gpuAcceleration ? "(" + this.settings.gpuAcceleration + ")" : ""}`
         },
         metrics() {
-          console.log(this.$store.state.metrics);
-            return this.$store.state.metrics;
+          // console.log(this.$store.state.metrics);
+          return this.$store.state.metrics;
         }
     },
     methods: {
@@ -349,6 +275,10 @@ export default {
                 this.snack = true;
             });
         },
+        showLogs(event) {
+          event;
+          this.$store.state.logsOverlay = true;
+        }
     }
 }
 </script>

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

@@ -142,8 +142,8 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
 
         ts.rawBytesEntry.set(packet.getData());
 
-        ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get());
-        ts.driverModePublisher.set(driverModeSupplier.getAsBoolean());
+        ts.pipelineIndexPublisher.setDefault(pipelineIndexSupplier.get());
+        ts.driverModePublisher.setDefault(driverModeSupplier.getAsBoolean());
         ts.latencyMillisEntry.set(result.getLatencyMillis());
         ts.hasTargetEntry.set(result.hasTargets());
 

photon-core/src/main/java/org/photonvision/vision/camera/LibcameraGpuSettables.java

@@ -29,13 +29,15 @@ import org.photonvision.vision.processes.VisionSourceSettables;
 
 public class LibcameraGpuSettables extends VisionSourceSettables {
     private FPSRatedVideoMode currentVideoMode;
-    private double lastExposure = 50;
+    private double lastManualExposure = 50;
     private int lastBrightness = 50;
-    private boolean lastExposureMode;
+    private boolean lastAutoExposureActive;
     private int lastGain = 50;
     private Pair<Integer, Integer> lastAwbGains = new Pair<>(18, 18);
     private boolean m_initialized = false;
 
+    private final LibCameraJNI.SensorModel sensorModel;
+
     private ImageRotationMode m_rotationMode;
 
     public void setRotation(ImageRotationMode rotationMode) {
@@ -51,7 +53,7 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
 
         videoModes = new HashMap<>();
 
-        LibCameraJNI.SensorModel sensorModel = LibCameraJNI.getSensorModel();
+        sensorModel = LibCameraJNI.getSensorModel();
 
         if (sensorModel == LibCameraJNI.SensorModel.IMX219) {
             // Settings for the IMX219 sensor, which is used on the Pi Camera Module v2
@@ -72,13 +74,14 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
                     6, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 3280 / 4, 2464 / 4, 15, 20, 1));
         } else if (sensorModel == LibCameraJNI.SensorModel.OV9281) {
             videoModes.put(
-                    0, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 1280, 800, 60, 60, 1));
+                    0, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 320, 240, 30, 30, .39));
             videoModes.put(
                     1, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 1280 / 2, 800 / 2, 60, 60, 1));
             videoModes.put(
-                    2, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 320, 240, 30, 30, .39));
+                    2, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 640, 480, 65, 90, .39));
             videoModes.put(
-                    3, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 640, 480, 65, 90, .39));
+                    3, new FPSRatedVideoMode(VideoMode.PixelFormat.kUnknown, 1280, 800, 60, 60, 1));
+
         } else {
             if (sensorModel == LibCameraJNI.SensorModel.IMX477) {
                 LibcameraGpuSource.logger.warn(
@@ -114,20 +117,33 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
 
     @Override
     public void setAutoExposure(boolean cameraAutoExposure) {
-        lastExposureMode = cameraAutoExposure;
-        // TODO (Matt) -- call LibCameraJNI's auto exposure function, when that exists
+        lastAutoExposureActive = cameraAutoExposure;
         LibCameraJNI.setAutoExposure(cameraAutoExposure);
     }
 
     @Override
     public void setExposure(double exposure) {
-        // Todo (Chris) - for now, handle auto exposure by using -1
-        if (exposure < 0.0) {
-            exposure = -1;
+        if (exposure < 0.0 || lastAutoExposureActive) {
+            // Auto-exposure is active right now, don't set anything.
+            return;
         }
 
-        // TODO convert to uS
-        lastExposure = exposure;
+        // HACKS!
+        // If we set exposure too low, libcamera crashes or slows down
+        // Very weird and smelly
+        // For now, band-aid this by just not setting it lower than the "it breaks" limit
+        // Limit is different depending on camera.
+        if (sensorModel == LibCameraJNI.SensorModel.OV9281) {
+            if (exposure < 6.0) {
+                exposure = 6.0;
+            }
+        } else if (sensorModel == LibCameraJNI.SensorModel.OV5647) {
+            if (exposure < 0.7) {
+                exposure = 0.7;
+            }
+        }
+
+        lastManualExposure = exposure;
         var success = LibCameraJNI.setExposure((int) Math.round(exposure) * 800);
         if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera exposure");
     }
@@ -150,19 +166,25 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
 
     @Override
     public void setRedGain(int red) {
-        lastAwbGains = Pair.of(red, lastAwbGains.getSecond());
-        setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());
+        if (sensorModel != LibCameraJNI.SensorModel.OV9281) {
+            lastAwbGains = Pair.of(red, lastAwbGains.getSecond());
+            setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());
+        }
     }
 
     @Override
     public void setBlueGain(int blue) {
-        lastAwbGains = Pair.of(lastAwbGains.getFirst(), blue);
-        setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());
+        if (sensorModel != LibCameraJNI.SensorModel.OV9281) {
+            lastAwbGains = Pair.of(lastAwbGains.getFirst(), blue);
+            setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());
+        }
     }
 
     public void setAwbGain(int red, int blue) {
-        var success = LibCameraJNI.setAwbGain(red / 10.0, blue / 10.0);
-        if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera AWB gains");
+        if (sensorModel != LibCameraJNI.SensorModel.OV9281) {
+            var success = LibCameraJNI.setAwbGain(red / 10.0, blue / 10.0);
+            if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera AWB gains");
+        }
     }
 
     @Override
@@ -202,8 +224,8 @@ public class LibcameraGpuSettables extends VisionSourceSettables {
 
         // We don't store last settings on the native side, and when you change video mode these get
         // reset on MMAL's end
-        setExposure(lastExposure);
-        setAutoExposure(lastExposureMode);
+        setExposure(lastManualExposure);
+        setAutoExposure(lastAutoExposureActive);
         setBrightness(lastBrightness);
         setGain(lastGain);
         setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());

photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java

@@ -17,20 +17,18 @@
 
 package org.photonvision.vision.frame.consumer;
 
-import edu.wpi.first.networktables.BooleanEntry;
+import edu.wpi.first.networktables.IntegerEntry;
 import edu.wpi.first.networktables.NetworkTable;
 import java.io.File;
 import java.text.DateFormat;
 import java.text.SimpleDateFormat;
 import java.util.Date;
-import java.util.concurrent.locks.ReentrantLock;
 import java.util.function.Consumer;
 import org.opencv.imgcodecs.Imgcodecs;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
-import org.photonvision.common.util.TimedTaskManager;
 import org.photonvision.vision.opencv.CVMat;
 
 public class FileSaveFrameConsumer implements Consumer<CVMat> {
@@ -44,29 +42,27 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
     private NetworkTable subTable;
     private final NetworkTable rootTable;
     private final Logger logger;
-    private boolean prevCommand = false;
+    private long imgSaveCountInternal = 0;
     private String camNickname;
     private String fnamePrefix;
-    private final long CMD_RESET_TIME_MS = 500;
-    private final BooleanEntry entry;
-    // Helps prevent race conditions between user set & auto-reset logic
-    private ReentrantLock lock;
+    private IntegerEntry entry;
 
     public FileSaveFrameConsumer(String camNickname, String streamPrefix) {
-        this.lock = new ReentrantLock();
         this.fnamePrefix = camNickname + "_" + streamPrefix;
         this.ntEntryName = streamPrefix + NT_SUFFIX;
         this.rootTable = NetworkTablesManager.getInstance().kRootTable;
         updateCameraNickname(camNickname);
-        entry = subTable.getBooleanTopic(ntEntryName).getEntry(false);
         this.logger = new Logger(FileSaveFrameConsumer.class, this.camNickname, LogGroup.General);
     }
 
     public void accept(CVMat image) {
         if (image != null && image.getMat() != null && !image.getMat().empty()) {
-            if (lock.tryLock()) {
-                boolean curCommand = entry.get(false);
-                if (curCommand && !prevCommand) {
+            var curCommand = entry.get(); // default to just our current count
+            if (curCommand >= 0) {
+                // Only do something if we got a valid current command
+                if (imgSaveCountInternal < curCommand) {
+                    // Save one frame.
+                    // Create the filename
                     Date now = new Date();
                     String savefile =
                             FILE_PATH
@@ -78,42 +74,32 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
                                     + tf.format(now)
                                     + FILE_EXTENSION;
 
+                    // write to file
                     Imgcodecs.imwrite(savefile, image.getMat());
 
-                    // Help the user a bit - set the NT entry back to false after 500ms
-                    TimedTaskManager.getInstance().addOneShotTask(this::resetCommand, CMD_RESET_TIME_MS);
-
+                    // Count one more image saved
+                    imgSaveCountInternal++;
                     logger.info("Saved new image at " + savefile);
-                } else if (!curCommand) {
-                    // If the entry is currently false, set it again. This will make sure it shows up on the
-                    // dashboard.
-                    entry.set(false);
+
+                } else if (imgSaveCountInternal > curCommand) {
+                    imgSaveCountInternal = curCommand;
                 }
-
-                prevCommand = curCommand;
-                lock.unlock();
-            }
-        }
-    }
-
-    private void resetCommand() {
-        lock.lock();
-        this.subTable.getEntry(ntEntryName).setBoolean(false);
-        lock.unlock();
-    }
-
-    private void removeEntries() {
-        if (this.subTable != null) {
-            if (this.subTable.containsKey(ntEntryName)) {
-                this.subTable.getEntry(ntEntryName).close();
             }
         }
     }
 
     public void updateCameraNickname(String newCameraNickname) {
-        removeEntries();
+        // Remove existing entries
+        if (this.subTable != null) {
+            if (this.subTable.containsKey(ntEntryName)) {
+                this.subTable.getEntry(ntEntryName).close();
+            }
+        }
+
+        // Recreate and re-init network tables structure
         this.camNickname = newCameraNickname;
         this.subTable = rootTable.getSubTable(this.camNickname);
-        resetCommand();
+        this.subTable.getEntry(ntEntryName).setInteger(imgSaveCountInternal);
+        this.entry = subTable.getIntegerTopic(ntEntryName).getEntry(-1); // Default negative
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindCirclesPipe.java

@@ -66,7 +66,7 @@ public class FindCirclesPipe
                 1.0,
                 params.minDist,
                 params.maxCannyThresh,
-                params.accuracy,
+                Math.max(1.0, params.accuracy),
                 minRadius,
                 maxRadius);
         // Great, we now found the center point of the circle and it's radius, but we have no idea what

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java

@@ -69,28 +69,7 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
         // target model for the draw 3d targets pipeline to work...
 
         // for now, hard code tag width based on enum value
-        double tagWidth;
-
-        // This needs
-        switch (settings.targetModel) {
-            case k200mmAprilTag:
-                {
-                    tagWidth = Units.inchesToMeters(3.25 * 2);
-                    break;
-                }
-            case k6in_16h5:
-                {
-                    tagWidth = Units.inchesToMeters(3 * 2);
-                    break;
-                }
-            default:
-                {
-                    // guess at 200mm?? If it's zero everything breaks, but it should _never_ be zero. Unless
-                    // users select the wrong model...
-                    tagWidth = 0.16;
-                    break;
-                }
-        }
+        double tagWidth = Units.inchesToMeters(3 * 2); // for 6in 16h5 tag.
 
         // AprilTagDetectorParams aprilTagDetectionParams =
         //         new AprilTagDetectorParams(

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java

@@ -24,17 +24,15 @@ import org.photonvision.vision.target.TargetModel;
 
 @JsonTypeName("AprilTagPipelineSettings")
 public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
-    public AprilTagFamily tagFamily = AprilTagFamily.kTag36h11;
+    public AprilTagFamily tagFamily = AprilTagFamily.kTag16h5;
     public int decimate = 1;
     public double blur = 0;
-    public int threads = 1;
+    public int threads = 4; // Multiple threads seems to be better performance on most platforms
     public boolean debug = false;
     public boolean refineEdges = true;
     public int numIterations = 200;
-
-    // TODO is this a legit, reasonable default?
-    public int hammingDist = 1;
-    public int decisionMargin = 30;
+    public int hammingDist = 0;
+    public int decisionMargin = 35;
 
     // 3d settings
 
@@ -42,7 +40,9 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
         super();
         pipelineType = PipelineType.AprilTag;
         outputShowMultipleTargets = true;
-        targetModel = TargetModel.k200mmAprilTag;
+        targetModel = TargetModel.k6in_16h5;
+        cameraExposure = 20;
+        cameraAutoExposure = false;
         ledMode = false;
     }
 

photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java

@@ -40,14 +40,14 @@ public class CVPipelineSettings implements Cloneable {
     public String pipelineNickname = "New Pipeline";
     public boolean cameraAutoExposure = false;
     // manual exposure only used if cameraAutoExposure if false
-    public double cameraExposure = 100;
+    public double cameraExposure = 20;
     public int cameraBrightness = 50;
     // Currently only used by a few cameras (notably the zero-copy Pi Camera driver) with the Gain
     // quirk
-    public int cameraGain = 50;
+    public int cameraGain = 75;
     // Currently only used by the zero-copy Pi Camera driver
-    public int cameraRedGain = 18;
-    public int cameraBlueGain = 24;
+    public int cameraRedGain = 11;
+    public int cameraBlueGain = 20;
     public int cameraVideoModeIndex = 0;
     public FrameDivisor streamingFrameDivisor = FrameDivisor.NONE;
     public boolean ledMode = false;

photon-core/src/main/java/org/photonvision/vision/pipeline/Calibration3dPipelineSettings.java

@@ -30,7 +30,7 @@ public class Calibration3dPipelineSettings extends AdvancedPipelineSettings {
 
     public Calibration3dPipelineSettings() {
         super();
-
+        this.cameraAutoExposure = true;
         this.inputShouldShow = true;
         this.outputShouldShow = true;
     }

photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipelineSettings.java

@@ -45,6 +45,7 @@ public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
     public ColoredShapePipelineSettings() {
         super();
         pipelineType = PipelineType.ColoredShape;
+        cameraExposure = 20;
     }
 
     @Override

photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java

@@ -77,18 +77,22 @@ public class DriverModePipeline
         // apply pipes
         var inputMat = frame.colorImage.getMat();
 
-        totalNanos += resizeImagePipe.run(inputMat).nanosElapsed;
+        boolean emptyIn = inputMat.empty();
 
-        if (!accelerated) {
-            var rotateImageResult = rotateImagePipe.run(inputMat);
-            totalNanos += rotateImageResult.nanosElapsed;
+        if (!emptyIn) {
+            if (!accelerated) {
+                var rotateImageResult = rotateImagePipe.run(inputMat);
+                totalNanos += rotateImageResult.nanosElapsed;
+            }
+
+            totalNanos += resizeImagePipe.run(inputMat).nanosElapsed;
+
+            var draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(inputMat, List.of()));
+
+            // calculate elapsed nanoseconds
+            totalNanos += draw2dCrosshairResult.nanosElapsed;
         }
 
-        var draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(inputMat, List.of()));
-
-        // calculate elapsed nanoseconds
-        totalNanos += draw2dCrosshairResult.nanosElapsed;
-
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 

photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipelineSettings.java

@@ -31,5 +31,6 @@ public class DriverModePipelineSettings extends CVPipelineSettings {
         pipelineIndex = PipelineManager.DRIVERMODE_INDEX;
         pipelineType = PipelineType.DriverMode;
         inputShouldShow = true;
+        cameraAutoExposure = true;
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java

@@ -110,65 +110,71 @@ public class OutputStreamPipeline {
         boolean inEmpty = inMat.empty();
         if (!inEmpty)
             sumPipeNanosElapsed += pipeProfileNanos[0] = resizeImagePipe.run(inMat).nanosElapsed;
-        sumPipeNanosElapsed += pipeProfileNanos[1] = resizeImagePipe.run(outMat).nanosElapsed;
 
-        // Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
-        if (outMat.channels() == 1) {
-            var outputMatPipeResult = outputMatPipe.run(outMat);
-            sumPipeNanosElapsed += pipeProfileNanos[2] = outputMatPipeResult.nanosElapsed;
-        } else {
-            pipeProfileNanos[2] = 0;
-        }
+        boolean outEmpty = outMat.empty();
+        if (!outEmpty)
+            sumPipeNanosElapsed += pipeProfileNanos[1] = resizeImagePipe.run(outMat).nanosElapsed;
 
-        // Draw 2D Crosshair on output
-        var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
-        sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
-
-        if (!(settings instanceof AprilTagPipelineSettings)) {
-            // If we're processing anything other than Apriltags...
-
-            var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
-            sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
-
-            if (settings.solvePNPEnabled) {
-                // Draw 3D Targets on input and output if possible
-                pipeProfileNanos[5] = 0;
-                pipeProfileNanos[6] = 0;
-                pipeProfileNanos[7] = 0;
-
-                var drawOnOutputResult = draw3dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[8] = drawOnOutputResult.nanosElapsed;
+        // Only attempt drawing on a non-empty frame
+        if (!outEmpty) {
+            // Convert single-channel HSV output mat to 3-channel BGR in preparation for streaming
+            if (outMat.channels() == 1) {
+                var outputMatPipeResult = outputMatPipe.run(outMat);
+                sumPipeNanosElapsed += pipeProfileNanos[2] = outputMatPipeResult.nanosElapsed;
             } else {
-                // Only draw 2d targets
-                pipeProfileNanos[5] = 0;
-
-                var draw2dTargetsOnOutput = draw2dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[6] = draw2dTargetsOnOutput.nanosElapsed;
-
-                pipeProfileNanos[7] = 0;
-                pipeProfileNanos[8] = 0;
+                pipeProfileNanos[2] = 0;
             }
 
-        } else {
-            // If we are doing apriltags...
-            if (settings.solvePNPEnabled) {
-                // Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
-                pipeProfileNanos[5] = 0;
-                pipeProfileNanos[6] = 0;
+            // Draw 2D Crosshair on output
+            var draw2dCrosshairResultOnInput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
+            sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
 
-                var drawOnInputResult = draw3dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+            if (!(settings instanceof AprilTagPipelineSettings)) {
+                // If we're processing anything other than Apriltags...
 
-                pipeProfileNanos[8] = 0;
+                var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
+                sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
+
+                if (settings.solvePNPEnabled) {
+                    // Draw 3D Targets on input and output if possible
+                    pipeProfileNanos[5] = 0;
+                    pipeProfileNanos[6] = 0;
+                    pipeProfileNanos[7] = 0;
+
+                    var drawOnOutputResult = draw3dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[8] = drawOnOutputResult.nanosElapsed;
+                } else {
+                    // Only draw 2d targets
+                    pipeProfileNanos[5] = 0;
+
+                    var draw2dTargetsOnOutput = draw2dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[6] = draw2dTargetsOnOutput.nanosElapsed;
+
+                    pipeProfileNanos[7] = 0;
+                    pipeProfileNanos[8] = 0;
+                }
 
             } else {
-                // Draw 2d apriltag markers
-                var draw2dTargetsOnInput = draw2dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
+                // If we are doing apriltags...
+                if (settings.solvePNPEnabled) {
+                    // Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
+                    pipeProfileNanos[5] = 0;
+                    pipeProfileNanos[6] = 0;
 
-                pipeProfileNanos[6] = 0;
-                pipeProfileNanos[7] = 0;
-                pipeProfileNanos[8] = 0;
+                    var drawOnInputResult = draw3dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+
+                    pipeProfileNanos[8] = 0;
+
+                } else {
+                    // Draw 2d apriltag markers
+                    var draw2dTargetsOnInput = draw2dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
+
+                    pipeProfileNanos[6] = 0;
+                    pipeProfileNanos[7] = 0;
+                    pipeProfileNanos[8] = 0;
+                }
             }
         }
 

photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipelineSettings.java

@@ -27,5 +27,7 @@ public class ReflectivePipelineSettings extends AdvancedPipelineSettings {
     public ReflectivePipelineSettings() {
         super();
         pipelineType = PipelineType.Reflective;
+        cameraExposure = 6;
+        cameraGain = 20;
     }
 }

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

@@ -22,6 +22,9 @@ import java.util.Arrays;
 import java.util.Comparator;
 import java.util.List;
 import org.photonvision.common.configuration.CameraConfiguration;
+import org.photonvision.common.configuration.ConfigManager;
+import org.photonvision.common.dataflow.DataChangeService;
+import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.vision.pipeline.*;
@@ -48,7 +51,7 @@ public class PipelineManager {
      * <br>
      * Used only when switching from any of the built-in pipelines back to a user-created pipeline.
      */
-    private int lastPipelineIndex;
+    private int lastUserPipelineIdx;
 
     /**
      * Creates a PipelineManager with a DriverModePipeline, a Calibration3dPipeline, and all provided
@@ -141,7 +144,7 @@ public class PipelineManager {
      *
      * @return The currently active pipeline.
      */
-    public CVPipeline getCurrentUserPipeline() {
+    public CVPipeline getCurrentPipeline() {
         if (currentPipelineIndex < 0) {
             switch (currentPipelineIndex) {
                 case CAL_3D_INDEX:
@@ -151,23 +154,7 @@ public class PipelineManager {
             }
         }
 
-        var desiredPipelineSettings = userPipelineSettings.get(currentPipelineIndex);
-        //        if (currentPipeline.getSettings().pipelineIndex !=
-        // desiredPipelineSettings.pipelineIndex) {
-        //            switch (desiredPipelineSettings.pipelineType) {
-        //                case Reflective:
-        //                    currentPipeline =
-        //                            new ReflectivePipeline((ReflectivePipelineSettings)
-        // desiredPipelineSettings);
-        //                    break;
-        //                case ColoredShape:
-        //                    currentPipeline =
-        //                            new ColoredShapePipeline((ColoredShapePipelineSettings)
-        // desiredPipelineSettings);
-        //                    break;
-        //            }
-        //        }
-
+        // Just return the current user pipeline, we're not on aa built-in one
         return currentUserPipeline;
     }
 
@@ -186,20 +173,21 @@ public class PipelineManager {
      * All externally accessible methods that intend to change the active pipeline MUST go through
      * here to ensure all proper steps are taken.
      *
-     * @param index Index of pipeline to be active
+     * @param newIndex Index of pipeline to be active
      */
-    private void setPipelineInternal(int index) {
-        if (index < 0) {
-            lastPipelineIndex = currentPipelineIndex;
+    private void setPipelineInternal(int newIndex) {
+        if (newIndex < 0 && currentPipelineIndex >= 0) {
+            // Transitioning to a built-in pipe, save off the current user one
+            lastUserPipelineIdx = currentPipelineIndex;
         }
 
-        if (userPipelineSettings.size() - 1 < index) {
+        if (userPipelineSettings.size() - 1 < newIndex) {
             logger.warn("User attempted to set index to non-existent pipeline!");
             return;
         }
 
-        currentPipelineIndex = index;
-        if (index >= 0) {
+        currentPipelineIndex = newIndex;
+        if (newIndex >= 0) {
             var desiredPipelineSettings = userPipelineSettings.get(currentPipelineIndex);
             switch (desiredPipelineSettings.pipelineType) {
                 case Reflective:
@@ -222,6 +210,11 @@ public class PipelineManager {
                     break;
             }
         }
+
+        DataChangeService.getInstance()
+                .publishEvent(
+                        new OutgoingUIEvent<>(
+                                "fullsettings", ConfigManager.getInstance().getConfig().toHashMap()));
     }
 
     /**
@@ -233,7 +226,7 @@ public class PipelineManager {
      */
     public void setCalibrationMode(boolean wantsCalibration) {
         if (!wantsCalibration) calibration3dPipeline.finishCalibration();
-        setPipelineInternal(wantsCalibration ? CAL_3D_INDEX : lastPipelineIndex);
+        setPipelineInternal(wantsCalibration ? CAL_3D_INDEX : lastUserPipelineIdx);
     }
 
     /**
@@ -244,7 +237,7 @@ public class PipelineManager {
      * @param state True to enter driver mode, false to exit driver mode.
      */
     public void setDriverMode(boolean state) {
-        setPipelineInternal(state ? DRIVERMODE_INDEX : lastPipelineIndex);
+        setPipelineInternal(state ? DRIVERMODE_INDEX : lastUserPipelineIdx);
     }
 
     /**

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

@@ -104,14 +104,14 @@ public class VisionModule {
         if (cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
             pipelineManager.userPipelineSettings.forEach(
                     it -> {
-                        if (it.cameraGain == -1) it.cameraGain = 20; // Sane default
+                        if (it.cameraGain == -1) it.cameraGain = 75; // Sane default
                     });
         }
         if (cameraQuirks.hasQuirk(CameraQuirk.AWBGain)) {
             pipelineManager.userPipelineSettings.forEach(
                     it -> {
-                        if (it.cameraRedGain == -1) it.cameraRedGain = 16; // Sane defaults
-                        if (it.cameraBlueGain == -1) it.cameraBlueGain = 16;
+                        if (it.cameraRedGain == -1) it.cameraRedGain = 11; // Sane defaults
+                        if (it.cameraBlueGain == -1) it.cameraBlueGain = 20;
                     });
         }
 
@@ -120,7 +120,7 @@ public class VisionModule {
         this.visionRunner =
                 new VisionRunner(
                         this.visionSource.getFrameProvider(),
-                        this.pipelineManager::getCurrentUserPipeline,
+                        this.pipelineManager::getCurrentPipeline,
                         this::consumeResult,
                         this.cameraQuirks);
         this.streamRunnable = new StreamRunnable(new OutputStreamPipeline());
@@ -401,7 +401,7 @@ public class VisionModule {
         }
         if (cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
             // If the gain is disabled for some reason, re-enable it
-            if (pipelineSettings.cameraGain == -1) pipelineSettings.cameraGain = 20;
+            if (pipelineSettings.cameraGain == -1) pipelineSettings.cameraGain = 75;
             visionSource.getSettables().setGain(Math.max(0, pipelineSettings.cameraGain));
         } else {
             pipelineSettings.cameraGain = -1;
@@ -409,8 +409,8 @@ public class VisionModule {
 
         if (cameraQuirks.hasQuirk(CameraQuirk.AWBGain)) {
             // If the AWB gains are disabled for some reason, re-enable it
-            if (pipelineSettings.cameraRedGain == -1) pipelineSettings.cameraRedGain = 16;
-            if (pipelineSettings.cameraBlueGain == -1) pipelineSettings.cameraBlueGain = 16;
+            if (pipelineSettings.cameraRedGain == -1) pipelineSettings.cameraRedGain = 11;
+            if (pipelineSettings.cameraBlueGain == -1) pipelineSettings.cameraBlueGain = 20;
             visionSource.getSettables().setRedGain(Math.max(0, pipelineSettings.cameraRedGain));
             visionSource.getSettables().setBlueGain(Math.max(0, pipelineSettings.cameraBlueGain));
         } else {
@@ -578,7 +578,7 @@ public class VisionModule {
     }
 
     public void setTargetModel(TargetModel targetModel) {
-        var settings = pipelineManager.getCurrentUserPipeline().getSettings();
+        var settings = pipelineManager.getCurrentPipeline().getSettings();
         if (settings instanceof ReflectivePipelineSettings) {
             ((ReflectivePipelineSettings) settings).targetModel = targetModel;
             saveAndBroadcastAll();

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

@@ -59,7 +59,7 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
 
                 var propName = wsEvent.propertyName;
                 var newPropValue = wsEvent.data;
-                var currentSettings = parentModule.pipelineManager.getCurrentUserPipeline().getSettings();
+                var currentSettings = parentModule.pipelineManager.getCurrentPipeline().getSettings();
 
                 // special case for non-PipelineSetting changes
                 switch (propName) {

photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java

@@ -24,6 +24,7 @@ import org.junit.jupiter.api.Assertions;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 import org.photonvision.common.util.TestUtils;
+import org.photonvision.vision.apriltag.AprilTagFamily;
 import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.frame.provider.FileFrameProvider;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
@@ -46,6 +47,7 @@ public class AprilTagTest {
         pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
         pipeline.getSettings().cornerDetectionUseConvexHulls = true;
         pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
+        pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11;
 
         var frameProvider =
                 new FileFrameProvider(

photon-lib/build.gradle

@@ -32,6 +32,7 @@ dependencies {
     implementation wpilibTools.deps.wpilibJava("hal")
     implementation wpilibTools.deps.wpilibJava("ntcore")
     implementation wpilibTools.deps.wpilibJava("wpilibj")
+    implementation wpilibTools.deps.wpilibJava("apriltag")
 
     // Junit
     testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2")
@@ -64,6 +65,7 @@ model {
                 }
             }
             nativeUtils.useRequiredLibrary(it, "wpilib_shared")
+            nativeUtils.useRequiredLibrary(it, "vision_shared")
         }
     }
     testSuites {
@@ -78,6 +80,7 @@ model {
             }
 
             nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
+            nativeUtils.useRequiredLibrary(it, "vision_shared")
             nativeUtils.useRequiredLibrary(it, "googletest_static")
         }
     }

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

@@ -56,7 +56,7 @@ public class PhotonCamera {
     DoubleArrayPublisher targetPoseEntry;
     DoublePublisher targetSkewEntry;
     StringSubscriber versionEntry;
-    BooleanPublisher inputSaveImgEntry, outputSaveImgEntry;
+    IntegerEntry inputSaveImgEntry, outputSaveImgEntry;
     IntegerEntry pipelineIndexEntry, ledModeEntry;
     IntegerSubscriber heartbeatEntry;
 
@@ -111,8 +111,8 @@ public class PhotonCamera {
         path = rootTable.getPath();
         rawBytesEntry = rootTable.getRawTopic("rawBytes").subscribe("rawBytes", new byte[] {});
         driverModeEntry = rootTable.getBooleanTopic("driverMode").getEntry(false);
-        inputSaveImgEntry = rootTable.getBooleanTopic("inputSaveImgCmd").getEntry(false);
-        outputSaveImgEntry = rootTable.getBooleanTopic("outputSaveImgCmd").getEntry(false);
+        inputSaveImgEntry = rootTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
+        outputSaveImgEntry = rootTable.getIntegerTopic("outputSaveImgCmd").getEntry(0);
         pipelineIndexEntry = rootTable.getIntegerTopic("pipelineIndex").getEntry(0);
         heartbeatEntry = rootTable.getIntegerTopic("heartbeat").subscribe(-1);
         ledModeEntry = mainTable.getIntegerTopic("ledMode").getEntry(-1);
@@ -181,7 +181,7 @@ public class PhotonCamera {
      * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
      */
     public void takeInputSnapshot() {
-        inputSaveImgEntry.set(true);
+        inputSaveImgEntry.set(inputSaveImgEntry.get() + 1);
     }
 
     /**
@@ -191,7 +191,7 @@ public class PhotonCamera {
      * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
      */
     public void takeOutputSnapshot() {
-        outputSaveImgEntry.set(true);
+        outputSaveImgEntry.set(outputSaveImgEntry.get() + 1);
     }
 
     /**
@@ -280,7 +280,7 @@ public class PhotonCamera {
             prevHeartbeatValue = curHeartbeat;
         }
 
-        return ((now - prevHeartbeatChangeTime) > HEARBEAT_DEBOUNCE_SEC);
+        return ((now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC);
     }
 
     private void verifyVersion() {

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

@@ -24,7 +24,9 @@
 
 package org.photonvision;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
@@ -33,7 +35,9 @@ import edu.wpi.first.wpilibj.DriverStation;
 import java.util.ArrayList;
 import java.util.HashSet;
 import java.util.List;
-import java.util.Map;
+import java.util.Optional;
+import java.util.Set;
+import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 public class RobotPoseEstimator {
@@ -50,7 +54,7 @@ public class RobotPoseEstimator {
      *       calculated
      * </ul>
      */
-    enum PoseStrategy {
+    public enum PoseStrategy {
         LOWEST_AMBIGUITY,
         CLOSEST_TO_CAMERA_HEIGHT,
         CLOSEST_TO_REFERENCE_POSE,
@@ -58,13 +62,13 @@ public class RobotPoseEstimator {
         AVERAGE_BEST_TARGETS
     }
 
-    private Map<Integer, Pose3d> aprilTags;
+    private AprilTagFieldLayout aprilTags;
     private PoseStrategy strategy;
-    private ArrayList<Pair<PhotonCamera, Transform3d>> cameras;
+    private List<Pair<PhotonCamera, Transform3d>> cameras;
     private Pose3d lastPose;
 
     private Pose3d referencePose;
-    private HashSet<Integer> reportedErrors;
+    private Set<Integer> reportedErrors;
 
     /**
      * Create a new RobotPoseEstimator.
@@ -72,15 +76,16 @@ public class RobotPoseEstimator {
      * <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>(); <p> map.put(1, new
      * Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is at (1.0,2.0,3.0) </code> }
      *
-     * @param aprilTags A Map linking AprilTag IDs to Pose3ds with respect to the FIRST field.
+     * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with respect to the
+     *     FIRST field.
      * @param strategy The strategy it should use to determine the best pose.
      * @param cameras An ArrayList of Pairs of PhotonCameras and their respective Transform3ds from
      *     the center of the robot to the cameras.
      */
     public RobotPoseEstimator(
-            Map<Integer, Pose3d> aprilTags,
+            AprilTagFieldLayout aprilTags,
             PoseStrategy strategy,
-            ArrayList<Pair<PhotonCamera, Transform3d>> cameras) {
+            List<Pair<PhotonCamera, Transform3d>> cameras) {
         this.aprilTags = aprilTags;
         this.strategy = strategy;
         this.cameras = cameras;
@@ -91,50 +96,57 @@ public class RobotPoseEstimator {
     /**
      * Update the estimated pose using the selected strategy.
      *
-     * @return The updated estimated pose and the latency in milliseconds
+     * @return The updated estimated pose and the latency in milliseconds Estimated pose may be null
+     *     if no targets were seen
      */
-    public Pair<Pose3d, Double> update() {
+    public Optional<Pair<Pose3d, Double>> update() {
         if (cameras.isEmpty()) {
             DriverStation.reportError("[RobotPoseEstimator] Missing any camera!", false);
-            return Pair.of(lastPose, 0.);
+            return Optional.empty();
         }
-        Pair<Pose3d, Double> pair;
+
+        Pair<Pose3d, Double> pair = getResultFromActiveStrategy();
+
+        if (pair != null) {
+            lastPose = pair.getFirst();
+        }
+
+        return Optional.ofNullable(pair);
+    }
+
+    private Pair<Pose3d, Double> getResultFromActiveStrategy() {
         switch (strategy) {
             case LOWEST_AMBIGUITY:
-                pair = lowestAmbiguityStrategy();
-                lastPose = pair.getFirst();
-                return pair;
+                return lowestAmbiguityStrategy();
             case CLOSEST_TO_CAMERA_HEIGHT:
-                pair = closestToCameraHeightStrategy();
-                lastPose = pair.getFirst();
-                return pair;
+                return closestToCameraHeightStrategy();
             case CLOSEST_TO_REFERENCE_POSE:
-                pair = closestToReferencePoseStrategy();
-                lastPose = pair.getFirst();
-                return pair;
+                return closestToReferencePoseStrategy();
             case CLOSEST_TO_LAST_POSE:
-                referencePose = lastPose;
-                pair = closestToReferencePoseStrategy();
-                lastPose = pair.getFirst();
-                return pair;
+                return closestToLastPoseStrategy();
             case AVERAGE_BEST_TARGETS:
-                pair = averageBestTargetsStrategy();
-                lastPose = pair.getFirst();
-                return pair;
+                return averageBestTargetsStrategy();
             default:
                 DriverStation.reportError("[RobotPoseEstimator] Invalid pose strategy!", false);
-                return Pair.of(lastPose, 0.);
+                return null;
         }
     }
 
     private Pair<Pose3d, Double> lowestAmbiguityStrategy() {
-        // Loop over each ambiguity of all the cameras
         int lowestAI = -1;
         int lowestAJ = -1;
         double lowestAmbiguityScore = 10;
+        ArrayList<PhotonPipelineResult> results = new ArrayList<PhotonPipelineResult>(cameras.size());
+
+        // Sample result from each camera
         for (int i = 0; i < cameras.size(); i++) {
             Pair<PhotonCamera, Transform3d> p = cameras.get(i);
-            List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
+            results.add(p.getFirst().getLatestResult());
+        }
+
+        // Loop over each ambiguity of all the cameras
+        for (int i = 0; i < cameras.size(); i++) {
+            List<PhotonTrackedTarget> targets = results.get(i).targets;
             for (int j = 0; j < targets.size(); j++) {
                 if (targets.get(j).getPoseAmbiguity() < lowestAmbiguityScore) {
                     lowestAI = i;
@@ -144,57 +156,45 @@ public class RobotPoseEstimator {
             }
         }
 
-        // No targets, return the last pose
+        // No targets, return null
         if (lowestAI == -1 || lowestAJ == -1) {
-            return Pair.of(lastPose, 0.);
+            return null;
         }
 
         // Pick the lowest and do the heavy calculations
-        PhotonTrackedTarget bestTarget =
-                cameras.get(lowestAI).getFirst().getLatestResult().targets.get(lowestAJ);
+        PhotonTrackedTarget bestTarget = results.get(lowestAI).targets.get(lowestAJ);
 
-        // If the map doesn't contain the ID fail
-        if (!aprilTags.containsKey(bestTarget.getFiducialId())) {
-            if (!reportedErrors.contains(bestTarget.getFiducialId())) {
-                DriverStation.reportError(
-                        "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
-                                + bestTarget.getFiducialId(),
-                        false);
-                reportedErrors.add(bestTarget.getFiducialId());
-            }
-            return Pair.of(lastPose, 0.);
+        Optional<Pose3d> fiducialPose = aprilTags.getTagPose(bestTarget.getFiducialId());
+        if (fiducialPose.isEmpty()) {
+            reportFiducialPoseError(bestTarget.getFiducialId());
+            return null;
         }
 
         return Pair.of(
-                aprilTags
-                        .get(bestTarget.getFiducialId())
+                fiducialPose
+                        .get()
                         .transformBy(bestTarget.getBestCameraToTarget().inverse())
                         .transformBy(cameras.get(lowestAI).getSecond().inverse()),
-                cameras.get(lowestAI).getFirst().getLatestResult().getLatencyMillis());
+                results.get(lowestAI).getLatencyMillis());
     }
 
     private Pair<Pose3d, Double> closestToCameraHeightStrategy() {
-        double smallestHeightDifference = 10e9;
-        double mili = 0;
-        Pose3d pose = lastPose;
+        double smallestHeightDifference = Double.MAX_VALUE;
+        double latency = 0;
+        Pose3d pose = null;
 
         for (int i = 0; i < cameras.size(); i++) {
             Pair<PhotonCamera, Transform3d> p = cameras.get(i);
-            List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
+            var result = p.getFirst().getLatestResult();
+            List<PhotonTrackedTarget> targets = result.targets;
             for (int j = 0; j < targets.size(); j++) {
                 PhotonTrackedTarget target = targets.get(j);
-                // If the map doesn't contain the ID fail
-                if (!aprilTags.containsKey(target.getFiducialId())) {
-                    if (!reportedErrors.contains(target.getFiducialId())) {
-                        DriverStation.reportWarning(
-                                "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
-                                        + target.getFiducialId(),
-                                false);
-                        reportedErrors.add(target.getFiducialId());
-                    }
+                Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
+                if (fiducialPose.isEmpty()) {
+                    reportFiducialPoseError(target.getFiducialId());
                     continue;
                 }
-                Pose3d targetPose = aprilTags.get(target.getFiducialId());
+                Pose3d targetPose = fiducialPose.get();
                 double alternativeDifference =
                         Math.abs(
                                 p.getSecond().getZ()
@@ -205,17 +205,24 @@ public class RobotPoseEstimator {
                                         - targetPose.transformBy(target.getBestCameraToTarget().inverse()).getZ());
                 if (alternativeDifference < smallestHeightDifference) {
                     smallestHeightDifference = alternativeDifference;
-                    pose = targetPose.transformBy(target.getAlternateCameraToTarget().inverse());
-                    mili = p.getFirst().getLatestResult().getLatencyMillis();
+                    pose =
+                            targetPose
+                                    .transformBy(target.getAlternateCameraToTarget().inverse())
+                                    .transformBy(p.getSecond().inverse());
+                    latency = result.getLatencyMillis();
                 }
                 if (bestDifference < smallestHeightDifference) {
                     smallestHeightDifference = bestDifference;
-                    pose = targetPose.transformBy(target.getBestCameraToTarget().inverse());
-                    mili = p.getFirst().getLatestResult().getLatencyMillis();
+                    pose =
+                            targetPose
+                                    .transformBy(target.getBestCameraToTarget().inverse())
+                                    .transformBy(p.getSecond().inverse());
+                    latency = result.getLatencyMillis();
                 }
             }
         }
-        return Pair.of(pose, mili);
+
+        return Pair.of(pose, latency);
     }
 
     private Pair<Pose3d, Double> closestToReferencePoseStrategy() {
@@ -223,51 +230,51 @@ public class RobotPoseEstimator {
             DriverStation.reportError(
                     "[RobotPoseEstimator] Tried to use reference pose strategy without setting the reference!",
                     false);
-            return Pair.of(lastPose, 0.);
+            return null;
         }
         double smallestDifference = 10e9;
-        double mili = 0;
-        Pose3d pose = lastPose;
+        double latency = 0;
+        Pose3d pose = null;
         for (int i = 0; i < cameras.size(); i++) {
             Pair<PhotonCamera, Transform3d> p = cameras.get(i);
-            List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
+            var result = p.getFirst().getLatestResult();
+            List<PhotonTrackedTarget> targets = result.targets;
             for (int j = 0; j < targets.size(); j++) {
                 PhotonTrackedTarget target = targets.get(j);
-                // If the map doesn't contain the ID fail
-                if (!aprilTags.containsKey(target.getFiducialId())) {
-                    if (!reportedErrors.contains(target.getFiducialId())) {
-                        DriverStation.reportWarning(
-                                "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
-                                        + target.getFiducialId(),
-                                false);
-                        reportedErrors.add(target.getFiducialId());
-                    }
+                Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
+                if (fiducialPose.isEmpty()) {
+                    reportFiducialPoseError(target.getFiducialId());
                     continue;
                 }
-                Pose3d targetPose = aprilTags.get(target.getFiducialId());
-                double alternativeDifference =
-                        Math.abs(
-                                calculateDifference(
-                                        referencePose,
-                                        targetPose.transformBy(target.getAlternateCameraToTarget().inverse())));
-                double bestDifference =
-                        Math.abs(
-                                calculateDifference(
-                                        referencePose,
-                                        targetPose.transformBy(target.getBestCameraToTarget().inverse())));
+                Pose3d targetPose = fiducialPose.get();
+                Pose3d botBestPose =
+                        targetPose
+                                .transformBy(target.getAlternateCameraToTarget().inverse())
+                                .transformBy(p.getSecond().inverse());
+                Pose3d botAltPose =
+                        targetPose
+                                .transformBy(target.getBestCameraToTarget().inverse())
+                                .transformBy(p.getSecond().inverse());
+                double alternativeDifference = Math.abs(calculateDifference(referencePose, botAltPose));
+                double bestDifference = Math.abs(calculateDifference(referencePose, botBestPose));
                 if (alternativeDifference < smallestDifference) {
                     smallestDifference = alternativeDifference;
-                    pose = targetPose.transformBy(target.getAlternateCameraToTarget().inverse());
-                    mili = p.getFirst().getLatestResult().getLatencyMillis();
+                    pose = botAltPose;
+                    latency = result.getLatencyMillis();
                 }
                 if (bestDifference < smallestDifference) {
                     smallestDifference = bestDifference;
-                    pose = targetPose.transformBy(target.getBestCameraToTarget().inverse());
-                    mili = p.getFirst().getLatestResult().getLatencyMillis();
+                    pose = botBestPose;
+                    latency = result.getLatencyMillis();
                 }
             }
         }
-        return Pair.of(pose, mili);
+        return Pair.of(pose, latency);
+    }
+
+    private Pair<Pose3d, Double> closestToLastPoseStrategy() {
+        setReferencePose(lastPose);
+        return closestToReferencePoseStrategy();
     }
 
     /** Return the average of the best target poses using ambiguity as weight */
@@ -277,34 +284,32 @@ public class RobotPoseEstimator {
         double totalAmbiguity = 0;
         for (int i = 0; i < cameras.size(); i++) {
             Pair<PhotonCamera, Transform3d> p = cameras.get(i);
-            List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
+            var result = p.getFirst().getLatestResult();
+            List<PhotonTrackedTarget> targets = result.targets;
             for (int j = 0; j < targets.size(); j++) {
                 PhotonTrackedTarget target = targets.get(j);
-                // If the map doesn't contain the ID fail
-                if (!aprilTags.containsKey(target.getFiducialId())) {
-                    if (!reportedErrors.contains(target.getFiducialId())) {
-                        DriverStation.reportWarning(
-                                "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
-                                        + target.getFiducialId(),
-                                false);
-                        reportedErrors.add(target.getFiducialId());
-                    }
+                Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
+                if (fiducialPose.isEmpty()) {
+                    reportFiducialPoseError(target.getFiducialId());
                     continue;
                 }
-                Pose3d targetPose = aprilTags.get(target.getFiducialId());
+                Pose3d targetPose = fiducialPose.get();
                 try {
                     totalAmbiguity += 1. / target.getPoseAmbiguity();
                 } catch (ArithmeticException e) {
                     // A total ambiguity of zero exists, using that pose instead!",
                     return Pair.of(
-                            targetPose.transformBy(target.getBestCameraToTarget().inverse()),
-                            p.getFirst().getLatestResult().getLatencyMillis());
+                            targetPose
+                                    .transformBy(target.getBestCameraToTarget().inverse())
+                                    .transformBy(p.getSecond().inverse()),
+                            result.getLatencyMillis());
                 }
                 tempPoses.add(
                         Pair.of(
-                                targetPose.transformBy(target.getBestCameraToTarget().inverse()),
-                                Pair.of(
-                                        target.getPoseAmbiguity(), p.getFirst().getLatestResult().getLatencyMillis())));
+                                targetPose
+                                        .transformBy(target.getBestCameraToTarget().inverse())
+                                        .transformBy(p.getSecond().inverse()),
+                                Pair.of(target.getPoseAmbiguity(), result.getLatencyMillis())));
             }
         }
 
@@ -312,19 +317,23 @@ public class RobotPoseEstimator {
         Rotation3d rotation = new Rotation3d();
         double latency = 0;
 
-        for (Pair<Pose3d, Pair<Double, Double>> pair : tempPoses) {
-            try {
-                double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity;
-                transform = transform.plus(pair.getFirst().getTranslation().times(weight));
-                rotation = rotation.plus(pair.getFirst().getRotation().times(weight));
-                latency += pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well
-            } catch (ArithmeticException e) {
-                DriverStation.reportWarning(
-                        "[RobotPoseEstimator] A total ambiguity of zero exists, using that pose instead!",
-                        false);
-                return Pair.of(pair.getFirst(), pair.getSecond().getSecond());
-            }
+        if (tempPoses.isEmpty()) {
+            return null;
         }
+
+        if (totalAmbiguity == 0) {
+            Pose3d p = tempPoses.get(0).getFirst();
+            double l = tempPoses.get(0).getSecond().getSecond();
+            return Pair.of(p, l);
+        }
+
+        for (Pair<Pose3d, Pair<Double, Double>> pair : tempPoses) {
+            double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity;
+            transform = transform.plus(pair.getFirst().getTranslation().times(weight));
+            rotation = rotation.plus(pair.getFirst().getRotation().times(weight));
+            latency += pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well
+        }
+
         return Pair.of(new Pose3d(transform, rotation), latency);
     }
 
@@ -338,12 +347,12 @@ public class RobotPoseEstimator {
     }
 
     /** @param aprilTags the aprilTags to set */
-    public void setAprilTags(Map<Integer, Pose3d> aprilTags) {
+    public void setAprilTags(AprilTagFieldLayout aprilTags) {
         this.aprilTags = aprilTags;
     }
 
     /** @return the aprilTags */
-    public Map<Integer, Pose3d> getAprilTags() {
+    public AprilTagFieldLayout getAprilTags() {
         return aprilTags;
     }
 
@@ -371,6 +380,15 @@ public class RobotPoseEstimator {
         this.referencePose = referencePose;
     }
 
+    /**
+     * Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE
+     *
+     * @param referencePose the referencePose to set
+     */
+    public void setReferencePose(Pose2d referencePose) {
+        setReferencePose(new Pose3d(referencePose));
+    }
+
     /**
      * UPdate the stored last pose. Useful for setting the initial estimate with CLOSEST_TO_LAST_POSE
      *
@@ -379,4 +397,12 @@ public class RobotPoseEstimator {
     public void setLastPose(Pose3d lastPose) {
         this.lastPose = lastPose;
     }
+
+    private void reportFiducialPoseError(int fiducialId) {
+        if (!reportedErrors.contains(fiducialId)) {
+            DriverStation.reportError(
+                    "[RobotPoseEstimator] Tried to get pose of unknown April Tag: " + fiducialId, false);
+            reportedErrors.add(fiducialId);
+        }
+    }
 }

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

@@ -27,7 +27,6 @@ package org.photonvision;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -139,13 +138,7 @@ public class SimVisionSystem {
      *     PhotonVision parameters.
      */
     public void processFrame(Pose2d robotPoseMeters) {
-        var robotPose3d =
-                new Pose3d(
-                        robotPoseMeters.getX(),
-                        robotPoseMeters.getY(),
-                        0.0,
-                        new Rotation3d(0, 0, robotPoseMeters.getRotation().getRadians()));
-        processFrame(robotPose3d);
+        processFrame(new Pose3d(robotPoseMeters));
     }
 
     /**

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

@@ -41,9 +41,13 @@ PhotonCamera::PhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
       rawBytesEntry(rootTable->GetRawTopic("rawBytes").Subscribe("raw", {})),
       driverModeEntry(rootTable->GetBooleanTopic("driverMode").Publish()),
       inputSaveImgEntry(
-          rootTable->GetBooleanTopic("inputSaveImgCmd").Publish()),
+          rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()),
+      inputSaveImgSubscriber(
+          rootTable->GetIntegerTopic("inputSaveImgCmd").Subscribe(0)),
       outputSaveImgEntry(
-          rootTable->GetBooleanTopic("outputSaveImgCmd").Publish()),
+          rootTable->GetIntegerTopic("outputSaveImgCmd").Publish()),
+      outputSaveImgSubscriber(
+          rootTable->GetIntegerTopic("outputSaveImgCmd").Subscribe(0)),
       pipelineIndexEntry(rootTable->GetIntegerTopic("pipelineIndex").Publish()),
       ledModeEntry(mainTable->GetIntegerTopic("ledMode").Publish()),
       versionEntry(mainTable->GetStringTopic("version").Subscribe("")),
@@ -89,9 +93,13 @@ void PhotonCamera::SetDriverMode(bool driverMode) {
   driverModeEntry.Set(driverMode);
 }
 
-void PhotonCamera::TakeInputSnapshot() { inputSaveImgEntry.Set(true); }
+void PhotonCamera::TakeInputSnapshot() {
+  inputSaveImgEntry.Set(inputSaveImgSubscriber.Get() + 1);
+}
 
-void PhotonCamera::TakeOutputSnapshot() { outputSaveImgEntry.Set(true); }
+void PhotonCamera::TakeOutputSnapshot() {
+  outputSaveImgEntry.Set(outputSaveImgSubscriber.Get() + 1);
+}
 
 bool PhotonCamera::GetDriverMode() const { return driverModeSubscriber.Get(); }
 

photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp

@@ -33,6 +33,7 @@
 #include <vector>
 
 #include <frc/Errors.h>
+#include <frc/apriltag/AprilTagFieldLayout.h>
 #include <frc/geometry/Pose3d.h>
 #include <frc/geometry/Rotation3d.h>
 #include <frc/geometry/Transform3d.h>
@@ -44,7 +45,7 @@
 
 namespace photonlib {
 RobotPoseEstimator::RobotPoseEstimator(
-    std::map<int, frc::Pose3d> tags, PoseStrategy strat,
+    std::shared_ptr<frc::AprilTagFieldLayout> tags, PoseStrategy strat,
     std::vector<std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>>
         cams)
     : aprilTags(tags),
@@ -112,7 +113,9 @@ RobotPoseEstimator::LowestAmbiguityStrategy() {
   PhotonTrackedTarget bestTarget =
       cameras[lowestAI].first->GetLatestResult().GetTargets()[lowestAJ];
 
-  if (aprilTags.count(bestTarget.GetFiducialId()) == 0) {
+  std::optional<frc::Pose3d> fiducialPose =
+      aprilTags->GetTagPose(bestTarget.GetFiducialId());
+  if (!fiducialPose) {
     FRC_ReportError(frc::warn::Warning,
                     "Tried to get pose of unknown April Tag: {}",
                     bestTarget.GetFiducialId());
@@ -120,7 +123,7 @@ RobotPoseEstimator::LowestAmbiguityStrategy() {
   }
 
   return std::make_pair(
-      aprilTags[bestTarget.GetFiducialId()]
+      fiducialPose.value()
           .TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
           .TransformBy(cameras[lowestAI].second.Inverse()),
       cameras[lowestAI].first->GetLatestResult().GetLatency() / 1000.);
@@ -138,13 +141,15 @@ RobotPoseEstimator::ClosestToCameraHeightStrategy() {
         p.first->GetLatestResult().GetTargets();
     for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
       PhotonTrackedTarget target = targets[j];
-      if (aprilTags.count(target.GetFiducialId()) == 0) {
+      std::optional<frc::Pose3d> fiducialPose =
+          aprilTags->GetTagPose(target.GetFiducialId());
+      if (!fiducialPose) {
         FRC_ReportError(frc::warn::Warning,
                         "Tried to get pose of unknown April Tag: {}",
                         target.GetFiducialId());
         continue;
       }
-      frc::Pose3d targetPose = aprilTags[target.GetFiducialId()];
+      frc::Pose3d targetPose = fiducialPose.value();
       units::meter_t alternativeDifference = units::math::abs(
           p.second.Z() -
           targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
@@ -180,13 +185,15 @@ RobotPoseEstimator::ClosestToReferencePoseStrategy() {
         p.first->GetLatestResult().GetTargets();
     for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
       PhotonTrackedTarget target = targets[j];
-      if (aprilTags.count(target.GetFiducialId()) == 0) {
+      std::optional<frc::Pose3d> fiducialPose =
+          aprilTags->GetTagPose(target.GetFiducialId());
+      if (!fiducialPose) {
         FRC_ReportError(frc::warn::Warning,
                         "Tried to get pose of unknown April Tag: {}",
                         target.GetFiducialId());
         continue;
       }
-      frc::Pose3d targetPose = aprilTags[target.GetFiducialId()];
+      frc::Pose3d targetPose = fiducialPose.value();
       units::meter_t alternativeDifference =
           units::math::abs(referencePose.Translation().Distance(
               targetPose
@@ -225,14 +232,16 @@ RobotPoseEstimator::AverageBestTargetsStrategy() {
         p.first->GetLatestResult().GetTargets();
     for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
       PhotonTrackedTarget target = targets[j];
-      if (aprilTags.count(target.GetFiducialId()) == 0) {
+      std::optional<frc::Pose3d> fiducialPose =
+          aprilTags->GetTagPose(target.GetFiducialId());
+      if (!fiducialPose) {
         FRC_ReportError(frc::warn::Warning,
                         "Tried to get pose of unknown April Tag: {}",
                         target.GetFiducialId());
         continue;
       }
 
-      frc::Pose3d targetPose = aprilTags[target.GetFiducialId()];
+      frc::Pose3d targetPose = fiducialPose.value();
       if (target.GetPoseAmbiguity() == 0) {
         FRC_ReportError(frc::warn::Warning,
                         "Pose ambiguity of zero exists, using that instead!",

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

@@ -166,8 +166,10 @@ class PhotonCamera {
   std::shared_ptr<nt::NetworkTable> rootTable;
   nt::RawSubscriber rawBytesEntry;
   nt::BooleanPublisher driverModeEntry;
-  nt::BooleanPublisher inputSaveImgEntry;
-  nt::BooleanPublisher outputSaveImgEntry;
+  nt::IntegerPublisher inputSaveImgEntry;
+  nt::IntegerSubscriber inputSaveImgSubscriber;
+  nt::IntegerPublisher outputSaveImgEntry;
+  nt::IntegerSubscriber outputSaveImgSubscriber;
   nt::IntegerPublisher pipelineIndexEntry;
   nt::IntegerPublisher ledModeEntry;
   nt::StringSubscriber versionEntry;

photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h

@@ -34,6 +34,10 @@
 
 #include "photonlib/PhotonCamera.h"
 
+namespace frc {
+class AprilTagFieldLayout;
+}  // namespace frc
+
 namespace photonlib {
 enum PoseStrategy : int {
   LOWEST_AMBIGUITY,
@@ -47,18 +51,18 @@ enum PoseStrategy : int {
  * A managing class to determine how an estimated pose should be chosen.
  */
 class RobotPoseEstimator {
+ public:
   using map_value_type =
       std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
   using size_type = std::vector<map_value_type>::size_type;
 
- public:
-  explicit RobotPoseEstimator(std::map<int, frc::Pose3d> aprilTags,
-                              PoseStrategy strategy,
-                              std::vector<map_value_type>);
+  explicit RobotPoseEstimator(
+      std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
+      PoseStrategy strategy, std::vector<map_value_type> cameras);
 
   std::pair<frc::Pose3d, units::millisecond_t> Update();
 
-  void SetPoseStrategy(PoseStrategy strategy);
+  inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
 
   inline void SetReferencePose(frc::Pose3d referencePose) {
     this->referencePose = referencePose;
@@ -79,7 +83,7 @@ class RobotPoseEstimator {
   frc::Pose3d GetReferencePose() const { return referencePose; }
 
  private:
-  std::map<int, frc::Pose3d> aprilTags;
+  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags;
   PoseStrategy strategy;
   std::vector<map_value_type> cameras;
   frc::Pose3d lastPose;

photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java

@@ -26,21 +26,23 @@ package org.photonvision;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.hal.JNIWrapper;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.net.WPINetJNI;
 import edu.wpi.first.networktables.NetworkTablesJNI;
 import edu.wpi.first.util.CombinedRuntimeLoader;
 import edu.wpi.first.util.WPIUtilJNI;
 import java.io.IOException;
 import java.util.ArrayList;
-import java.util.HashMap;
 import java.util.List;
-import java.util.Map;
+import java.util.Optional;
 import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
 import org.photonvision.RobotPoseEstimator.PoseStrategy;
@@ -49,6 +51,8 @@ import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;
 
 class RobotPoseEstimatorTest {
+    static AprilTagFieldLayout aprilTags;
+
     @BeforeAll
     public static void init() {
         JNIWrapper.Helper.setExtractOnStaticLoad(false);
@@ -63,14 +67,17 @@ class RobotPoseEstimatorTest {
             // TODO Auto-generated catch block
             e.printStackTrace();
         }
+
+        List<AprilTag> atList = new ArrayList<AprilTag>(2);
+        atList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
+        atList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
+        var fl = Units.feetToMeters(54.0);
+        var fw = Units.feetToMeters(27.0);
+        aprilTags = new AprilTagFieldLayout(atList, fl, fw);
     }
 
     @Test
     void testLowestAmbiguityStrategy() {
-        Map<Integer, Pose3d> aprilTags = new HashMap<>();
-        aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
-        aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
-
         ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
 
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@@ -132,10 +139,10 @@ class RobotPoseEstimatorTest {
         RobotPoseEstimator estimator =
                 new RobotPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameras);
 
-        Pair<Pose3d, Double> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.getFirst();
+        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().getFirst();
 
-        assertEquals(2, estimatedPose.getSecond());
+        assertEquals(2, estimatedPose.get().getSecond());
         assertEquals(1, pose.getX(), .01);
         assertEquals(3, pose.getY(), .01);
         assertEquals(2, pose.getZ(), .01);
@@ -143,10 +150,6 @@ class RobotPoseEstimatorTest {
 
     @Test
     void testClosestToCameraHeightStrategy() {
-        Map<Integer, Pose3d> aprilTags = new HashMap<>();
-        aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
-        aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
-
         ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
 
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@@ -208,21 +211,17 @@ class RobotPoseEstimatorTest {
         RobotPoseEstimator estimator =
                 new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, cameras);
 
-        Pair<Pose3d, Double> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.getFirst();
+        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().getFirst();
 
-        assertEquals(2, estimatedPose.getSecond());
+        assertEquals(2, estimatedPose.get().getSecond());
         assertEquals(4, pose.getX(), .01);
         assertEquals(4, pose.getY(), .01);
-        assertEquals(4, pose.getZ(), .01);
+        assertEquals(0, pose.getZ(), .01);
     }
 
     @Test
     void closestToReferencePoseStrategy() {
-        Map<Integer, Pose3d> aprilTags = new HashMap<>();
-        aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
-        aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
-
         ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
 
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@@ -285,10 +284,10 @@ class RobotPoseEstimatorTest {
                 new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, cameras);
         estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
 
-        Pair<Pose3d, Double> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.getFirst();
+        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().getFirst();
 
-        assertEquals(4, estimatedPose.getSecond());
+        assertEquals(4, estimatedPose.get().getSecond());
         assertEquals(1, pose.getX(), .01);
         assertEquals(1.1, pose.getY(), .01);
         assertEquals(.9, pose.getZ(), .01);
@@ -296,10 +295,6 @@ class RobotPoseEstimatorTest {
 
     @Test
     void closestToLastPose() {
-        Map<Integer, Pose3d> aprilTags = new HashMap<>();
-        aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
-        aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
-
         ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
 
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@@ -363,8 +358,8 @@ class RobotPoseEstimatorTest {
 
         estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
 
-        Pair<Pose3d, Double> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.getFirst();
+        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().getFirst();
 
         cameraOne.result =
                 new PhotonPipelineResult(
@@ -418,9 +413,9 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(7, 8)))));
 
         estimatedPose = estimator.update();
-        pose = estimatedPose.getFirst();
+        pose = estimatedPose.get().getFirst();
 
-        assertEquals(2, estimatedPose.getSecond());
+        assertEquals(2, estimatedPose.get().getSecond());
         assertEquals(.9, pose.getX(), .01);
         assertEquals(1.1, pose.getY(), .01);
         assertEquals(1, pose.getZ(), .01);
@@ -428,10 +423,6 @@ class RobotPoseEstimatorTest {
 
     @Test
     void averageBestPoses() {
-        Map<Integer, Pose3d> aprilTags = new HashMap<>();
-        aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
-        aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
-
         ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
 
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@@ -493,9 +484,9 @@ class RobotPoseEstimatorTest {
         RobotPoseEstimator estimator =
                 new RobotPoseEstimator(aprilTags, PoseStrategy.AVERAGE_BEST_TARGETS, cameras);
 
-        Pair<Pose3d, Double> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.getFirst();
-        assertEquals(2.6885245901639347, estimatedPose.getSecond(), .01);
+        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().getFirst();
+        assertEquals(2.6885245901639347, estimatedPose.get().getSecond(), .01);
         assertEquals(2.15, pose.getX(), .01);
         assertEquals(2.15, pose.getY(), .01);
         assertEquals(2.15, pose.getZ(), .01);

photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp

@@ -26,6 +26,7 @@
 #include <utility>
 #include <vector>
 
+#include <frc/apriltag/AprilTagFieldLayout.h>
 #include <frc/geometry/Pose3d.h>
 #include <frc/geometry/Rotation3d.h>
 #include <frc/geometry/Transform3d.h>
@@ -40,11 +41,14 @@
 #include "photonlib/RobotPoseEstimator.h"
 
 TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
-  std::map<int, frc::Pose3d> aprilTags;
-  aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
-                                   units::meter_t(3), frc::Rotation3d())});
-  aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
-                                   units::meter_t(5), frc::Rotation3d())});
+  std::vector<frc::AprilTag> tags = {
+      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
+                      frc::Rotation3d())},
+      {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
+                      frc::Rotation3d())}};
+
+  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
+      std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
 
   std::vector<
       std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -122,11 +126,14 @@ TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
 }
 
 TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
-  std::map<int, frc::Pose3d> aprilTags;
-  aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
-                                   units::meter_t(3), frc::Rotation3d())});
-  aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
-                                   units::meter_t(5), frc::Rotation3d())});
+  std::vector<frc::AprilTag> tags = {
+      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
+                      frc::Rotation3d())},
+      {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
+                      frc::Rotation3d())},
+  };
+  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
+      std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
 
   std::vector<
       std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -204,11 +211,14 @@ TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
 }
 
 TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
-  std::map<int, frc::Pose3d> aprilTags;
-  aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
-                                   units::meter_t(3), frc::Rotation3d())});
-  aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
-                                   units::meter_t(5), frc::Rotation3d())});
+  std::vector<frc::AprilTag> tags = {
+      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
+                      frc::Rotation3d())},
+      {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
+                      frc::Rotation3d())},
+  };
+  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
+      std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
 
   std::vector<
       std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -288,11 +298,13 @@ TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
 }
 
 TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
-  std::map<int, frc::Pose3d> aprilTags;
-  aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
-                                   units::meter_t(3), frc::Rotation3d())});
-  aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
-                                   units::meter_t(5), frc::Rotation3d())});
+  std::vector<frc::AprilTag> tags = {
+      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
+                      frc::Rotation3d())},
+      {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
+                      frc::Rotation3d())}};
+  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
+      std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
 
   std::vector<
       std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -431,11 +443,13 @@ TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
 }
 
 TEST(RobotPoseEstimatorTest, AverageBestPoses) {
-  std::map<int, frc::Pose3d> aprilTags;
-  aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
-                                   units::meter_t(3), frc::Rotation3d())});
-  aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
-                                   units::meter_t(5), frc::Rotation3d())});
+  std::vector<frc::AprilTag> tags = {
+      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
+                      frc::Rotation3d())},
+      {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
+                      frc::Rotation3d())}};
+  std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
+      std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
 
   std::vector<
       std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>

photon-server/src/main/java/org/photonvision/server/RequestHandler.java

@@ -25,6 +25,7 @@ import java.io.FileInputStream;
 import java.io.FileNotFoundException;
 import java.io.FileOutputStream;
 import java.io.IOException;
+import java.nio.file.Files;
 import java.nio.file.Path;
 import java.nio.file.Paths;
 import java.util.HashMap;
@@ -193,6 +194,43 @@ public class RequestHandler {
         }
     }
 
+    private static ShellExec shell = new ShellExec();
+
+    public static void onExportCurrentLogs(Context ctx) {
+        if (!Platform.isLinux()) {
+            logger.warn("Cannot export journalctl on non-Linux platforms! Ignoring");
+            ctx.status(500);
+            return;
+        }
+
+        try {
+            var tempPath = Files.createTempFile("photonvision-journalctl", ".txt");
+            shell.executeBashCommand(
+                    "journalctl -u photonvision.service > " + tempPath.toAbsolutePath().toString());
+
+            while (!shell.isOutputCompleted()) {
+                // TODO: add timeout
+            }
+
+            if (shell.getExitCode() == 0) {
+                // Wrote to the temp file! Add it to the ctx
+                var stream = new FileInputStream(tempPath.toFile());
+                logger.info("Uploading settings with size " + stream.available());
+                ctx.result(stream);
+                ctx.contentType("application/zip");
+                ctx.header("Content-Disposition: attachment; filename=\"photonvision-journalctl.txt\"");
+                ctx.status(200);
+            } else {
+                logger.error("Could not export journactl logs! (exit code != 0)");
+                ctx.status(500);
+            }
+        } catch (IOException e) {
+            // TODO Auto-generated catch block
+            logger.error("Could not export journactl logs! (IOexception)", e);
+            ctx.status(500);
+        }
+    }
+
     public static void onCalibrationEnd(Context ctx) {
         logger.info("Calibrating camera! This will take a long time...");
         var index = Integer.parseInt(ctx.body());

photon-server/src/main/java/org/photonvision/server/Server.java

@@ -84,6 +84,7 @@ public class Server {
         app.post("/api/settings/import", RequestHandler::onSettingUpload);
         app.post("/api/settings/offlineUpdate", RequestHandler::onOfflineUpdate);
         app.get("/api/settings/photonvision_config.zip", RequestHandler::onSettingsDownload);
+        app.get("/api/settings/photonvision-journalctl.txt", RequestHandler::onExportCurrentLogs);
         app.post("/api/settings/camera", RequestHandler::onCameraSettingsSave);
         app.post("/api/settings/general", RequestHandler::onGeneralSettings);
         app.post("/api/settings/endCalibration", RequestHandler::onCalibrationEnd);

scripts/generatePiImage.sh

@@ -1,23 +1,47 @@
-# We need to look for a JAR with the "-raspi" suffix so we don't accidentally bundle the big jar
-# Not that it really matters, but it'll save us 50 megs or so
-NEW_JAR=$(realpath $(find . -name photonvision\*-linuxarm32.jar))
+if [ "$#" -ne 2 ]; then
+    echo "Illegal number of parameters -- expected (Image release URL) (image suffix)"
+    exit 1
+fi
+
+# 1st arg should be the release to download the image template from. The release ought to only have one
+# artifact for a "xz" image.
+
+NEW_JAR=$(realpath $(find . -name photonvision\*-linuxarm64.jar))
 echo "Using jar: " $NEW_JAR
+echo "Downloading image from" $1
 sudo apt-get install -y xz-utils
-curl -sk https://api.github.com/repos/photonvision/photon-pi-gen/releases/tags/v2023.1.0-beta-1 | grep "browser_download_url.*xz" | cut -d : -f 2,3 | tr -d '"' | wget -qi -
+curl -sk $1 | grep "browser_download_url.*xz" | cut -d : -f 2,3 | tr -d '"' | wget -qi -
 ls
 FILE_NAME=$(ls | grep image_*.xz)
-echo "Downloaded " $FILE_NAME
+
+if [ -z "$FILE_NAME" ]
+then
+    echo "Could not locate image archive!"
+    exit 1
+fi
+
+echo "Downloaded " $FILE_NAME " -- decompressing now..."
 xz -T0 -v --decompress $FILE_NAME
 IMAGE_FILE=$(ls | grep *.img)
 ls
-echo "Unziped image: " $IMAGE_FILE
+
+if [ -z "$FILE_NAME" ]
+then
+    echo "Could not locate unzipped image!"
+    exit 1
+fi
+
+echo "Unziped image: " $IMAGE_FILE " -- mounting"
 TMP=$(mktemp -d)
 LOOP=$(sudo losetup --show -fP "${IMAGE_FILE}")
+echo "Image mounted! Copying jar..."
 sudo mount ${LOOP}p2 $TMP
 pushd .
 cd $TMP/opt/photonvision
 sudo cp $NEW_JAR photonvision.jar
 
+echo "Jar updated! Creating service..."
+
 cd $TMP/etc/systemd/system/multi-user.target.wants
 sudo bash -c "printf \
 \"[Unit]
@@ -35,9 +59,13 @@ RestartSec=1
 WantedBy=multi-user.target\" > photonvision.service"
 
 popd
+
+echo "Service created!"
+
 sudo umount ${TMP}
 sudo rmdir ${TMP}
-NEW_IMAGE=$(basename "${NEW_JAR/jar/img}")
+NEW_IMAGE=$(basename "${NEW_JAR/.jar/_$2.img}")
+echo "Renaming image " $IMAGE_FILE " -> " $NEW_IMAGE
 mv $IMAGE_FILE $NEW_IMAGE
 xz -T0 -v -z $NEW_IMAGE
-mv $NEW_IMAGE.xz $(basename "${NEW_JAR/.jar/-image.xz}")
+mv $NEW_IMAGE.xz $(basename "${NEW_JAR/.jar/-image_$2.xz}")

scripts/install.sh

@@ -5,7 +5,30 @@ if [ "$(id -u)" != "0" ]; then
    exit 1
 fi
 
+ARCH=$(uname -m)
+ARCH_NAME=""
+if [ "$ARCH" = "aarch64" ]; then
+  ARCH_NAME="linuxarm64"
+elif [ "$ARCH" = "armv7l" ]; then
+  ARCH_NAME="linuxarm32"
+elif [ "$ARCH" = "x86_64" ]; then
+  ARCH_NAME="linuxx64"
+else
+  if [ "$#" -ne 1 ]; then
+      echo "Can't determine current arch; please provide it (one of):"
+      echo ""
+      echo "- linuxarm32 (32-bit Linux ARM)"
+      echo "- linuxarm64 (64-bit Linux ARM)"
+      echo "- linuxx64   (64-bit Linux)"
+      exit 1
+  else
+    echo "Can't detect arch (got $ARCH) -- using user-provided $1"
+    ARCH_NAME=$1
+  fi
+fi
+
 echo "This is the installation script for PhotonVision."
+echo "Installing for platform $ARCH_NAME"
 
 echo "Installing curl..."
 apt-get install --yes curl
@@ -39,7 +62,7 @@ echo "Downloading latest stable release of PhotonVision..."
 mkdir -p /opt/photonvision
 cd /opt/photonvision
 curl -sk https://api.github.com/repos/photonvision/photonvision/releases/latest |
-    grep "browser_download_url.*jar" |
+    grep "browser_download_url.*$ARCH_NAME.jar" |
     cut -d : -f 2,3 |
     tr -d '"' |
     wget -qi - -O photonvision.jar

shared/common.gradle

@@ -37,7 +37,7 @@ dependencies {
     implementation wpilibTools.deps.wpilibJava("wpilibj")
 
     implementation "edu.wpi.first.thirdparty.frc2023.opencv:opencv-java:$opencvVersion"
-    jniPlatforms.each { implementation "edu.wpi.first.thirdparty.frc2023.opencv:opencv-jni:$opencvVersion:$it" }
+    implementation "edu.wpi.first.thirdparty.frc2023.opencv:opencv-jni:$opencvVersion:$jniPlatform"
 
     implementation "org.ejml:ejml-simple:0.41"