Update cv-select.vue (#719)

Co-authored-by: Mohammad Durrani <46766905+mdurrani808@users.noreply.github.com>

.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'
@@ -26,24 +26,20 @@ allprojects {
 apply from: "versioningHelper.gradle"
 
 ext {
-    wpilibVersion = "2023.1.1-beta-7-15-g1e7fcd5"
+    wpilibVersion = "2023.1.1"
     opencvVersion = "4.6.0-4"
     joglVersion = "2.4.0-rc-20200307"
     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/App.vue

@@ -134,7 +134,7 @@
 
 <script>
 import Logs from "./views/LogsView"
-// import {mapState} from "vuex";
+import { ReconnectingWebsocket } from "./plugins/ReconnectingWebsocket.js"
 
 export default {
   name: 'App',
@@ -145,7 +145,8 @@ export default {
     // Used so that we can switch back to the previously selected pipeline after camera calibration
     previouslySelectedIndices: [],
     timer: undefined,
-    teamNumberDialog: true
+    teamNumberDialog: true,
+    websocket: null,
   }),
   computed: {
     needsTeamNumberSet: {
@@ -190,15 +191,12 @@ export default {
       }
     });
 
-    this.recreateWebsocket();
-  },
-  methods: {
-    recreateWebsocket() {
-      const wsDataURL = 'ws://' + this.$address + '/websocket_data';
-      let socket = new WebSocket(wsDataURL);
-      socket.binaryType = "arraybuffer";
+    const wsDataURL = 'ws://' + this.$address + '/websocket_data';
+    this.websocket = new ReconnectingWebsocket(
+      wsDataURL,
 
-      socket.onmessage = (event) => {
+      // On data in
+      (event) => {
         try {
           let message = this.$msgPack.decode(event.data);
           for (let prop in message) {
@@ -210,31 +208,21 @@ export default {
           console.log(event)
           console.error('error: ' + JSON.stringify(event.data) + " , " + error);
         }
-      };
+      },
 
-      socket.onerror = () => {
-        socket.close();
-        this.$store.commit("backendConnected", false)
-      };
+      // on connect
+      (event) => {
+        event; this.$store.commit("backendConnected", true);
+        this.$store.state.connectedCallbacks.forEach(it => it());
+       },
 
+      // on disconnect
+      (event) => { event; this.$store.commit("backendConnected", false) }
+    );
 
-      socket.onopen = () => {
-        clearInterval(this.timerId);
-
-        socket.onclose = () => {
-          this.$store.commit("backendConnected", false)
-          this.timerId = setInterval(() => {
-            this.recreateWebsocket();
-          }, 1000);
-        };
-
-
-        this.$store.commit("backendConnected", true)
-        this.$store.state.connectedCallbacks.forEach(it => it())
-      };
-
-      this.$store.commit("websocket", socket);
-    },
+    this.$store.commit("websocket", this.websocket);
+  },
+  methods: {
     handleMessage(key, value) {
       if (key === "logMessage") {
         this.logMessage(value["logMessage"], value["logLevel"]);

photon-client/src/components/common/cv-image.vue

@@ -4,7 +4,7 @@
     crossOrigin="anonymous"
     :style="styleObject"
     :src="src"
-    alt=""
+    :alt="alt"
     @click="clickHandler"
     @error="loadErrHandler"
   />
@@ -14,7 +14,7 @@
     export default {
         name: "CvImage",
         // eslint-disable-next-line vue/require-prop-types
-        props: ['address', 'scale', 'maxHeight', 'maxHeightMd', 'maxHeightLg', 'maxHeightXl', 'colorPicking', 'id', 'disconnected'],
+        props: ['address', 'scale', 'maxHeight', 'maxHeightMd', 'maxHeightLg', 'maxHeightXl', 'colorPicking', 'id', 'disconnected', 'alt'],
         data() {
             return {
                 seed: 1.0,

photon-client/src/components/common/cv-select.vue

@@ -37,7 +37,7 @@ import TooltippedLabel from "./cv-tooltipped-label";
             TooltippedLabel,
         },
       // eslint-disable-next-line vue/require-prop-types
-        props: ['list', 'name', 'value', 'disabled', 'selectCols', 'rules', 'tooltip'],
+        props: ['list', 'name', 'value', 'disabled', 'filteredIndices', 'selectCols', 'rules', 'tooltip'],
         computed: {
             localValue: {
                 get() {
@@ -50,6 +50,7 @@ import TooltippedLabel from "./cv-tooltipped-label";
             indexList() {
                 let list = [];
                 for (let i = 0; i < this.list.length; i++) {
+                    if (this.filteredIndices instanceof Set && this.filteredIndices.has(i)) continue;
                     list.push({
                         name: this.list[i],
                         index: i

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/mixins/global/dataHandleMixin.js

@@ -2,14 +2,14 @@ export const dataHandleMixin = {
     methods: {
         handleInput(key, value) {
             let msg = this.$msgPack.encode({[key]: value});
-            this.$store.state.websocket.send(msg);
+            this.$store.state.websocket.ws.send(msg);
         },
         handleInputWithIndex(key, value, cameraIndex = this.$store.getters.currentCameraIndex) {
             let msg = this.$msgPack.encode({
                 [key]: value,
                 ["cameraIndex"]: cameraIndex,
             });
-            this.$store.state.websocket.send(msg);
+            this.$store.state.websocket.ws.send(msg);
         },
         handleData(val) {
             this.handleInput(val, this[val]);
@@ -22,7 +22,7 @@ export const dataHandleMixin = {
                     ["cameraIndex"]: this.$store.getters.currentCameraIndex
                 }
             });
-            this.$store.state.websocket.send(msg);
+            this.$store.state.websocket.ws.send(msg);
             this.$emit('update')
         },
         handlePipelineUpdate(key, val) {
@@ -32,7 +32,7 @@ export const dataHandleMixin = {
                     ["cameraIndex"]: this.$store.getters.currentCameraIndex
                 }
             });
-            this.$store.state.websocket.send(msg);
+            this.$store.state.websocket.ws.send(msg);
             this.$emit('update')
         },
         handleTruthyPipelineData(val) {
@@ -42,7 +42,7 @@ export const dataHandleMixin = {
                     ["cameraIndex"]: this.$store.getters.currentCameraIndex
                 }
             });
-            this.$store.state.websocket.send(msg);
+            this.$store.state.websocket.ws.send(msg);
             this.$emit('update')
         },
         rollback(val, e) {

photon-client/src/plugins/ReconnectingWebsocket.js

@@ -0,0 +1,74 @@
+/**
+ * Auto-reconnecting Websocket, a stripped down version of the NT4 client from
+ * https://raw.githubusercontent.com/wpilibsuite/NetworkTablesClients/2f8d378ac08d5ca703d590cfb019fc4af062db89/nt4/js/src/nt4.js
+ */
+export class ReconnectingWebsocket {
+    constructor(serverAddr,
+        onDataIn_in,
+        onConnect_in,
+        onDisconnect_in) {
+
+        this.onDataIn = onDataIn_in;
+        this.onConnect = onConnect_in;
+        this.onDisconnect = onDisconnect_in;
+
+        // WS Connection State (with defaults)
+        this.serverAddr = serverAddr;
+        this.serverConnectionActive = false;
+
+        //Trigger the websocket to connect automatically
+        this.ws_connect();
+    }
+
+    //////////////////////////////////////////////////////////////
+    // Websocket connection Maintenance
+
+    ws_onOpen() {
+        // Set the flag allowing general server communication
+        this.serverConnectionActive = true;
+
+        console.log("[WebSocket] Connected!");
+
+        // User connection-opened hook
+        this.onConnect();
+    }
+
+    ws_onClose(e) {
+        //Clear flags to stop server communication
+        this.ws = null;
+        this.serverConnectionActive = false;
+
+        // User connection-closed hook
+        this.onDisconnect();
+
+        console.log('[WebSocket] Socket is closed. Reconnect will be attempted in 0.5 second.', e.reason);
+        setTimeout(this.ws_connect.bind(this), 500);
+
+        if (!e.wasClean) {
+            console.error('Socket encountered error!');
+        }
+
+    }
+
+    ws_onError(e) {
+        console.log("[WebSocket] Websocket error - " + e.toString());
+        this.ws.close();
+    }
+
+    ws_onMessage(e) {
+       this.onDataIn(e);
+    }
+
+    ws_connect() {
+        this.ws = new WebSocket(this.serverAddr);
+        this.ws.binaryType = "arraybuffer";
+        this.ws.onopen = this.ws_onOpen.bind(this);
+        this.ws.onmessage = this.ws_onMessage.bind(this);
+        this.ws.onclose = this.ws_onClose.bind(this);
+        this.ws.onerror = this.ws_onError.bind(this);
+
+        console.log("[WebSocket] Starting...");
+    }
+}
+
+export default { ReconnectingWebsocket }

photon-client/src/plugins/objectDiff.js

@@ -1,121 +0,0 @@
-//https://gomakethings.com/getting-the-differences-between-two-objects-with-vanilla-js/
-export const diff = function (obj1, obj2) {
-
-    // Make sure an object to compare is provided
-    if (!obj2 || Object.prototype.toString.call(obj2) !== '[object Object]') {
-        return obj1;
-    }
-
-    //
-    // Variables
-    //
-
-    let diffs = {};
-    let key;
-
-
-    //
-    // Methods
-    //
-
-    /**
-     * Check if two arrays are equal
-     * @param  {Array}   arr1 The first array
-     * @param  {Array}   arr2 The second array
-     * @return {Boolean}      If true, both arrays are equal
-     */
-    const arraysMatch = function (arr1, arr2) {
-
-        // Check if the arrays are the same length
-        if (arr1.length !== arr2.length) return false;
-
-        // Check if all items exist and are in the same order
-        for (let i = 0; i < arr1.length; i++) {
-            if (arr1[i] !== arr2[i]) return false;
-        }
-
-        // Otherwise, return true
-        return true;
-
-    };
-
-    /**
-     * Compare two items and push non-matches to object
-     * @param  {*}      item1 The first item
-     * @param  {*}      item2 The second item
-     * @param  {String} key   The key in our object
-     */
-    const compare = function (item1, item2, key) {
-
-        // Get the object type
-        let type1 = Object.prototype.toString.call(item1);
-        let type2 = Object.prototype.toString.call(item2);
-
-        // If type2 is undefined it has been removed
-        if (type2 === '[object Undefined]') {
-            diffs[key] = null;
-            return;
-        }
-
-        // If items are different types
-        if (type1 !== type2) {
-            diffs[key] = item2;
-            return;
-        }
-
-        // If an object, compare recursively
-        if (type1 === '[object Object]') {
-            let objDiff = diff(item1, item2);
-            if (Object.keys(objDiff).length > 1) {
-                diffs[key] = objDiff;
-            }
-            return;
-        }
-
-        // If an array, compare
-        if (type1 === '[object Array]') {
-            if (!arraysMatch(item1, item2)) {
-                diffs[key] = item2;
-            }
-            return;
-        }
-
-        // Else if it's a function, convert to a string and compare
-        // Otherwise, just compare
-        if (type1 === '[object Function]') {
-            if (item1.toString() !== item2.toString()) {
-                diffs[key] = item2;
-            }
-        } else {
-            if (item1 !== item2) {
-                diffs[key] = item2;
-            }
-        }
-
-    };
-
-
-    //
-    // Compare our objects
-    //
-
-    // Loop through the first object
-    for (key in obj1) {
-        if (obj1.hasOwnProperty(key)) {
-            compare(obj1[key], obj2[key], key);
-        }
-    }
-
-    // Loop through the second object and find missing items
-    for (key in obj2) {
-        if (obj2.hasOwnProperty(key)) {
-            if (!obj1[key] && obj1[key] !== obj2[key] ) {
-                diffs[key] = obj2[key];
-            }
-        }
-    }
-
-    // Return the object of differences
-    return diffs;
-
-};

photon-client/src/store/index.js

@@ -52,7 +52,7 @@ export default new Vuex.Store({
                 isFovConfigurable: true,
                 calibrated: false,
                 currentPipelineSettings: {
-                    pipelineType: 4, // One of "calib", "driver", "reflective", "shape", "AprilTag"
+                    pipelineType: 5, // One of "calib", "driver", "reflective", "shape", "AprilTag"
                     // 2 is reflective
 
                     // Settings that apply to all pipeline types
@@ -91,7 +91,7 @@ export default new Vuex.Store({
                     cornerDetectionAccuracyPercentage: 10,
 
                     // Settings that apply to AprilTag
-                    tagFamily: 0,
+                    tagFamily: 1,
                     decimate: 1.0,
                     blur: 0.0,
                     threads: 1,

photon-client/src/views/CamerasView.vue

@@ -676,7 +676,7 @@ export default {
                 console.log("starting calibration with index " + calData.videoModeIndex);
             }
             this.$store.commit('currentPipelineIndex', -2);
-            this.$store.state.websocket.send(this.$msgPack.encode(data));
+            this.$store.state.websocket.ws.send(this.$msgPack.encode(data));
         },
         sendCalibrationFinish() {
             console.log("finishing calibration for index " + this.$store.getters.currentCameraIndex);

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/PipelineView.vue

@@ -1,75 +1,75 @@
 <template>
   <div>
     <v-container
-      class="pa-3"
-      fluid
+        class="pa-3"
+        fluid
     >
       <v-row
-        no-gutters
-        align="center"
-        justify="center"
+          no-gutters
+          align="center"
+          justify="center"
       >
         <v-col
-          cols="12"
-          :class="['pb-3 ', 'pr-lg-3']"
-          lg="8"
-          align-self="stretch"
+            cols="12"
+            :class="['pb-3 ', 'pr-lg-3']"
+            lg="8"
+            align-self="stretch"
         >
           <v-card
-            color="primary"
-            height="100%"
-            style="display: flex; flex-direction: column"
-            dark
+              color="primary"
+              height="100%"
+              style="display: flex; flex-direction: column"
+              dark
           >
             <v-card-title
-              class="pb-0 mb-0 pl-4 pt-1"
-              style="height: 15%; min-height: 50px;"
+                class="pb-0 mb-0 pl-4 pt-1"
+                style="height: 15%; min-height: 50px;"
             >
               Cameras
               <v-chip
-                :class="fpsTooLow ? 'ml-2 mt-1' : 'mt-2'"
-                x-small
-                label
-                :color="fpsTooLow ? 'error' : 'transparent'"
-                :text-color="fpsTooLow ? 'white' : 'grey'"
+                  :class="fpsTooLow ? 'ml-2 mt-1' : 'mt-2'"
+                  x-small
+                  label
+                  :color="fpsTooLow ? 'error' : 'transparent'"
+                  :text-color="fpsTooLow ? 'white' : 'grey'"
               >
                 <span class="pr-1">Processing @ {{ Math.round($store.state.pipelineResults.fps) }}&nbsp;FPS &ndash;</span>
                 <span v-if="fpsTooLow && !$store.getters.currentPipelineSettings.inputShouldShow && $store.getters.pipelineType == 2">HSV thresholds are too broad; narrow them for better performance</span>
-                <span v-else-if="$fpsTooLow && getters.currentCameraSettings.inputShouldShow">stop viewing the raw stream for better performance</span>
+                <span v-else-if="fpsTooLow && getters.currentCameraSettings.inputShouldShow">stop viewing the raw stream for better performance</span>
                 <span v-else>{{ Math.min(Math.round($store.state.pipelineResults.latency), 9999) }} ms latency</span>
               </v-chip>
               <v-switch
-                v-model="driverMode"
-                label="Driver Mode"
-                style="margin-left: auto;"
-                color="accent"
+                  v-model="driverMode"
+                  label="Driver Mode"
+                  style="margin-left: auto;"
+                  color="accent"
               />
             </v-card-title>
             <v-row
-              align="center"
+                align="center"
             >
               <v-col
-                v-for="idx in (selectedOutputs instanceof Array ? selectedOutputs : [selectedOutputs])"
-                :key="idx"
-                cols="12"
-                :md="selectedOutputs.length === 1 ? 12 : Math.floor(12 / selectedOutputs.length)"
-                class="pb-0 pt-0"
-                style="height: 100%;"
+                  v-for="idx in (selectedOutputs instanceof Array ? selectedOutputs : [selectedOutputs])"
+                  :key="idx"
+                  cols="12"
+                  :md="selectedOutputs.length === 1 ? 12 : Math.floor(12 / selectedOutputs.length)"
+                  class="pb-0 pt-0"
+                  style="height: 100%;"
               >
                 <div style="position: relative; width: 100%; height: 100%;">
                   <cv-image
-                    :id="idx === 0 ? 'raw-stream' : 'processed-stream'"
-                    ref="streams"
-                    :idx=idx
-                    :disconnected="!$store.state.backendConnected"
-                    scale="100"
-                    :max-height="$store.getters.isDriverMode ? '40vh' : '300px'"
-                    :max-height-md="$store.getters.isDriverMode ? '50vh' : '380px'"
-                    :max-height-lg="$store.getters.isDriverMode ? '55vh' : '390px'"
-                    :max-height-xl="$store.getters.isDriverMode ? '60vh' : '450px'"
-                    :alt="'Stream ' + idx"
-                    :color-picking="$store.state.colorPicking && idx === 0"
-                    @click="onImageClick"
+                      :id="idx === 0 ? 'raw-stream' : 'processed-stream'"
+                      ref="streams"
+                      :idx=idx
+                      :disconnected="!$store.state.backendConnected"
+                      scale="100"
+                      :max-height="$store.getters.isDriverMode ? '40vh' : '300px'"
+                      :max-height-md="$store.getters.isDriverMode ? '50vh' : '380px'"
+                      :max-height-lg="$store.getters.isDriverMode ? '55vh' : '390px'"
+                      :max-height-xl="$store.getters.isDriverMode ? '60vh' : '450px'"
+                      :alt="idx === 0 ? 'Raw stream' : 'Processed stream'"
+                      :color-picking="$store.state.colorPicking && idx === 0"
+                      @click="onImageClick"
                   />
                 </div>
               </v-col>
@@ -77,44 +77,44 @@
           </v-card>
         </v-col>
         <v-col
-          cols="12"
-          class="pb-3"
-          lg="4"
-          align-self="stretch"
+            cols="12"
+            class="pb-3"
+            lg="4"
+            align-self="stretch"
         >
           <v-card
-            color="primary"
+              color="primary"
           >
             <camera-and-pipeline-select />
           </v-card>
           <v-card
-            :disabled="$store.getters.isDriverMode || $store.state.colorPicking"
-            class="mt-3"
-            color="primary"
+              :disabled="$store.getters.isDriverMode || $store.state.colorPicking"
+              class="mt-3"
+              color="primary"
           >
             <v-row
-              align="center"
-              class="pl-3 pr-3"
+                align="center"
+                class="pl-3 pr-3"
             >
               <v-col lg="12">
                 <p style="color: white;">
                   Processing mode:
                 </p>
                 <v-btn-toggle
-                  v-model="processingMode"
-                  mandatory
-                  dark
-                  class="fill"
+                    v-model="processingMode"
+                    mandatory
+                    dark
+                    class="fill"
                 >
                   <v-btn
-                    color="secondary"
+                      color="secondary"
                   >
                     <v-icon>mdi-crop-square</v-icon>
                     <span>2D</span>
                   </v-btn>
                   <v-btn
-                    color="secondary"
-                    @click="on3DClick"
+                      color="secondary"
+                      @click="on3DClick"
                   >
                     <v-icon>mdi-cube-outline</v-icon>
                     <span>3D</span>
@@ -126,22 +126,22 @@
                   Stream display:
                 </p>
                 <v-btn-toggle
-                  v-model="selectedOutputs"
-                  :multiple="$vuetify.breakpoint.mdAndUp"
-                  mandatory
-                  dark
-                  class="fill"
+                    v-model="selectedOutputs"
+                    :multiple="$vuetify.breakpoint.mdAndUp"
+                    mandatory
+                    dark
+                    class="fill"
                 >
                   <v-btn
-                    color="secondary"
-                    class="fill"
+                      color="secondary"
+                      class="fill"
                   >
                     <v-icon>mdi-import</v-icon>
                     <span>Raw</span>
                   </v-btn>
                   <v-btn
-                    color="secondary"
-                    class="fill"
+                      color="secondary"
+                      class="fill"
                   >
                     <v-icon>mdi-export</v-icon>
                     <span>Processed</span>
@@ -154,29 +154,29 @@
       </v-row>
       <v-row no-gutters>
         <v-col
-          v-for="(tabs, idx) in tabGroups"
-          :key="idx"
-          :cols="Math.floor(12 / tabGroups.length)"
-          :class="idx !== tabGroups.length - 1 ? 'pr-3' : ''"
-          align-self="stretch"
+            v-for="(tabs, idx) in tabGroups"
+            :key="idx"
+            :cols="Math.floor(12 / tabGroups.length)"
+            :class="idx !== tabGroups.length - 1 ? 'pr-3' : ''"
+            align-self="stretch"
         >
           <v-card
-            color="primary"
-            height="100%"
-            class="pr-4 pl-4"
+              color="primary"
+              height="100%"
+              class="pr-4 pl-4"
           >
             <v-tabs
-              v-if="!$store.getters.isDriverMode"
-              v-model="selectedTabs[idx]"
-              grow
-              background-color="primary"
-              dark
-              height="48"
-              slider-color="accent"
+                v-if="!$store.getters.isDriverMode"
+                v-model="selectedTabs[idx]"
+                grow
+                background-color="primary"
+                dark
+                height="48"
+                slider-color="accent"
             >
               <v-tab
-                v-for="(tab, i) in tabs"
-                :key="i"
+                  v-for="(tab, i) in tabs"
+                  :key="i"
               >
                 {{ tab.name }}
               </v-tab>
@@ -184,10 +184,10 @@
             <div class="pl-4 pr-4 pt-2">
               <keep-alive>
                 <component
-                  :is="(tabs[selectedTabs[idx]] || tabs[0]).component"
-                  :ref="(tabs[selectedTabs[idx]] || tabs[0]).name"
-                  v-model="$store.getters.pipeline"
-                  @update="$emit('save')"
+                    :is="(tabs[selectedTabs[idx]] || tabs[0]).component"
+                    :ref="(tabs[selectedTabs[idx]] || tabs[0]).name"
+                    v-model="$store.getters.pipeline"
+                    @update="$emit('save')"
                 />
               </keep-alive>
             </div>
@@ -197,18 +197,18 @@
     </v-container>
 
     <v-snackbar
-      v-model="showNTWarning"
-      color="error"
-      timeout="-1"
-      top
+        v-model="showNTWarning"
+        color="error"
+        timeout="-1"
+        top
     >
       {{ $store.state.settings.networkSettings.runNTServer ?
         "NetworkTables server enabled! PhotonLib may not work." :
         "NetworkTables not connected! Are you on a network with a robot?" }}
       <template v-slot:action>
         <v-btn
-          text
-          @click="hideNTWarning = true"
+            text
+            @click="hideNTWarning = true"
         >
           Hide
         </v-btn>
@@ -216,12 +216,12 @@
     </v-snackbar>
 
     <v-dialog
-      v-model="dialog"
-      width="500"
+        v-model="dialog"
+        width="500"
     >
       <v-card
-        color="primary"
-        dark
+          color="primary"
+          dark
       >
         <v-card-title>
           Current resolution not calibrated
@@ -230,9 +230,9 @@
         <v-card-text>
           Because the current resolution {{ this.$store.getters.currentVideoFormat.width }} x {{ this.$store.getters.currentVideoFormat.height }} is not yet calibrated, 3D mode cannot be enabled. Please
           <a
-            href="/#/cameras"
-            class="white--text"
-            @click="$emit('switch-to-cameras')"
+              href="/#/cameras"
+              class="white--text"
+              @click="$emit('switch-to-cameras')"
           > visit the Cameras tab</a> to calibrate this resolution. For now, SolvePNP will do nothing.
         </v-card-text>
 
@@ -241,9 +241,9 @@
         <v-card-actions>
           <v-spacer />
           <v-btn
-            color="white"
-            text
-            @click="closeUncalibratedDialog"
+              color="white"
+              text
+              @click="closeUncalibratedDialog"
           >
             OK
           </v-btn>
@@ -264,252 +264,264 @@ import TargetsTab from "./PipelineViews/TargetsTab";
 import Map3DTab from './PipelineViews/Map3DTab';
 import PnPTab from './PipelineViews/PnPTab';
 import AprilTagTab from './PipelineViews/AprilTagTab';
+import ArucoTab from './PipelineViews/ArucoTab';
 
 export default {
-    name: 'Pipeline',
-    components: {
-        CameraAndPipelineSelect,
-        cvImage,
-        InputTab,
-        ThresholdTab,
-        ContoursTab,
-        OutputTab,
-        TargetsTab,
-        Map3DTab,
-        PnPTab,
-        AprilTagTab,
-    },
-    data() {
-        return {
-            selectedTabsData: [0, 0, 0, 0],
-            counterData: 0,
-            dialog: false,
-            processingModeOverride: false,
-            hideNTWarning: false,
-        }
-    },
-    computed: {
-        selectedTabs: {
-            get() {
-                return this.$store.getters.isDriverMode ? [0] : this.selectedTabsData;
-            },
-            set(value) {
-                this.selectedTabsData = value;
-            }
-        },
-        tabGroups: {
-            get() {
-                let tabs = {
-                    input: {
-                        name: "Input",
-                        component: "InputTab",
-                    },
-                    threshold: {
-                        name: "Threshold",
-                        component: "ThresholdTab",
-                    },
-                    contours: {
-                        name: "Contours",
-                        component: "ContoursTab",
-                    },
-                    apriltag: {
-                        name: "AprilTag",
-                        component: "AprilTagTab",
-                    },
-                    output: {
-                        name: "Output",
-                        component: "OutputTab",
-                    },
-                    targets: {
-                        name: "Targets",
-                        component: "TargetsTab",
-                    },
-                    pnp: {
-                        name: "PnP",
-                        component: "PnPTab",
-                    },
-                    map3d: {
-                        name: "3D",
-                        component: "Map3DTab",
-                    }
-                };
-
-                // If not in 3d, name "3D" is illegal
-                const allow3d = this.$store.getters.currentPipelineSettings.solvePNPEnabled;
-                // If in apriltag, "Threshold" and "Contours" are illegal -- otherwise "AprilTag" is
-                const isAprilTag = (this.$store.getters.currentPipelineSettings.pipelineType - 2) === 2;
-
-                // 2D array of tab names and component names; each sub-array is a separate tab group
-                let ret = [];
-                if (this.$vuetify.breakpoint.smAndDown || this.$store.getters.isDriverMode || (this.$vuetify.breakpoint.mdAndDown && !this.$store.state.compactMode)) {
-                    // One big tab group with all the tabs
-                    ret[0] = Object.values(tabs);
-                } else if (this.$vuetify.breakpoint.mdAndDown || !this.$store.state.compactMode) {
-                    // Two tab groups, one with "input, threshold, contours, output" and the other with "target info, 3D"
-                    ret[0] = [tabs.input, tabs.threshold, tabs.contours, tabs.apriltag, tabs.output];
-                    ret[1] = [tabs.targets, tabs.pnp, tabs.map3d];
-                } else if (this.$vuetify.breakpoint.lgAndDown) {
-                    // Three tab groups, one with "input", one with "threshold, contours, output", and the other with "target info, 3D"
-                    ret[0] = [tabs.input];
-                    ret[1] = [tabs.threshold, tabs.contours, tabs.apriltag, tabs.output];
-                    ret[2] = [tabs.targets, tabs.pnp, tabs.map3d];
-                } else if (this.$vuetify.breakpoint.xl) {
-                    // Three tab groups, one with "input", one with "threshold, contours", and the other with "output, target info, 3D"
-                    ret[0] = [tabs.input];
-                    ret[1] = [tabs.threshold];
-                    ret[2] = [tabs.contours, tabs.apriltag, tabs.output];
-                    ret[3] = [tabs.targets, tabs.pnp, tabs.map3d];
-                }
-
-                for(let i = 0; i < ret.length; i++) {
-                  const group = ret[i];
-
-                  // All the tabs we allow
-                  const filteredGroup = group.filter(it =>
-                          !(!allow3d && it.name === "3D") //Filter out 3D tab any time 3D isn't calibrated
-                       && !((!allow3d || isAprilTag) && it.name === "PnP") //Filter out the PnP config tab if 3D isn't available, or we're doing Apriltags
-                       && !(isAprilTag && (it.name === "Threshold")) //Filter out threshold tab if we're doing apriltags
-                       && !(isAprilTag && (it.name === "Contours")) //Filter out contours if we're doing Apriltag
-                       && !(!isAprilTag && it.name === "AprilTag") //Filter out apriltag unless we actually are doing Apriltags
-                      );
-                  ret[i] = filteredGroup;
-                }
-
-                // One last filter to remove empty lists
-                return ret.filter(it => it !== undefined && it.length > 0);
-            }
-        },
-        processingMode: {
-            get() {
-                return (this.$store.getters.currentPipelineSettings.solvePNPEnabled || this.processingModeOverride) ? 1 : 0;
-            },
-            set(value) {
-                if (this.$store.getters.isCalibrated) {
-                    this.$store.getters.currentPipelineSettings.solvePNPEnabled = value === 1;
-                    this.handlePipelineUpdate("solvePNPEnabled", value === 1);
-                }
-            }
-        },
-        driverMode: {
-            get() {
-                return this.$store.getters.isDriverMode;
-            },
-            set(value) {
-                this.$store.getters.currentCameraSettings.currentPipelineIndex = value ? -1 : 0;
-                this.handleInputWithIndex('currentPipeline', value ? -1 : 0);
-            }
-        },
-        selectedOutputs: {
-            // All this logic exists to deal with the reality that the output select buttons sometimes need an array and sometimes need a number (depending on whether or not they're exclusive)
-            get() {
-                // We switch the selector to single-select only on sm-and-down size devices, so we have to return a Number instead of an Array in that state
-                let ret = [];
-                if (this.$store.state.colorPicking) {
-                    ret = [0]; // We want the input stream only while color picking
-                } else if (this.$store.getters.isDriverMode) {
-                    ret = [1]; // We want only the output stream in driver mode
-                } else {
-                  if (this.$store.getters.currentPipelineSettings.inputShouldShow) ret = ret.concat([0]);
-                  if (this.$store.getters.currentPipelineSettings.outputShouldShow) ret = ret.concat([1]);
-                  if (!ret.length) ret = [0];
-                }
-
-                if (this.$vuetify.breakpoint.mdAndUp) {
-                    return ret;
-                } else {
-                    return ret[0] || 0;
-                }
-            },
-            set(value) {
-                let valToCommit = [0];
-                if (value instanceof Array) {
-                  // Value is already an array, we don't need to do anything
-                  valToCommit = value;
-                } else if (value) {
-                  // Value is assumed to be a number, so we wrap it into an array
-                  valToCommit = [value];
-                }
-
-                this.$store.commit("mutatePipeline", {"inputShouldShow": valToCommit.includes(0)});
-                this.$store.commit("mutatePipeline", {"outputShouldShow": valToCommit.includes(1)});
-                this.handlePipelineUpdate("inputShouldShow", valToCommit.includes(0));
-            }
-        },
-        fpsTooLow: {
-            get() {
-                // For now we only show the FPS is too low warning when GPU acceleration is enabled, because we don't really trust the presented video modes otherwise
-                return this.$store.state.pipelineResults.fps - this.$store.getters.currentVideoFormat.fps < -5 && this.$store.state.pipelineResults.fps !== 0 && !this.$store.getters.isDriverMode && this.$store.state.settings.general.gpuAcceleration;
-            }
-        },
-        latency: {
-            get() {
-                return this.$store.getters.currentPipelineResults.latency;
-            }
-        },
-        isCalibrated: {
-          get() {
-            const resolution = this.$store.getters.videoFormatList[this.$store.getters.currentPipelineSettings.cameraVideoModeIndex];
-            return this.$store.getters.currentCameraSettings.calibrations
-                    .some(e => e.width === resolution.width && e.height === resolution.height)
-          }
-        },
-        isRobotConnected: {
-          get() {
-            // return this.$store.state.ntConnectionInfo.connected && this.$store.state.backendConnected;
-            return true;
-          }
-        },
-        showNTWarning: {
-          get() {
-            return (!this.$store.state.ntConnectionInfo.connected || this.$store.state.settings.networkSettings.runNTServer) && this.$store.state.settings.networkSettings.teamNumber > 0 && this.$store.state.backendConnected && !this.hideNTWarning;
-          }
-        },
-    },
-    created() {
-        this.$store.state.connectedCallbacks.push(this.reloadStreams)
-    },
-    methods: {
-        reloadStreams() {
-            // Reload the streams as we technically close and reopen them
-            this.$refs.streams.forEach(it => it.reload())
-        },
-        onImageClick(event) {
-            // Only run on the input stream
-            if (event.target.alt !== "Stream0") return;
-            // Get a reference to the threshold tab (if it is shown) and call its "onClick" method
-            let ref = this.$refs["Threshold"];
-            if (ref && ref[0])
-                ref[0].onClick(event)
-        },
-        on3DClick() {
-            if (!this.$store.getters.isCalibrated) {
-                this.dialog = true;
-                this.processingModeOverride = true;
-            }
-        },
-        closeUncalibratedDialog() {
-            this.dialog = false;
-            this.processingModeOverride = false;
-            // this.$store.getters.currentPipelineSettings.solvePNPEnabled = false;
-            this.handlePipelineUpdate("solvePNPEnabled", false);
-        }
+  name: 'Pipeline',
+  components: {
+    CameraAndPipelineSelect,
+    cvImage,
+    InputTab,
+    ThresholdTab,
+    ContoursTab,
+    OutputTab,
+    TargetsTab,
+    Map3DTab,
+    PnPTab,
+    AprilTagTab,
+    ArucoTab,
+  },
+  data() {
+    return {
+      selectedTabsData: [0, 0, 0, 0],
+      counterData: 0,
+      dialog: false,
+      processingModeOverride: false,
+      hideNTWarning: false,
     }
+  },
+  computed: {
+    selectedTabs: {
+      get() {
+        return this.$store.getters.isDriverMode ? [0] : this.selectedTabsData;
+      },
+      set(value) {
+        this.selectedTabsData = value;
+      }
+    },
+    tabGroups: {
+      get() {
+        let tabs = {
+          input: {
+            name: "Input",
+            component: "InputTab",
+          },
+          threshold: {
+            name: "Threshold",
+            component: "ThresholdTab",
+          },
+          contours: {
+            name: "Contours",
+            component: "ContoursTab",
+          },
+          apriltag: {
+            name: "AprilTag",
+            component: "AprilTagTab",
+          },
+          aruco: {
+            name: "Aruco",
+            component: "ArucoTab",
+          },
+          output: {
+            name: "Output",
+            component: "OutputTab",
+          },
+          targets: {
+            name: "Targets",
+            component: "TargetsTab",
+          },
+          pnp: {
+            name: "PnP",
+            component: "PnPTab",
+          },
+          map3d: {
+            name: "3D",
+            component: "Map3DTab",
+          }
+        };
+
+        // If not in 3d, name "3D" is illegal
+        const allow3d = this.$store.getters.currentPipelineSettings.solvePNPEnabled;
+        // If in apriltag, "Threshold" and "Contours" are illegal -- otherwise "AprilTag" is
+        const isAprilTag = (this.$store.getters.currentPipelineSettings.pipelineType - 2) === 2;
+        const isAruco = (this.$store.getters.currentPipelineSettings.pipelineType - 2) === 3;
+
+        // 2D array of tab names and component names; each sub-array is a separate tab group
+        let ret = [];
+        if (this.$vuetify.breakpoint.smAndDown || this.$store.getters.isDriverMode || (this.$vuetify.breakpoint.mdAndDown && !this.$store.state.compactMode)) {
+          // One big tab group with all the tabs
+          ret[0] = Object.values(tabs);
+        } else if (this.$vuetify.breakpoint.mdAndDown || !this.$store.state.compactMode) {
+          // Two tab groups, one with "input, threshold, contours, output" and the other with "target info, 3D"
+          ret[0] = [tabs.input, tabs.threshold, tabs.contours, tabs.apriltag, tabs.aruco, tabs.output];
+          ret[1] = [tabs.targets, tabs.pnp, tabs.map3d];
+        } else if (this.$vuetify.breakpoint.lgAndDown) {
+          // Three tab groups, one with "input", one with "threshold, contours, output", and the other with "target info, 3D"
+          ret[0] = [tabs.input];
+          ret[1] = [tabs.threshold, tabs.contours, tabs.apriltag,tabs.aruco, tabs.output];
+          ret[2] = [tabs.targets, tabs.pnp, tabs.map3d];
+        } else if (this.$vuetify.breakpoint.xl) {
+          // Three tab groups, one with "input", one with "threshold, contours", and the other with "output, target info, 3D"
+          ret[0] = [tabs.input];
+          ret[1] = [tabs.threshold];
+          ret[2] = [tabs.contours, tabs.apriltag, tabs.aruco,tabs.output];
+          ret[3] = [tabs.targets, tabs.pnp, tabs.map3d];
+        }
+
+        for(let i = 0; i < ret.length; i++) {
+          const group = ret[i];
+
+          // All the tabs we allow
+          const filteredGroup = group.filter(it =>
+              !(!allow3d && it.name === "3D") //Filter out 3D tab any time 3D isn't calibrated
+              && !((!allow3d || isAprilTag || isAruco) && it.name === "PnP") //Filter out the PnP config tab if 3D isn't available, or we're doing Apriltags
+              && !((isAprilTag || isAruco) && (it.name === "Threshold")) //Filter out threshold tab if we're doing apriltags
+              && !((isAprilTag || isAruco)&& (it.name === "Contours")) //Filter out contours if we're doing Apriltag
+              && !(!isAprilTag && it.name === "AprilTag") //Filter out apriltag unless we actually are doing Apriltags
+              && !(!isAruco && it.name === "Aruco")
+          );
+          ret[i] = filteredGroup;
+        }
+
+        // One last filter to remove empty lists
+        return ret.filter(it => it !== undefined && it.length > 0);
+      }
+    },
+    processingMode: {
+      get() {
+        return (this.$store.getters.currentPipelineSettings.solvePNPEnabled || this.processingModeOverride) ? 1 : 0;
+      },
+      set(value) {
+        if (this.$store.getters.isCalibrated) {
+          this.$store.getters.currentPipelineSettings.solvePNPEnabled = value === 1;
+          this.handlePipelineUpdate("solvePNPEnabled", value === 1);
+        }
+      }
+    },
+    driverMode: {
+      get() {
+        return this.$store.getters.isDriverMode;
+      },
+      set(value) {
+        this.$store.getters.currentCameraSettings.currentPipelineIndex = value ? -1 : 0;
+        this.handleInputWithIndex('currentPipeline', value ? -1 : 0);
+      }
+    },
+    selectedOutputs: {
+      // All this logic exists to deal with the reality that the output select buttons sometimes need an array and sometimes need a number (depending on whether or not they're exclusive)
+      get() {
+        // We switch the selector to single-select only on sm-and-down size devices, so we have to return a Number instead of an Array in that state
+        let ret = [];
+        if (this.$store.state.colorPicking) {
+          ret = [0]; // We want the input stream only while color picking
+        } else if (this.$store.getters.isDriverMode) {
+          ret = [1]; // We want only the output stream in driver mode
+        } else {
+          if (this.$store.getters.currentPipelineSettings.inputShouldShow) ret = ret.concat([0]);
+          if (this.$store.getters.currentPipelineSettings.outputShouldShow) ret = ret.concat([1]);
+          if (!ret.length) ret = [0];
+        }
+
+        if (this.$vuetify.breakpoint.mdAndUp) {
+          return ret;
+        } else {
+          return ret[0] || 0;
+        }
+      },
+      set(value) {
+        let valToCommit = [0];
+        if (value instanceof Array) {
+          // Value is already an array, we don't need to do anything
+          valToCommit = value;
+        } else if (value) {
+          // Value is assumed to be a number, so we wrap it into an array
+          valToCommit = [value];
+        }
+
+        this.$store.commit("mutatePipeline", {"inputShouldShow": valToCommit.includes(0)});
+        this.$store.commit("mutatePipeline", {"outputShouldShow": valToCommit.includes(1)});
+        this.handlePipelineUpdate("inputShouldShow", valToCommit.includes(0));
+      }
+    },
+    fpsTooLow: {
+      get() {
+        // For now we only show the FPS is too low warning when GPU acceleration is enabled, because we don't really trust the presented video modes otherwise
+        const currFPS = this.$store.state.pipelineResults.fps;
+        const targetFPS = this.$store.getters.currentVideoFormat.fps;
+        const driverMode = this.$store.getters.isDriverMode;
+        const gpuAccel = this.$store.state.settings.general.gpuAcceleration === true;
+        const isReflective = this.$store.getters.pipelineType === 2;
+
+        return (currFPS - targetFPS) < -5 && this.$store.state.pipelineResults.fps !== 0 && !driverMode && gpuAccel && isReflective;
+      }
+    },
+    latency: {
+      get() {
+        return this.$store.getters.currentPipelineResults.latency;
+      }
+    },
+    isCalibrated: {
+      get() {
+        const resolution = this.$store.getters.videoFormatList[this.$store.getters.currentPipelineSettings.cameraVideoModeIndex];
+        return this.$store.getters.currentCameraSettings.calibrations
+            .some(e => e.width === resolution.width && e.height === resolution.height)
+      }
+    },
+    isRobotConnected: {
+      get() {
+        // return this.$store.state.ntConnectionInfo.connected && this.$store.state.backendConnected;
+        return true;
+      }
+    },
+    showNTWarning: {
+      get() {
+        return (!this.$store.state.ntConnectionInfo.connected || this.$store.state.settings.networkSettings.runNTServer) && this.$store.state.settings.networkSettings.teamNumber > 0 && this.$store.state.backendConnected && !this.hideNTWarning;
+      }
+    },
+  },
+  created() {
+    this.$store.state.connectedCallbacks.push(this.reloadStreams)
+  },
+  methods: {
+    reloadStreams() {
+      // Reload the streams as we technically close and reopen them
+      this.$refs.streams.forEach(it => it.reload())
+    },
+    onImageClick(event) {
+      // Get a reference to the threshold tab (if it is shown) and call its "onClick" method
+      let ref = this.$refs["Threshold"];
+      if (ref && ref[0])
+        ref[0].onClick(event)
+    },
+    on3DClick() {
+      if (!this.$store.getters.isCalibrated) {
+        this.dialog = true;
+        this.processingModeOverride = true;
+      }
+    },
+    closeUncalibratedDialog() {
+      this.dialog = false;
+      this.processingModeOverride = false;
+      // this.$store.getters.currentPipelineSettings.solvePNPEnabled = false;
+      this.handlePipelineUpdate("solvePNPEnabled", false);
+    }
+  }
 }
 </script>
 
 <style scoped>
 .v-btn-toggle.fill {
-    width: 100%;
-    height: 100%;
+  width: 100%;
+  height: 100%;
 }
 
 .v-btn-toggle.fill > .v-btn {
-    width: 50%;
-    height: 100%;
+  width: 50%;
+  height: 100%;
 }
 
 th {
-    width: 80px;
-    text-align: center;
+  width: 80px;
+  text-align: center;
 }
 </style>

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

@@ -1,196 +1,154 @@
 <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)"
+    <CVselect
+        v-model="selectedFamily"
+        name="Target family"
+        :list="['AprilTag family 36h11', 'AprilTag family 25h9', 'AprilTag family 16h5']"
+        select-cols="8"
+        @input="handlePipelineUpdate('tagFamily', 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'
+import CVselect from '../../components/common/cv-select'
 
-    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,
+    CVselect,
+  },
+  data() {
+    return {
+      familyList: ["AprilTag family 36h11", "AprilTag family 25h9", "AprilTag family 16h5"],
     }
+  },
+  computed: {
+    selectedFamily: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.tagFamily
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"tagFamily": 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/PipelineViews/ArucoTab.vue

@@ -0,0 +1,76 @@
+<template>
+  <div>
+    <CVslider
+        v-model="decimate"
+        class="pt-2"
+        slider-cols="8"
+        name="Decimate"
+        min="1"
+        max="8"
+        step=".5"
+        tooltip="Increases FPS at the expense of range by reducing image resolution initially"
+        @input="handlePipelineData('decimate')"
+    />
+    <CVslider
+        v-model="numIterations"
+        class="pt-2"
+        slider-cols="8"
+        name="Corner Iterations"
+        min="30"
+        max="1000"
+        step="5"
+        tooltip="How many iterations are going to be used in order to refine corners. Higher values are lead to more accuracy at the cost of performance"
+        @input="handlePipelineData('numIterations')"
+    />
+    <CVslider
+        v-model="cornerAccuracy"
+        class="pt-2"
+        slider-cols="8"
+        name="Corner Accuracy"
+        min=".01"
+        max="100"
+        step=".01"
+        tooltip="Minimum accuracy for the corners, lower is better but more performance intensive "
+        @input="handlePipelineData('cornerAccuracy')"
+    />
+  </div>
+</template>
+
+<script>
+import CVslider from '../../components/common/cv-slider'
+
+export default {
+  name: "Aruco",
+  components: {
+    CVslider
+  },
+  computed: {
+    decimate: {
+      get() {
+        return this.$store.getters.currentPipelineSettings.decimate
+      },
+      set(val) {
+        this.$store.commit("mutatePipeline", {"decimate": val});
+      },
+    },
+      numIterations: {
+        get() {
+          return this.$store.getters.currentPipelineSettings.numIterations
+        },
+        set(val) {
+          this.$store.commit("mutatePipeline", {"numIterations": val});
+        },
+      },
+        cornerAccuracy: {
+          get() {
+            return this.$store.getters.currentPipelineSettings.cornerAccuracy
+          },
+          set(val) {
+            this.$store.commit("mutatePipeline", {"cornerAccuracy": val});
+          },
+        },
+    },
+    methods: {
+    }
+  }
+</script>

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

@@ -62,11 +62,13 @@
       @input="handlePipelineData('cameraBlueGain')"
       @rollback="e => rollback('cameraBlueGain', e)"
     />
+    <!-- TODO: stop filtering out the 90 degree rotation modes when we fix those in libcamera -->
     <CVselect
       v-model="inputImageRotationMode"
       name="Orientation"
       tooltip="Rotates the camera stream"
       :list="['Normal','90° CW','180°','90° CCW']"
+      :filtered-indices="this.$store.state.settings.general.gpuAcceleration ? new Set([1, 3]) : undefined"
       :select-cols="largeBox"
       @input="handlePipelineData('inputImageRotationMode')"
       @rollback="e => rollback('inputImageRotationMode',e)"

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

@@ -10,6 +10,7 @@
     />
 
     <CVselect
+      v-if="!isTagPipeline"
       v-model="contourTargetOrientation"
       name="Target Orientation"
       tooltip="Used to determine how to calculate target landmarks (e.g. the top, left, or bottom of the target)"
@@ -21,7 +22,8 @@
     <CVswitch
       v-model="outputShowMultipleTargets"
       name="Show Multiple Targets"
-      tooltip="If enabled, up to five targets will be displayed and sent to user code"
+      tooltip="If enabled, up to five targets will be displayed and sent to user code, instead of just one"
+      :disabled="isTagPipeline"
       class="mb-4"
       text-cols="3"
       @input="handlePipelineData('outputShowMultipleTargets')"
@@ -137,6 +139,11 @@
                 get() {
                     return undefined; // TODO fix
                 }
+            },
+            isTagPipeline: {
+                get() {
+                     return this.$store.getters.currentPipelineSettings.pipelineType > 3;
+                }
             }
         },
         methods: {

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

@@ -19,7 +19,7 @@
                 Target
               </th>
               <th
-                v-if="$store.getters.pipelineType === 4"
+                  v-if="$store.getters.pipelineType === 4 || (($store.getters.pipelineType - 2) === 3)"
                 class="text-center"
               >
                 Fiducial ID
@@ -62,7 +62,7 @@
               :key="index"
             >
               <td>{{ index }}</td>
-              <td v-if="$store.getters.pipelineType === 4">
+              <td v-if="$store.getters.pipelineType === 4 || (($store.getters.pipelineType - 2) === 3)">
                 {{ parseInt(value.fiducialId) }}
               </td>
               <template v-if="!$store.getters.currentPipelineSettings.solvePNPEnabled">

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

@@ -247,7 +247,7 @@ export default {
             'cameraIndex': this.$store.state.currentCameraIndex
           }
         });
-        this.$store.state.websocket.send(msg);
+        this.$store.state.websocket.ws.send(msg);
         this.$emit('update');
       }
     },

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());
 
@@ -189,11 +189,21 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
     public static List<PhotonTrackedTarget> simpleFromTrackedTargets(List<TrackedTarget> targets) {
         var ret = new ArrayList<PhotonTrackedTarget>();
         for (var t : targets) {
-            var points = new Point[4];
-            t.getMinAreaRect().points(points);
-            var cornerList = new ArrayList<TargetCorner>();
-
-            for (int i = 0; i < 4; i++) cornerList.add(new TargetCorner(points[i].x, points[i].y));
+            var minAreaRectCorners = new ArrayList<TargetCorner>();
+            var detectedCorners = new ArrayList<TargetCorner>();
+            {
+                var points = new Point[4];
+                t.getMinAreaRect().points(points);
+                for (int i = 0; i < 4; i++) {
+                    minAreaRectCorners.add(new TargetCorner(points[i].x, points[i].y));
+                }
+            }
+            {
+                var points = t.getTargetCorners();
+                for (int i = 0; i < points.size(); i++) {
+                    detectedCorners.add(new TargetCorner(points.get(i).x, points.get(i).y));
+                }
+            }
 
             ret.add(
                     new PhotonTrackedTarget(
@@ -205,7 +215,8 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
                             t.getBestCameraToTarget3d(),
                             t.getAltCameraToTarget3d(),
                             t.getPoseAmbiguity(),
-                            cornerList));
+                            minAreaRectCorners,
+                            detectedCorners));
         }
         return ret;
     }

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

@@ -196,14 +196,18 @@ public class TestUtils {
 
     public enum ApriltagTestImages {
         kRobots,
-        kTag1_640_480;
+        kTag1_640_480,
+        kTag1_16h5_1280,
+        kTag_corner_1280;
 
         public final Path path;
 
         Path getPath() {
             // Strip leading k
             var filename = this.toString().substring(1).toLowerCase();
-            return Path.of("apriltag", filename + ".jpg");
+            var extension = ".jpg";
+            if (filename.equals("tag1_16h5_1280")) extension = ".png";
+            return Path.of("apriltag", filename + extension);
         }
 
         ApriltagTestImages() {
@@ -302,6 +306,7 @@ public class TestUtils {
 
     private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json";
     private static final String LIFECAM_480P_CAL_FILE = "lifecam480p.json";
+    public static final String LIMELIGHT_480P_CAL_FILE = "limelight_1280_720.json";
 
     public static CameraCalibrationCoefficients getCoeffs(String filename, boolean testMode) {
         try {

photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java

@@ -172,6 +172,17 @@ public class MathUtils {
     private static final Rotation3d WPILIB_BASE_ROTATION =
             new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(0, 1, 0, 0, 0, 1, 1, 0, 0));
 
+    public static Transform3d convertOpenCVtoPhotonTransform(Transform3d cameraToTarget3d) {
+        // TODO: Refactor into new pipe?
+        // CameraToTarget _should_ be in opencv-land EDN
+        var nwu =
+                CoordinateSystem.convert(
+                        new Pose3d().transformBy(cameraToTarget3d),
+                        CoordinateSystem.EDN(),
+                        CoordinateSystem.NWU());
+        return new Transform3d(nwu.getTranslation(), WPILIB_BASE_ROTATION.rotateBy(nwu.getRotation()));
+    }
+
     public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
         // TODO: Refactor into new pipe?
         // CameraToTarget _should_ be in opencv-land EDN
@@ -208,6 +219,14 @@ public class MathUtils {
         return new Transform3d(pose.getTranslation(), ocvRotation);
     }
 
+    public static Pose3d convertArucotoOpenCV(Transform3d pose) {
+        var ocvRotation =
+                APRILTAG_BASE_ROTATION.rotateBy(
+                        new Rotation3d(VecBuilder.fill(0, 0, 1), Units.degreesToRadians(180))
+                                .rotateBy(pose.getRotation()));
+        return new Pose3d(pose.getTranslation(), ocvRotation);
+    }
+
     public static void rotationToOpencvRvec(Rotation3d rotation, Mat rvecOutput) {
         var angle = rotation.getAngle();
         var axis = rotation.getAxis().times(angle);

photon-core/src/main/java/org/photonvision/vision/aruco/ArucoDetectionResult.java

@@ -0,0 +1,83 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.aruco;
+
+import java.util.Arrays;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+
+public class ArucoDetectionResult {
+    private static final Logger logger =
+            new Logger(ArucoDetectionResult.class, LogGroup.VisionModule);
+    double[] xCorners;
+    double[] yCorners;
+
+    int id;
+
+    double[] tvec, rvec;
+
+    public ArucoDetectionResult(
+            double[] xCorners, double[] yCorners, int id, double[] tvec, double[] rvec) {
+        this.xCorners = xCorners;
+        this.yCorners = yCorners;
+        this.id = id;
+        this.tvec = tvec;
+        this.rvec = rvec;
+        // logger.debug("Creating a new detection result: " + this.toString());
+    }
+
+    public double[] getTvec() {
+        return tvec;
+    }
+
+    public double[] getRvec() {
+        return rvec;
+    }
+
+    public double[] getxCorners() {
+        return xCorners;
+    }
+
+    public double[] getyCorners() {
+        return yCorners;
+    }
+
+    public int getId() {
+        return id;
+    }
+
+    public double getCenterX() {
+        return (xCorners[0] + xCorners[1] + xCorners[2] + xCorners[3]) * .25;
+    }
+
+    public double getCenterY() {
+        return (yCorners[0] + yCorners[1] + yCorners[2] + yCorners[3]) * .25;
+    }
+
+    @Override
+    public String toString() {
+        return "ArucoDetectionResult{"
+                + "xCorners="
+                + Arrays.toString(xCorners)
+                + ", yCorners="
+                + Arrays.toString(yCorners)
+                + ", id="
+                + id
+                + '}';
+    }
+}

photon-core/src/main/java/org/photonvision/vision/aruco/ArucoDetectorParams.java

@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.aruco;
+
+import org.opencv.aruco.Aruco;
+import org.opencv.aruco.ArucoDetector;
+import org.opencv.aruco.DetectorParameters;
+import org.opencv.aruco.Dictionary;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+
+public class ArucoDetectorParams {
+    private static final Logger logger = new Logger(PhotonArucoDetector.class, LogGroup.VisionModule);
+
+    private float m_decimate = -1;
+    private int m_iterations = -1;
+    private double m_accuracy = -1;
+
+    DetectorParameters parameters = DetectorParameters.create();
+    ArucoDetector detector;
+
+    public ArucoDetectorParams() {
+        setDecimation(1);
+        setCornerAccuracy(25);
+        setCornerRefinementMaxIterations(100);
+
+        detector = new ArucoDetector(Dictionary.get(Aruco.DICT_APRILTAG_16h5), parameters);
+    }
+
+    public void setDecimation(float decimate) {
+        if (decimate == m_decimate) return;
+
+        logger.info("Setting decimation from " + m_decimate + " to " + decimate);
+
+        // We only need to mutate the parameters -- the detector keeps a poitner to the parameters
+        // object internally, so it should automatically update
+        parameters.set_aprilTagQuadDecimate((float) decimate);
+        m_decimate = decimate;
+    }
+
+    public void setCornerRefinementMaxIterations(int iters) {
+        if (iters == m_iterations || iters <= 0) return;
+
+        parameters.set_cornerRefinementMethod(Aruco.CORNER_REFINE_SUBPIX);
+        parameters.set_cornerRefinementMaxIterations(iters); // 200
+
+        m_iterations = iters;
+    }
+
+    public void setCornerAccuracy(double accuracy) {
+        if (accuracy == m_accuracy || accuracy <= 0) return;
+
+        parameters.set_cornerRefinementMinAccuracy(
+                accuracy / 1000.0); // divides by 1000 because the UI multiplies it by 1000
+        m_accuracy = accuracy;
+    }
+
+    public ArucoDetector getDetector() {
+        return detector;
+    }
+}

photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java

@@ -0,0 +1,132 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.aruco;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import java.util.ArrayList;
+import org.opencv.aruco.Aruco;
+import org.opencv.aruco.ArucoDetector;
+import org.opencv.core.Mat;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+
+public class PhotonArucoDetector {
+    private static final Logger logger = new Logger(PhotonArucoDetector.class, LogGroup.VisionModule);
+
+    private static final Rotation3d ARUCO_BASE_ROTATION =
+            new Rotation3d(VecBuilder.fill(0, 0, 1), Units.degreesToRadians(180));
+
+    Mat ids;
+
+    Mat tvecs;
+    Mat rvecs;
+    ArrayList<Mat> corners;
+
+    Mat cornerMat;
+    Translation3d translation;
+    Rotation3d rotation;
+    double timeStartDetect;
+    double timeEndDetect;
+    Pose3d tagPose;
+    double timeStartProcess;
+    double timeEndProcess;
+    double[] xCorners = new double[4];
+    double[] yCorners = new double[4];
+
+    public PhotonArucoDetector() {
+        logger.debug("New Aruco Detector");
+        ids = new Mat();
+        tvecs = new Mat();
+        rvecs = new Mat();
+        corners = new ArrayList<Mat>();
+        tagPose = new Pose3d();
+        translation = new Translation3d();
+        rotation = new Rotation3d();
+    }
+
+    public ArucoDetectionResult[] detect(
+            Mat grayscaleImg,
+            float tagSize,
+            CameraCalibrationCoefficients coeffs,
+            ArucoDetector detector) {
+        detector.detectMarkers(grayscaleImg, corners, ids);
+        if (coeffs != null) {
+            Aruco.estimatePoseSingleMarkers(
+                    corners,
+                    tagSize,
+                    coeffs.getCameraIntrinsicsMat(),
+                    coeffs.getDistCoeffsMat(),
+                    rvecs,
+                    tvecs);
+        }
+
+        ArucoDetectionResult[] toReturn = new ArucoDetectionResult[corners.size()];
+        timeStartProcess = System.currentTimeMillis();
+        for (int i = 0; i < corners.size(); i++) {
+            cornerMat = corners.get(i);
+            // logger.debug(cornerMat.dump());
+            xCorners =
+                    new double[] {
+                        cornerMat.get(0, 0)[0],
+                        cornerMat.get(0, 1)[0],
+                        cornerMat.get(0, 2)[0],
+                        cornerMat.get(0, 3)[0]
+                    };
+            yCorners =
+                    new double[] {
+                        cornerMat.get(0, 0)[1],
+                        cornerMat.get(0, 1)[1],
+                        cornerMat.get(0, 2)[1],
+                        cornerMat.get(0, 3)[1]
+                    };
+            cornerMat.release();
+
+            double[] tvec;
+            double[] rvec;
+            if (coeffs != null) {
+                // Need to apply a 180 rotation about Z
+                var origRvec = rvecs.get(i, 0);
+                var axisangle = VecBuilder.fill(origRvec[0], origRvec[1], origRvec[2]);
+                Rotation3d rotation = new Rotation3d(axisangle, axisangle.normF());
+                var ocvRotation = ARUCO_BASE_ROTATION.rotateBy(rotation);
+
+                var angle = ocvRotation.getAngle();
+                var finalAxisAngle = ocvRotation.getAxis().times(angle);
+
+                tvec = tvecs.get(i, 0);
+                rvec = finalAxisAngle.getData();
+            } else {
+                tvec = new double[] {0, 0, 0};
+                rvec = new double[] {0, 0, 0};
+            }
+
+            toReturn[i] =
+                    new ArucoDetectionResult(xCorners, yCorners, (int) ids.get(i, 0)[0], tvec, rvec);
+        }
+        rvecs.release();
+        tvecs.release();
+        ids.release();
+
+        return toReturn;
+    }
+}

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/AprilTagPoseEstimatorPipe.java

@@ -21,6 +21,10 @@ import edu.wpi.first.apriltag.AprilTagDetection;
 import edu.wpi.first.apriltag.AprilTagPoseEstimate;
 import edu.wpi.first.apriltag.AprilTagPoseEstimator;
 import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config;
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.Point;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.pipe.CVPipe;
 
 public class AprilTagPoseEstimatorPipe
@@ -37,9 +41,48 @@ public class AprilTagPoseEstimatorPipe
         super();
     }
 
+    MatOfPoint2f temp = new MatOfPoint2f();
+
     @Override
     protected AprilTagPoseEstimate process(AprilTagDetection in) {
-        return m_poseEstimator.estimateOrthogonalIteration(in, params.nIters);
+        // Save the corner points of our detection to an array
+        Point corners[] = new Point[4];
+        for (int i = 0; i < 4; i++) {
+            corners[i] = new Point(in.getCornerX(i), in.getCornerY(i));
+        }
+        // And shove into our matofpoints
+        temp.fromArray(corners);
+
+        // Probably overwrites what was in temp before. I hope
+        Calib3d.undistortImagePoints(
+                temp,
+                temp,
+                params.calibration.getCameraIntrinsicsMat(),
+                params.calibration.getDistCoeffsMat());
+
+        // Save out undistorted corners
+        corners = temp.toArray();
+
+        // Apriltagdetection expects an array in form [x1 y1 x2 y2 ...]
+        var fixedCorners = new double[8];
+        for (int i = 0; i < 4; i++) {
+            fixedCorners[i * 2] = corners[i].x;
+            fixedCorners[i * 2 + 1] = corners[i].y;
+        }
+
+        // Create a new Detection with the fixed corners
+        var corrected =
+                new AprilTagDetection(
+                        in.getFamily(),
+                        in.getId(),
+                        in.getHamming(),
+                        in.getDecisionMargin(),
+                        in.getHomography(),
+                        in.getCenterX(),
+                        in.getCenterY(),
+                        fixedCorners);
+
+        return m_poseEstimator.estimateOrthogonalIteration(corrected, params.nIters);
     }
 
     @Override
@@ -57,11 +100,14 @@ public class AprilTagPoseEstimatorPipe
 
     public static class AprilTagPoseEstimatorPipeParams {
         final AprilTagPoseEstimator.Config config;
+        final CameraCalibrationCoefficients calibration;
         final int nIters;
 
-        public AprilTagPoseEstimatorPipeParams(Config config, int nIters) {
+        public AprilTagPoseEstimatorPipeParams(
+                Config config, CameraCalibrationCoefficients cal, int nIters) {
             this.config = config;
             this.nIters = nIters;
+            this.calibration = cal;
         }
 
         @Override

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

@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import edu.wpi.first.math.util.Units;
+import java.util.List;
+import org.opencv.aruco.DetectorParameters;
+import org.opencv.core.Mat;
+import org.photonvision.vision.aruco.ArucoDetectionResult;
+import org.photonvision.vision.aruco.PhotonArucoDetector;
+import org.photonvision.vision.pipe.CVPipe;
+
+public class ArucoDetectionPipe
+        extends CVPipe<Mat, List<ArucoDetectionResult>, ArucoDetectionPipeParams> {
+    PhotonArucoDetector detector = new PhotonArucoDetector();
+
+    @Override
+    protected List<ArucoDetectionResult> process(Mat in) {
+        return List.of(
+                detector.detect(
+                        in,
+                        (float) Units.inchesToMeters(6),
+                        params.cameraCalibrationCoefficients,
+                        params.detectorParams));
+    }
+
+    @Override
+    public void setParams(ArucoDetectionPipeParams params) {
+        super.setParams(params);
+    }
+
+    public DetectorParameters getParameters() {
+        return params == null ? null : params.detectorParams.get_params();
+    }
+}

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

@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import java.util.Objects;
+import org.opencv.aruco.ArucoDetector;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+
+public class ArucoDetectionPipeParams {
+    public ArucoDetector detectorParams;
+    public final CameraCalibrationCoefficients cameraCalibrationCoefficients;
+
+    public ArucoDetectionPipeParams(
+            ArucoDetector detector, CameraCalibrationCoefficients cameraCalibrationCoefficients) {
+        this.detectorParams = detector;
+        this.cameraCalibrationCoefficients = cameraCalibrationCoefficients;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+        if (this == o) return true;
+        if (o == null || getClass() != o.getClass()) return false;
+        ArucoDetectionPipeParams that = (ArucoDetectionPipeParams) o;
+        return Objects.equals(detectorParams, that.detectorParams)
+                && Objects.equals(cameraCalibrationCoefficients, that.cameraCalibrationCoefficients);
+    }
+
+    @Override
+    public int hashCode() {
+        return Objects.hash(detectorParams, cameraCalibrationCoefficients);
+    }
+
+    @Override
+    public String toString() {
+        return "ArucoDetectionPipeParams{"
+                + "detectorParams="
+                + detectorParams
+                + ", cameraCalibrationCoefficients="
+                + cameraCalibrationCoefficients
+                + '}';
+    }
+}

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

@@ -47,7 +47,7 @@ public class CornerDetectionPipe
                     {
                         var targetCorners =
                                 detectExtremeCornersByApproxPolyDp(target, params.calculateConvexHulls);
-                        target.setCorners(targetCorners);
+                        target.setTargetCorners(targetCorners);
                         break;
                     }
                 default:

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

@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import java.awt.*;
+import org.photonvision.vision.frame.FrameDivisor;
+
+public class Draw2dArucoPipe extends Draw2dTargetsPipe {
+    public static class Draw2dArucoParams extends Draw2dTargetsPipe.Draw2dTargetsParams {
+        public Draw2dArucoParams(
+                boolean shouldDraw, boolean showMultipleTargets, FrameDivisor divisor) {
+            super(shouldDraw, showMultipleTargets, divisor);
+            // We want to show the polygon, not the rotated box
+            this.showRotatedBox = false;
+            this.showMaximumBox = false;
+            this.rotatedBoxColor = Color.RED;
+        }
+    }
+}

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

@@ -30,6 +30,7 @@ public class Draw3dAprilTagsPipe extends Draw3dTargetsPipe {
                 FrameDivisor divisor) {
             super(shouldDraw, cameraCalibrationCoefficients, targetModel, divisor);
             this.shouldDrawHull = false;
+            this.redistortPoints = true;
         }
     }
 }

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

@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+import org.photonvision.vision.frame.FrameDivisor;
+import org.photonvision.vision.target.TargetModel;
+
+public class Draw3dArucoPipe extends Draw3dTargetsPipe {
+    public static class Draw3dArucoParams extends Draw3dContoursParams {
+        public Draw3dArucoParams(
+                boolean shouldDraw,
+                CameraCalibrationCoefficients cameraCalibrationCoefficients,
+                TargetModel targetModel,
+                FrameDivisor divisor) {
+            super(shouldDraw, cameraCalibrationCoefficients, targetModel, divisor);
+            this.shouldDrawHull = false;
+        }
+    }
+}

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

@@ -93,7 +93,12 @@ public class Draw3dTargetsPipe
                         params.cameraCalibrationCoefficients.getDistCoeffsMat(),
                         tempMat,
                         jac);
-                // Distort the points so they match the image they're being overlaid on
+
+                if (params.redistortPoints) {
+                    // Distort the points so they match the image they're being overlaid on
+                    distortPoints(tempMat, tempMat);
+                }
+
                 var bottomPoints = tempMat.toList();
 
                 Calib3d.projectPoints(
@@ -104,6 +109,11 @@ public class Draw3dTargetsPipe
                         params.cameraCalibrationCoefficients.getDistCoeffsMat(),
                         tempMat,
                         jac);
+
+                if (params.redistortPoints) {
+                    // Distort the points so they match the image they're being overlaid on
+                    distortPoints(tempMat, tempMat);
+                }
                 var topPoints = tempMat.toList();
 
                 dividePointList(bottomPoints);
@@ -290,6 +300,8 @@ public class Draw3dTargetsPipe
         public final CameraCalibrationCoefficients cameraCalibrationCoefficients;
         public final FrameDivisor divisor;
 
+        public boolean redistortPoints = false;
+
         public Draw3dContoursParams(
                 boolean shouldDraw,
                 CameraCalibrationCoefficients cameraCalibrationCoefficients,

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(
@@ -118,7 +97,9 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
 
                 poseEstimatorPipe.setParams(
                         new AprilTagPoseEstimatorPipeParams(
-                                new Config(tagWidth, fx, fy, cx, cy), settings.numIterations));
+                                new Config(tagWidth, fx, fy, cx, cy),
+                                frameStaticProperties.cameraCalibration,
+                                settings.numIterations));
             }
         }
     }

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/ArucoPipeline.java

@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import java.util.ArrayList;
+import java.util.List;
+import org.opencv.core.Mat;
+import org.photonvision.vision.aruco.ArucoDetectionResult;
+import org.photonvision.vision.aruco.ArucoDetectorParams;
+import org.photonvision.vision.frame.Frame;
+import org.photonvision.vision.frame.FrameThresholdType;
+import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
+import org.photonvision.vision.pipe.impl.*;
+import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TrackedTarget;
+import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters;
+
+@SuppressWarnings("DuplicatedCode")
+public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSettings> {
+    private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
+    private final GrayscalePipe grayscalePipe = new GrayscalePipe();
+
+    private final ArucoDetectionPipe arucoDetectionPipe = new ArucoDetectionPipe();
+    private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
+
+    ArucoDetectorParams m_arucoDetectorParams = new ArucoDetectorParams();
+
+    public ArucoPipeline() {
+        super(FrameThresholdType.GREYSCALE);
+        settings = new ArucoPipelineSettings();
+    }
+
+    public ArucoPipeline(ArucoPipelineSettings settings) {
+        super(FrameThresholdType.GREYSCALE);
+        this.settings = settings;
+    }
+
+    @Override
+    protected void setPipeParamsImpl() {
+        // Sanitize thread count - not supported to have fewer than 1 threads
+        settings.threads = Math.max(1, settings.threads);
+
+        RotateImagePipe.RotateImageParams rotateImageParams =
+                new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
+        rotateImagePipe.setParams(rotateImageParams);
+
+        m_arucoDetectorParams.setDecimation((float) settings.decimate);
+        m_arucoDetectorParams.setCornerRefinementMaxIterations(settings.numIterations);
+        m_arucoDetectorParams.setCornerAccuracy(settings.cornerAccuracy);
+
+        arucoDetectionPipe.setParams(
+                new ArucoDetectionPipeParams(
+                        m_arucoDetectorParams.getDetector(), frameStaticProperties.cameraCalibration));
+    }
+
+    @Override
+    protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings) {
+        long sumPipeNanosElapsed = 0L;
+        Mat rawInputMat;
+        rawInputMat = frame.colorImage.getMat();
+
+        List<TrackedTarget> targetList;
+        CVPipeResult<List<ArucoDetectionResult>> tagDetectionPipeResult;
+
+        if (rawInputMat.empty()) {
+            return new CVPipelineResult(sumPipeNanosElapsed, 0, List.of(), frame);
+        }
+
+        tagDetectionPipeResult = arucoDetectionPipe.run(rawInputMat);
+        targetList = new ArrayList<>();
+        for (ArucoDetectionResult detection : tagDetectionPipeResult.output) {
+            // TODO this should be in a pipe, not in the top level here (Matt)
+
+            // populate the target list
+            // Challenge here is that TrackedTarget functions with OpenCV Contour
+            TrackedTarget target =
+                    new TrackedTarget(
+                            detection,
+                            new TargetCalculationParameters(
+                                    false, null, null, null, null, frameStaticProperties));
+
+            var correctedBestPose = target.getBestCameraToTarget3d();
+
+            target.setBestCameraToTarget3d(
+                    new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
+
+            targetList.add(target);
+        }
+
+        var fpsResult = calculateFPSPipe.run(null);
+        var fps = fpsResult.output;
+
+        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, frame);
+    }
+}

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

@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import com.fasterxml.jackson.annotation.JsonTypeName;
+import org.photonvision.vision.target.TargetModel;
+
+@JsonTypeName("ArucoPipelineSettings")
+public class ArucoPipelineSettings extends AdvancedPipelineSettings {
+    public double decimate = 1;
+    public int threads = 2;
+    public int numIterations = 100;
+    public double cornerAccuracy = 25.0;
+    public boolean useAruco3 = true;
+
+    // 3d settings
+
+    public ArucoPipelineSettings() {
+        super();
+        pipelineType = PipelineType.Aruco;
+        outputShowMultipleTargets = true;
+        targetModel = TargetModel.kAruco6in_16h5;
+        cameraExposure = -1;
+        cameraAutoExposure = true;
+        ledMode = false;
+    }
+}

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

@@ -31,7 +31,8 @@ import org.photonvision.vision.opencv.ImageRotationMode;
     @JsonSubTypes.Type(value = ColoredShapePipelineSettings.class),
     @JsonSubTypes.Type(value = ReflectivePipelineSettings.class),
     @JsonSubTypes.Type(value = DriverModePipelineSettings.class),
-    @JsonSubTypes.Type(value = AprilTagPipelineSettings.class)
+    @JsonSubTypes.Type(value = AprilTagPipelineSettings.class),
+    @JsonSubTypes.Type(value = ArucoPipelineSettings.class)
 })
 public class CVPipelineSettings implements Cloneable {
     public int pipelineIndex = 0;
@@ -40,14 +41,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/ColoredShapePipeline.java

@@ -223,7 +223,7 @@ public class ColoredShapePipeline
             collect2dTargetsResult.output.forEach(
                     shape -> {
                         shape.getMinAreaRect().points(rectPoints);
-                        shape.setCorners(Arrays.asList(rectPoints));
+                        shape.setTargetCorners(Arrays.asList(rectPoints));
                     });
             sumPipeNanosElapsed += cornerDetectionResult.nanosElapsed;
 

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

@@ -24,6 +24,7 @@ import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.DualOffsetValues;
 import org.photonvision.vision.pipe.impl.*;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TargetModel;
 import org.photonvision.vision.target.TrackedTarget;
 
 /**
@@ -37,6 +38,9 @@ public class OutputStreamPipeline {
     private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
     private final Draw2dAprilTagsPipe draw2dAprilTagsPipe = new Draw2dAprilTagsPipe();
     private final Draw3dAprilTagsPipe draw3dAprilTagsPipe = new Draw3dAprilTagsPipe();
+
+    private final Draw2dArucoPipe draw2dArucoPipe = new Draw2dArucoPipe();
+    private final Draw3dArucoPipe draw3dArucoPipe = new Draw3dArucoPipe();
     private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
     private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe();
 
@@ -65,6 +69,13 @@ public class OutputStreamPipeline {
                         settings.streamingFrameDivisor);
         draw2dAprilTagsPipe.setParams(draw2DAprilTagsParams);
 
+        var draw2DArucoParams =
+                new Draw2dArucoPipe.Draw2dArucoParams(
+                        settings.outputShouldDraw,
+                        settings.outputShowMultipleTargets,
+                        settings.streamingFrameDivisor);
+        draw2dArucoPipe.setParams(draw2DArucoParams);
+
         var draw2dCrosshairParams =
                 new Draw2dCrosshairPipe.Draw2dCrosshairParams(
                         settings.outputShouldDraw,
@@ -92,6 +103,14 @@ public class OutputStreamPipeline {
                         settings.streamingFrameDivisor);
         draw3dAprilTagsPipe.setParams(draw3dAprilTagsParams);
 
+        var draw3dArucoParams =
+                new Draw3dArucoPipe.Draw3dArucoParams(
+                        settings.outputShouldDraw,
+                        frameStaticProperties.cameraCalibration,
+                        TargetModel.k6in_16h5,
+                        settings.streamingFrameDivisor);
+        draw3dArucoPipe.setParams(draw3dArucoParams);
+
         resizeImagePipe.setParams(
                 new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor));
     }
@@ -110,65 +129,91 @@ 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(inMat, targetsToDraw));
+            sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
 
-                var drawOnInputResult = draw3dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+            if (!(settings instanceof AprilTagPipelineSettings)
+                    && !(settings instanceof ArucoPipelineSettings)) {
+                // If we're processing anything other than Apriltags..
+                var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
+                sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
 
-                pipeProfileNanos[8] = 0;
+                if (settings.solvePNPEnabled) {
+                    // Draw 3D Targets on input and output if possible
+                    pipeProfileNanos[5] = 0;
+                    pipeProfileNanos[6] = 0;
+                    pipeProfileNanos[7] = 0;
 
-            } else {
-                // Draw 2d apriltag markers
-                var draw2dTargetsOnInput = draw2dAprilTagsPipe.run(Pair.of(outMat, targetsToDraw));
-                sumPipeNanosElapsed += pipeProfileNanos[5] = draw2dTargetsOnInput.nanosElapsed;
+                    var drawOnOutputResult = draw3dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[8] = drawOnOutputResult.nanosElapsed;
+                } else {
+                    // Only draw 2d targets
+                    pipeProfileNanos[5] = 0;
 
-                pipeProfileNanos[6] = 0;
-                pipeProfileNanos[7] = 0;
-                pipeProfileNanos[8] = 0;
+                    var draw2dTargetsOnOutput = draw2dTargetsPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[6] = draw2dTargetsOnOutput.nanosElapsed;
+
+                    pipeProfileNanos[7] = 0;
+                    pipeProfileNanos[8] = 0;
+                }
+
+            } else if (settings instanceof AprilTagPipelineSettings) {
+                // 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;
+
+                    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;
+                }
+            } else if (settings instanceof ArucoPipelineSettings) {
+                if (settings.solvePNPEnabled) {
+                    // Draw 3d Apriltag markers (camera is calibrated and running in 3d mode)
+                    pipeProfileNanos[5] = 0;
+                    pipeProfileNanos[6] = 0;
+
+                    var drawOnInputResult = draw3dArucoPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+
+                    pipeProfileNanos[8] = 0;
+
+                } else {
+                    // Draw 2d apriltag markers
+                    var draw2dTargetsOnInput = draw2dArucoPipe.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/PipelineType.java

@@ -23,7 +23,8 @@ public enum PipelineType {
     DriverMode(-1, DriverModePipeline.class),
     Reflective(0, ReflectivePipeline.class),
     ColoredShape(1, ColoredShapePipeline.class),
-    AprilTag(2, AprilTagPipeline.class);
+    AprilTag(2, AprilTagPipeline.class),
+    Aruco(3, ArucoPipeline.class);
 
     public final int baseIndex;
     public final Class clazz;

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

@@ -21,7 +21,11 @@ import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Comparator;
 import java.util.List;
+import org.opencv.aruco.Aruco;
 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 +52,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 +145,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 +155,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 +174,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:
@@ -217,11 +206,21 @@ public class PipelineManager {
                     currentUserPipeline =
                             new AprilTagPipeline((AprilTagPipelineSettings) desiredPipelineSettings);
                     break;
+
+                case Aruco:
+                    logger.debug("Creating Aruco Pipeline");
+                    currentUserPipeline = new ArucoPipeline((ArucoPipelineSettings) desiredPipelineSettings);
+                    break;
                 default:
                     // Can be calib3d or drivermode, both of which are special cases
                     break;
             }
         }
+
+        DataChangeService.getInstance()
+                .publishEvent(
+                        new OutgoingUIEvent<>(
+                                "fullsettings", ConfigManager.getInstance().getConfig().toHashMap()));
     }
 
     /**
@@ -233,7 +232,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 +243,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);
     }
 
     /**
@@ -307,6 +306,12 @@ public class PipelineManager {
                     added.pipelineNickname = nickname;
                     return added;
                 }
+            case Aruco:
+                {
+                    var added = new ArucoPipelineSettings();
+                    added.pipelineNickname = nickname;
+                    return added;
+                }
             default:
                 {
                     logger.error("Got invalid pipeline type: " + type.toString());

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/main/java/org/photonvision/vision/target/TargetModel.java

@@ -109,6 +109,14 @@ public enum TargetModel implements Releasable {
                     new Point3(Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0),
                     new Point3(-Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0)),
             Units.inchesToMeters(3.25 * 2)),
+    kAruco6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners
+            // do not
+            List.of(
+                    new Point3(Units.inchesToMeters(3), Units.inchesToMeters(3), 0),
+                    new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0),
+                    new Point3(-Units.inchesToMeters(3), -Units.inchesToMeters(3), 0),
+                    new Point3(Units.inchesToMeters(3), -Units.inchesToMeters(3), 0)),
+            Units.inchesToMeters(3 * 2)),
     k6in_16h5( // Nominal edge length of 200 mm includes the white border, but solvePNP corners
             // do not
             List.of(

photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java

@@ -18,7 +18,10 @@ package org.photonvision.vision.target;
 
 import edu.wpi.first.apriltag.AprilTagDetection;
 import edu.wpi.first.apriltag.AprilTagPoseEstimate;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import java.util.HashMap;
 import java.util.List;
 import org.opencv.core.CvType;
@@ -28,6 +31,7 @@ import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Point;
 import org.opencv.core.RotatedRect;
 import org.photonvision.common.util.math.MathUtils;
+import org.photonvision.vision.aruco.ArucoDetectionResult;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.*;
 
@@ -37,7 +41,7 @@ public class TrackedTarget implements Releasable {
 
     private MatOfPoint2f m_approximateBoundingPolygon;
 
-    private List<Point> m_targetCorners;
+    private List<Point> m_targetCorners = List.of();
 
     private Point m_targetOffsetPoint;
     private Point m_robotOffsetPoint;
@@ -135,6 +139,59 @@ public class TrackedTarget implements Releasable {
         setCameraRelativeRvec(rvec);
     }
 
+    public TrackedTarget(ArucoDetectionResult result, TargetCalculationParameters params) {
+        m_targetOffsetPoint = new Point(result.getCenterX(), result.getCenterY());
+        m_robotOffsetPoint = new Point();
+
+        m_pitch =
+                TargetCalculations.calculatePitch(
+                        result.getCenterY(), params.cameraCenterPoint.y, params.verticalFocalLength);
+        m_yaw =
+                TargetCalculations.calculateYaw(
+                        result.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength);
+
+        double[] xCorners = result.getxCorners();
+        double[] yCorners = result.getyCorners();
+
+        Point[] cornerPoints =
+                new Point[] {
+                    new Point(xCorners[0], yCorners[0]),
+                    new Point(xCorners[1], yCorners[1]),
+                    new Point(xCorners[2], yCorners[2]),
+                    new Point(xCorners[3], yCorners[3])
+                };
+        m_targetCorners = List.of(cornerPoints);
+        MatOfPoint contourMat = new MatOfPoint(cornerPoints);
+        m_approximateBoundingPolygon = new MatOfPoint2f(cornerPoints);
+        m_mainContour = new Contour(contourMat);
+        m_area = m_mainContour.getArea() / params.imageArea * 100;
+        m_fiducialId = result.getId();
+        m_shape = null;
+
+        // TODO implement skew? or just yeet
+
+        var tvec = new Mat(3, 1, CvType.CV_64FC1);
+        tvec.put(0, 0, result.getTvec());
+        setCameraRelativeTvec(tvec);
+
+        var rvec = new Mat(3, 1, CvType.CV_64FC1);
+        rvec.put(0, 0, result.getRvec());
+        setCameraRelativeRvec(rvec);
+
+        {
+            Translation3d translation =
+                    // new Translation3d(tVec.get(0, 0)[0], tVec.get(1, 0)[0], tVec.get(2, 0)[0]);
+                    new Translation3d(result.getTvec()[0], result.getTvec()[1], result.getTvec()[2]);
+            var axisangle =
+                    VecBuilder.fill(result.getRvec()[0], result.getRvec()[1], result.getRvec()[2]);
+            Rotation3d rotation = new Rotation3d(axisangle, axisangle.normF());
+            Transform3d targetPose =
+                    MathUtils.convertOpenCVtoPhotonTransform(new Transform3d(translation, rotation));
+
+            m_bestCameraToTarget3d = targetPose;
+        }
+    }
+
     public void setFiducialId(int id) {
         m_fiducialId = id;
     }
@@ -232,7 +289,7 @@ public class TrackedTarget implements Releasable {
         if (m_cameraRelativeRvec != null) m_cameraRelativeRvec.release();
     }
 
-    public void setCorners(List<Point> targetCorners) {
+    public void setTargetCorners(List<Point> targetCorners) {
         this.m_targetCorners = targetCorners;
     }
 

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

@@ -18,12 +18,12 @@
 package org.photonvision.vision.pipeline;
 
 import edu.wpi.first.math.geometry.Translation3d;
-import java.io.IOException;
 import java.util.stream.Collectors;
 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;
@@ -32,7 +32,7 @@ import org.photonvision.vision.target.TrackedTarget;
 
 public class AprilTagTest {
     @BeforeEach
-    public void Init() throws IOException {
+    public void Init() {
         TestUtils.loadLibraries();
     }
 
@@ -46,6 +46,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(
@@ -65,22 +66,21 @@ public class AprilTagTest {
 
         // Draw on input
         var outputPipe = new OutputStreamPipeline();
-        outputPipe.process(
-                pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
+        var ret =
+                outputPipe.process(
+                        pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
 
-        TestUtils.showImage(
-                pipelineResult.inputAndOutputFrame.colorImage.getMat(), "Pipeline output", 999999);
+        TestUtils.showImage(ret.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
 
         // these numbers are not *accurate*, but they are known and expected
         var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
         Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2);
         Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2);
-        Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2);
+        Assertions.assertEquals(0.0, pose.getTranslation().getZ(), 0.2);
 
         var objX = new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getY();
         var objY = new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getZ();
         var objZ = new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getX();
-        System.out.printf("Object x %.2f y %.2f z %.2f\n", objX, objY, objZ);
 
         // We expect the object X to be forward, or -X in world space
         Assertions.assertEquals(
@@ -92,6 +92,49 @@ public class AprilTagTest {
         Assertions.assertEquals(1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.1);
     }
 
+    @Test
+    public void testApriltagDistorted() {
+        var pipeline = new AprilTagPipeline();
+
+        pipeline.getSettings().inputShouldShow = true;
+        pipeline.getSettings().outputShouldDraw = true;
+        pipeline.getSettings().solvePNPEnabled = true;
+        pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
+        pipeline.getSettings().cornerDetectionUseConvexHulls = true;
+        pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
+        pipeline.getSettings().tagFamily = AprilTagFamily.kTag16h5;
+
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag_corner_1280, false),
+                        TestUtils.WPI2020Image.FOV,
+                        TestUtils.getCoeffs(TestUtils.LIMELIGHT_480P_CAL_FILE, false));
+        frameProvider.requestFrameThresholdType(pipeline.getThresholdType());
+
+        CVPipelineResult pipelineResult;
+        try {
+            pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
+            printTestResults(pipelineResult);
+        } catch (RuntimeException e) {
+            // For now, will throw coz rotation3d ctor
+            return;
+        }
+
+        // Draw on input
+        var outputPipe = new OutputStreamPipeline();
+        var ret =
+                outputPipe.process(
+                        pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
+
+        TestUtils.showImage(ret.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
+
+        // these numbers are not *accurate*, but they are known and expected
+        var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
+        Assertions.assertEquals(4, pose.getTranslation().getX(), 0.2);
+        Assertions.assertEquals(2, pose.getTranslation().getY(), 0.2);
+        Assertions.assertEquals(0.0, pose.getTranslation().getZ(), 0.2);
+    }
+
     private static void printTestResults(CVPipelineResult pipelineResult) {
         double fps = 1000 / pipelineResult.getLatencyMillis();
         System.out.println(

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

@@ -0,0 +1,86 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline;
+
+import java.io.IOException;
+import java.util.stream.Collectors;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.photonvision.common.util.TestUtils;
+import org.photonvision.vision.camera.QuirkyCamera;
+import org.photonvision.vision.frame.provider.FileFrameProvider;
+import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.target.TargetModel;
+import org.photonvision.vision.target.TrackedTarget;
+
+public class ArucoPipelineTest {
+    @BeforeEach
+    public void Init() throws IOException {
+        TestUtils.loadLibraries();
+    }
+
+    @Test
+    public void testApriltagFacingCameraAruco() {
+        var pipeline = new ArucoPipeline();
+
+        pipeline.getSettings().inputShouldShow = true;
+        pipeline.getSettings().outputShouldDraw = true;
+        pipeline.getSettings().solvePNPEnabled = true;
+        pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
+        pipeline.getSettings().cornerDetectionUseConvexHulls = true;
+        pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
+
+        // pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11;
+
+        var frameProvider =
+                new FileFrameProvider(
+                        TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag1_16h5_1280, false),
+                        106,
+                        TestUtils.getCoeffs("laptop_1280.json", false));
+        frameProvider.requestFrameThresholdType(pipeline.getThresholdType());
+
+        CVPipelineResult pipelineResult;
+        try {
+            pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
+            printTestResults(pipelineResult);
+        } catch (RuntimeException e) {
+            // For now, will throw coz rotation3d ctor
+            return;
+        }
+
+        // Draw on input
+        var outputPipe = new OutputStreamPipeline();
+        outputPipe.process(
+                pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
+
+        TestUtils.showImage(
+                pipelineResult.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
+    }
+
+    private static void printTestResults(CVPipelineResult pipelineResult) {
+        double fps = 1000 / pipelineResult.getLatencyMillis();
+        System.out.println(
+                "Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " " + "fps)");
+        System.out.println("Found " + pipelineResult.targets.size() + " valid targets");
+        System.out.println(
+                "Found targets at "
+                        + pipelineResult.targets.stream()
+                                .map(TrackedTarget::getBestCameraToTarget3d)
+                                .collect(Collectors.toList()));
+    }
+}

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/generate/photonlib.json.in

@@ -22,7 +22,7 @@
         "windowsx86-64",
         "linuxathena",
         "linuxx86-64",
-        "osxx86-64"
+        "osxuniversal"
       ]
     }
   ],

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

@@ -31,18 +31,23 @@ import edu.wpi.first.networktables.DoubleArrayPublisher;
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.IntegerEntry;
 import edu.wpi.first.networktables.IntegerSubscriber;
+import edu.wpi.first.networktables.MultiSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.PubSubOption;
 import edu.wpi.first.networktables.RawSubscriber;
 import edu.wpi.first.networktables.StringSubscriber;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import java.util.Set;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.hardware.VisionLEDMode;
 import org.photonvision.targeting.PhotonPipelineResult;
 
 /** Represents a camera that is connected to PhotonVision. */
 public class PhotonCamera {
+    static final String kTableName = "photonvision";
+
     protected final NetworkTable rootTable;
     RawSubscriber rawBytesEntry;
     BooleanEntry driverModeEntry;
@@ -56,7 +61,7 @@ public class PhotonCamera {
     DoubleArrayPublisher targetPoseEntry;
     DoublePublisher targetSkewEntry;
     StringSubscriber versionEntry;
-    BooleanPublisher inputSaveImgEntry, outputSaveImgEntry;
+    IntegerEntry inputSaveImgEntry, outputSaveImgEntry;
     IntegerEntry pipelineIndexEntry, ledModeEntry;
     IntegerSubscriber heartbeatEntry;
 
@@ -96,6 +101,8 @@ public class PhotonCamera {
 
     Packet packet = new Packet(1);
 
+    private final MultiSubscriber m_topicNameSubscriber;
+
     /**
      * Constructs a PhotonCamera from a root table.
      *
@@ -106,17 +113,27 @@ public class PhotonCamera {
      */
     public PhotonCamera(NetworkTableInstance instance, String cameraName) {
         name = cameraName;
-        var mainTable = instance.getTable("photonvision");
+        var mainTable = instance.getTable(kTableName);
         this.rootTable = mainTable.getSubTable(cameraName);
         path = rootTable.getPath();
-        rawBytesEntry = rootTable.getRawTopic("rawBytes").subscribe("rawBytes", new byte[] {});
+        rawBytesEntry =
+                rootTable
+                        .getRawTopic("rawBytes")
+                        .subscribe(
+                                "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
         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);
         versionEntry = mainTable.getStringTopic("version").subscribe("");
+
+        m_topicNameSubscriber =
+                new MultiSubscriber(
+                        instance,
+                        new String[] {"/photonvision/"},
+                        new PubSubOption[] {PubSubOption.topicsOnly(true)});
     }
 
     /**
@@ -181,7 +198,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 +208,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 +297,7 @@ public class PhotonCamera {
             prevHeartbeatValue = curHeartbeat;
         }
 
-        return ((now - prevHeartbeatChangeTime) > HEARBEAT_DEBOUNCE_SEC);
+        return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC;
     }
 
     private void verifyVersion() {
@@ -292,12 +309,25 @@ public class PhotonCamera {
         // Heartbeat entry is assumed to always be present. If it's not present, we
         // assume that a camera with that name was never connected in the first place.
         if (!heartbeatEntry.exists()) {
-            DriverStation.reportError(
-                    "PhotonVision coprocessor at path " + path + " not found on NetworkTables!", true);
+            Set<String> cameraNames = rootTable.getInstance().getTable(kTableName).getSubTables();
+            if (cameraNames.isEmpty()) {
+                DriverStation.reportError(
+                        "Could not find any PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!",
+                        false);
+            } else {
+                DriverStation.reportError(
+                        "PhotonVision coprocessor at path "
+                                + path
+                                + " not found on NetworkTables. Double check that your camera names match!",
+                        true);
+                DriverStation.reportError(
+                        "Found the following PhotonVision cameras on NetworkTables:\n"
+                                + String.join("\n", cameraNames),
+                        false);
+            }
         }
-
         // Check for connection status. Warn if disconnected.
-        if (!isConnected()) {
+        else if (!isConnected()) {
             DriverStation.reportWarning(
                     "PhotonVision coprocessor at path " + path + " is not sending new data.", true);
         }

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,29 +62,27 @@ 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.
      *
-     * <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 WPILib {@link 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.
+     *     the center of the robot to the camera mount positions (ie, robot -> camera).
      */
     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 +93,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 +153,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 +202,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 +227,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 +281,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 +314,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 +344,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 +377,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 +394,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/SimPhotonCamera.java

@@ -36,6 +36,8 @@ import org.photonvision.targeting.PhotonTrackedTarget;
 public class SimPhotonCamera {
     NTTopicSet ts = new NTTopicSet();
     PhotonPipelineResult latestResult;
+    private long heartbeatCounter = 0;
+
     /**
      * Constructs a Simulated PhotonCamera from a root table.
      *
@@ -46,7 +48,7 @@ public class SimPhotonCamera {
      */
     public SimPhotonCamera(NetworkTableInstance instance, String cameraName) {
         ts.removeEntries();
-        ts.subTable = instance.getTable("/photonvision").getSubTable(cameraName);
+        ts.subTable = instance.getTable(PhotonCamera.kTableName).getSubTable(cameraName);
         ts.updateEntries();
     }
 
@@ -134,6 +136,8 @@ public class SimPhotonCamera {
             ts.targetPoseEntry.set(poseData);
         }
 
+        ts.heartbeatPublisher.set(heartbeatCounter++);
+
         latestResult = newResult;
     }
 

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

@@ -24,10 +24,11 @@
 
 package org.photonvision;
 
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
 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;
@@ -50,7 +51,7 @@ public class SimVisionSystem {
     int cameraResWidth;
     int cameraResHeight;
     double minTargetArea;
-    Transform3d cameraToRobot;
+    Transform3d robotToCamera;
 
     Field2d dbgField;
     FieldObject2d dbgRobot;
@@ -68,7 +69,8 @@ public class SimVisionSystem {
      * @param camDiagFOVDegrees Diagonal Field of View of the camera used. Align it with the
      *     manufacturer specifications, and/or whatever is configured in the PhotonVision Setting
      *     page.
-     * @param cameraToRobot Transform to move from the camera's mount position to the robot's position
+     * @param robotToCamera Transform to move from the center of the robot to the camera's mount
+     *     position
      * @param maxLEDRangeMeters Maximum distance at which your camera can illuminate the target and
      *     make it visible. Set to 9000 or more if your vision system does not rely on LED's.
      * @param cameraResWidth Width of your camera's image sensor in pixels
@@ -80,12 +82,12 @@ public class SimVisionSystem {
     public SimVisionSystem(
             String camName,
             double camDiagFOVDegrees,
-            Transform3d cameraToRobot,
+            Transform3d robotToCamera,
             double maxLEDRangeMeters,
             int cameraResWidth,
             int cameraResHeight,
             double minTargetArea) {
-        this.cameraToRobot = cameraToRobot;
+        this.robotToCamera = robotToCamera;
         this.maxLEDRangeMeters = maxLEDRangeMeters;
         this.cameraResWidth = cameraResWidth;
         this.cameraResHeight = cameraResHeight;
@@ -120,14 +122,31 @@ public class SimVisionSystem {
         ;
     }
 
+    /**
+     * Adds all apriltags from the provided {@link AprilTagFieldLayout} as sim vision targets. The
+     * poses added will preserve the tag layout's alliance origin at the time of calling this method.
+     *
+     * @param tagLayout The field tag layout to get Apriltag poses and IDs from
+     */
+    public void addVisionTargets(AprilTagFieldLayout tagLayout) {
+        for (AprilTag tag : tagLayout.getTags()) {
+            addSimVisionTarget(
+                    new SimVisionTarget(
+                            tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation
+                            Units.inchesToMeters(6),
+                            Units.inchesToMeters(6),
+                            tag.ID));
+        }
+    }
+
     /**
      * Adjust the camera position relative to the robot. Use this if your camera is on a gimbal or
      * turret or some other mobile platform.
      *
-     * @param newCameraToRobot New Transform from the robot to the camera
+     * @param newRobotToCamera New Transform from the robot to the camera
      */
-    public void moveCamera(Transform3d newCameraToRobot) {
-        this.cameraToRobot = newCameraToRobot;
+    public void moveCamera(Transform3d newRobotToCamera) {
+        this.robotToCamera = newRobotToCamera;
     }
 
     /**
@@ -139,13 +158,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));
     }
 
     /**
@@ -157,7 +170,7 @@ public class SimVisionSystem {
      *     PhotonVision parameters.
      */
     public void processFrame(Pose3d robotPoseMeters) {
-        Pose3d cameraPose = robotPoseMeters.transformBy(cameraToRobot.inverse());
+        Pose3d cameraPose = robotPoseMeters.transformBy(robotToCamera);
 
         dbgRobot.setPose(robotPoseMeters.toPose2d());
         dbgCamera.setPose(cameraPose.toPose2d());
@@ -218,6 +231,9 @@ public class SimVisionSystem {
                                         camToTargetTrans,
                                         camToTargetTransAlt,
                                         0.0, // TODO - simulate ambiguity when straight on?
+                                        List.of(
+                                                new TargetCorner(0, 0), new TargetCorner(0, 0),
+                                                new TargetCorner(0, 0), new TargetCorner(0, 0)),
                                         List.of(
                                                 new TargetCorner(0, 0), new TargetCorner(0, 0),
                                                 new TargetCorner(0, 0), new TargetCorner(0, 0))));

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

@@ -33,17 +33,24 @@
 namespace photonlib {
 
 constexpr const units::second_t VERSION_CHECK_INTERVAL = 5_s;
+static const std::vector<std::string_view> PHOTON_PREFIX = {"/photonvision/"};
 
-PhotonCamera::PhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
+PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
                            const std::string_view cameraName)
-    : mainTable(instance->GetTable("photonvision")),
+    : mainTable(instance.GetTable("photonvision")),
       rootTable(mainTable->GetSubTable(cameraName)),
-      rawBytesEntry(rootTable->GetRawTopic("rawBytes").Subscribe("raw", {})),
+      rawBytesEntry(
+          rootTable->GetRawTopic("rawBytes")
+              .Subscribe("raw", {}, {.periodic = 0.01, .sendAll = true})),
       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("")),
@@ -52,13 +59,12 @@ PhotonCamera::PhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
       pipelineIndexSubscriber(
           rootTable->GetIntegerTopic("pipelineIndex").Subscribe(-1)),
       ledModeSubscriber(mainTable->GetIntegerTopic("ledMode").Subscribe(0)),
+      m_topicNameSubscriber(instance, PHOTON_PREFIX, {.topicsOnly = true}),
       path(rootTable->GetPath()),
       m_cameraName(cameraName) {}
 
 PhotonCamera::PhotonCamera(const std::string_view cameraName)
-    : PhotonCamera(std::make_shared<nt::NetworkTableInstance>(
-                       nt::NetworkTableInstance::GetDefault()),
-                   cameraName) {}
+    : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}
 
 PhotonPipelineResult PhotonCamera::GetLatestResult() {
   if (test) return testResult;
@@ -89,9 +95,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(); }
 
@@ -126,10 +136,29 @@ void PhotonCamera::VerifyVersion() {
   const std::string& versionString = versionEntry.Get("");
   if (versionString.empty()) {
     std::string path_ = path;
-    FRC_ReportError(
-        frc::warn::Warning,
-        "PhotonVision coprocessor at path {} not found on NetworkTables!",
-        path_);
+    std::vector<std::string> cameraNames =
+        rootTable->GetInstance().GetTable("photonvision")->GetSubTables();
+    if (cameraNames.empty()) {
+      FRC_ReportError(frc::warn::Warning,
+                      "Could not find any PhotonVision coprocessors on "
+                      "NetworkTables. Double check that PhotonVision is "
+                      "running, and that your camera is connected!");
+    } else {
+      FRC_ReportError(
+          frc::warn::Warning,
+          "PhotonVision coprocessor at path {} not found on NetworkTables. "
+          "Double check that your camera names match!",
+          path_);
+
+      std::string cameraNameOutString;
+      for (unsigned int i = 0; i < cameraNames.size(); i++) {
+        cameraNameOutString += "\n" + cameraNames[i];
+      }
+      FRC_ReportError(
+          frc::warn::Warning,
+          "Found the following PhotonVision cameras on NetworkTables:{}",
+          cameraNameOutString);
+    }
   } else if (!VersionMatches(versionString)) {
     FRC_ReportError(frc::warn::Warning,
                     "Photon version {} does not match coprocessor version {}!",

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

@@ -30,13 +30,15 @@
 #include <frc/geometry/Translation2d.h>
 #include <wpi/SmallVector.h>
 
+static constexpr const uint8_t MAX_CORNERS = 8;
+
 namespace photonlib {
 
 PhotonTrackedTarget::PhotonTrackedTarget(
     double yaw, double pitch, double area, double skew, int id,
     const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
     double ambiguity,
-    const wpi::SmallVector<std::pair<double, double>, 4> corners)
+    const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners)
     : yaw(yaw),
       pitch(pitch),
       area(area),
@@ -45,12 +47,12 @@ PhotonTrackedTarget::PhotonTrackedTarget(
       bestCameraToTarget(pose),
       altCameraToTarget(alternatePose),
       poseAmbiguity(ambiguity),
-      corners(corners) {}
+      minAreaRectCorners(minAreaRectCorners) {}
 
 bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
   return other.yaw == yaw && other.pitch == pitch && other.area == area &&
          other.skew == skew && other.bestCameraToTarget == bestCameraToTarget &&
-         other.corners == corners;
+         other.minAreaRectCorners == minAreaRectCorners;
 }
 
 bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
@@ -77,7 +79,16 @@ Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
          << target.poseAmbiguity;
 
   for (int i = 0; i < 4; i++) {
-    packet << target.corners[i].first << target.corners[i].second;
+    packet << target.minAreaRectCorners[i].first
+           << target.minAreaRectCorners[i].second;
+  }
+
+  uint8_t num_corners =
+      std::min<uint8_t>(target.detectedCorners.size(), MAX_CORNERS);
+  packet << num_corners;
+  for (size_t i = 0; i < target.detectedCorners.size(); i++) {
+    packet << target.detectedCorners[i].first
+           << target.detectedCorners[i].second;
   }
 
   return packet;
@@ -111,12 +122,21 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
 
   packet >> target.poseAmbiguity;
 
-  target.corners.clear();
+  target.minAreaRectCorners.clear();
+  double first = 0;
+  double second = 0;
   for (int i = 0; i < 4; i++) {
-    double first = 0;
-    double second = 0;
     packet >> first >> second;
-    target.corners.emplace_back(first, second);
+    target.minAreaRectCorners.emplace_back(first, second);
+  }
+
+  uint8_t numCorners = 0;
+  packet >> numCorners;
+  target.detectedCorners.clear();
+  target.detectedCorners.reserve(numCorners);
+  for (size_t i = 0; i < numCorners; i++) {
+    packet >> first >> second;
+    target.detectedCorners.emplace_back(first, second);
   }
 
   return packet;

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

@@ -30,6 +30,7 @@
 #include <networktables/BooleanTopic.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/IntegerTopic.h>
+#include <networktables/MultiSubscriber.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
 #include <networktables/RawTopic.h>
@@ -57,7 +58,7 @@ class PhotonCamera {
    * @param cameraName The name of the camera, as seen in the UI.
    * over.
    */
-  explicit PhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
+  explicit PhotonCamera(nt::NetworkTableInstance instance,
                         const std::string_view cameraName);
 
   /**
@@ -166,8 +167,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;
@@ -176,6 +179,8 @@ class PhotonCamera {
   nt::IntegerSubscriber pipelineIndexSubscriber;
   nt::IntegerSubscriber ledModeSubscriber;
 
+  nt::MultiSubscriber m_topicNameSubscriber;
+
   std::string path;
   std::string m_cameraName;
 

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

@@ -92,10 +92,27 @@ class PhotonTrackedTarget {
   int GetFiducialId() const { return fiducialId; }
 
   /**
-   * Returns the corners of the minimum area rectangle bounding this target.
+   * Return a list of the 4 corners in image space (origin top left, x right, y
+   * down), in no particular order, of the minimum area bounding rectangle of
+   * this target
    */
-  wpi::SmallVector<std::pair<double, double>, 4> GetCorners() const {
-    return corners;
+  wpi::SmallVector<std::pair<double, double>, 4> GetMinAreaRectCorners() const {
+    return minAreaRectCorners;
+  }
+
+  /**
+   * Return a list of the n corners in image space (origin top left, x right, y
+   * down), in no particular order, detected for this target.
+   * For fiducials, the order is known and is always counter-clock wise around
+   * the tag, like so
+   *
+   * -> +X     3 ----- 2
+   * |         |       |
+   * V + Y     |       |
+   *           0 ----- 1
+   */
+  std::vector<std::pair<double, double>> GetDetectedCorners() {
+    return detectedCorners;
   }
 
   /**
@@ -137,6 +154,7 @@ class PhotonTrackedTarget {
   frc::Transform3d bestCameraToTarget;
   frc::Transform3d altCameraToTarget;
   double poseAmbiguity;
-  wpi::SmallVector<std::pair<double, double>, 4> corners;
+  wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners;
+  std::vector<std::pair<double, double>> detectedCorners;
 };
 }  // namespace photonlib

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,33 @@ 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>);
+  /**
+   * Create a new RobotPoseEstimator.
+   *
+   * @param aprilTags A WPILib {@link 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 camera mount positions
+   * (ie, robot -> camera).
+   */
+  explicit RobotPoseEstimator(
+      std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
+      PoseStrategy strategy, std::vector<map_value_type> cameras);
 
+  /**
+   * Update the estimated pose using the selected strategy.
+   *
+   * @return The updated estimated pose and the latency in milliseconds.
+   */
   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 +98,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/main/native/include/photonlib/SimPhotonCamera.h

@@ -37,7 +37,7 @@
 namespace photonlib {
 class SimPhotonCamera : public PhotonCamera {
  public:
-  SimPhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
+  SimPhotonCamera(nt::NetworkTableInstance instance,
                   const std::string& cameraName)
       : PhotonCamera(instance, cameraName) {
     latencyMillisEntry = rootTable->GetEntry("latencyMillis");
@@ -48,14 +48,12 @@ class SimPhotonCamera : public PhotonCamera {
     targetSkewEntry = rootTable->GetEntry("targetSkewEntry");
     targetPoseEntry = rootTable->GetEntry("targetPoseEntry");
     rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("raw");
-    versionEntry = instance->GetTable("photonvision")->GetEntry("version");
+    versionEntry = instance.GetTable("photonvision")->GetEntry("version");
     // versionEntry.SetString(PhotonVersion.versionString);
   }
 
   explicit SimPhotonCamera(const std::string& cameraName)
-      : SimPhotonCamera(std::make_shared<nt::NetworkTableInstance>(
-                            nt::NetworkTableInstance::GetDefault()),
-                        cameraName) {}
+      : SimPhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}
 
   virtual ~SimPhotonCamera() = default;
 

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

@@ -47,6 +47,11 @@ class PacketTest {
                         new Transform3d(new Translation3d(), new Rotation3d()),
                         new Transform3d(new Translation3d(), new Rotation3d()),
                         0.25,
+                        List.of(
+                                new TargetCorner(1, 2),
+                                new TargetCorner(3, 4),
+                                new TargetCorner(5, 6),
+                                new TargetCorner(7, 8)),
                         List.of(
                                 new TargetCorner(1, 2),
                                 new TargetCorner(3, 4),
@@ -85,6 +90,11 @@ class PacketTest {
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
                                         0.25,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -99,6 +109,11 @@ class PacketTest {
                                         new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
                                         new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
                                         0.25,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),

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();
@@ -87,6 +94,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
                                         0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -101,6 +113,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
                                         new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
                                         0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -120,6 +137,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
                                         new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
                                         0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -132,10 +154,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 +165,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();
@@ -163,6 +181,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
                                         new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
                                         0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -177,6 +200,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
                                         new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
                                         0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -196,6 +224,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(4, 4, 4), new Rotation3d()),
                                         new Transform3d(new Translation3d(5, 5, 5), new Rotation3d()),
                                         0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -208,21 +241,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();
@@ -239,6 +268,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
                                         new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
                                         0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -253,6 +287,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
                                         new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
                                         0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -272,6 +311,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
                                         new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
                                         0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -285,10 +329,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 +340,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();
@@ -316,6 +356,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
                                         new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
                                         0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -330,6 +375,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
                                         new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
                                         0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -349,6 +399,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
                                         new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
                                         0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -363,8 +418,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(
@@ -379,6 +434,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
                                         new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
                                         0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -393,6 +453,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2.1, 1.9, 2), new Rotation3d()),
                                         new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
                                         0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -411,6 +476,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2.4, 2.4, 2.2), new Rotation3d()),
                                         new Transform3d(new Translation3d(2, 1, 2), new Rotation3d()),
                                         0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -418,9 +488,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 +498,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();
@@ -448,6 +514,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
                                         new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
                                         0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -462,6 +533,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
                                         new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
                                         0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -481,6 +557,11 @@ class RobotPoseEstimatorTest {
                                         new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
                                         new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
                                         0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)),
                                         List.of(
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
@@ -493,9 +574,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/java/org/photonvision/SimVisionSystemTest.java

@@ -182,8 +182,7 @@ class SimVisionSystemTest {
                 new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI));
         var robotToCamera =
                 new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, Math.PI / 4, 0));
-        var sysUnderTest =
-                new SimVisionSystem("Test", 80.0, robotToCamera.inverse(), 99999, 1234, 1234, 0);
+        var sysUnderTest = new SimVisionSystem("Test", 80.0, robotToCamera, 99999, 1234, 1234, 0);
         sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 3.0, 0.5, 1736));
 
         var robotPose = new Pose2d(new Translation2d(14.98, 0), Rotation2d.fromDegrees(5));
@@ -247,14 +246,15 @@ class SimVisionSystemTest {
     @ParameterizedTest
     @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999})
     public void testCameraPitch(double testPitch) {
+        testPitch = testPitch * -1;
+
         final var targetPose =
                 new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, 3 * Math.PI / 4));
         final var robotPose = new Pose2d(new Translation2d(10, 0), new Rotation2d(0));
         var sysUnderTest = new SimVisionSystem("Test", 120.0, new Transform3d(), 99999, 640, 480, 0);
         sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 23));
 
-        // Here, passing in a positive testPitch points the camera downward (since moveCamera takes the
-        // camera->robot transform)
+        // Transform is now robot -> camera
         sysUnderTest.moveCamera(
                 new Transform3d(
                         new Translation3d(), new Rotation3d(0, Units.degreesToRadians(testPitch), 0)));
@@ -263,10 +263,11 @@ class SimVisionSystemTest {
         assertTrue(res.hasTargets());
         var tgt = res.getBestTarget();
 
-        // Since the camera is level with the target, a downward point will mean the target is in the
-        // upper half of the image
-        // which should produce positive pitch.
-        assertEquals(testPitch, tgt.getPitch(), 0.0001);
+        // Since the camera is level with the target, a positive-upward point will mean the target is in
+        // the
+        // lower half of the image
+        // which should produce negative pitch.
+        assertEquals(testPitch * -1, tgt.getPitch(), 0.0001);
     }
 
     private static Stream<Arguments> distCalCParamProvider() {
@@ -309,7 +310,7 @@ class SimVisionSystemTest {
                 new SimVisionSystem(
                         "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!",
                         160.0,
-                        robotToCamera.inverse(),
+                        robotToCamera,
                         99999,
                         640,
                         480,

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);

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

@@ -26,6 +26,7 @@ import edu.wpi.first.networktables.IntegerPublisher;
 import edu.wpi.first.networktables.IntegerSubscriber;
 import edu.wpi.first.networktables.IntegerTopic;
 import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.PubSubOption;
 import edu.wpi.first.networktables.RawPublisher;
 
 /**
@@ -65,7 +66,10 @@ public class NTTopicSet {
     public IntegerPublisher heartbeatPublisher;
 
     public void updateEntries() {
-        rawBytesEntry = subTable.getRawTopic("rawBytes").publish("rawBytes");
+        rawBytesEntry =
+                subTable
+                        .getRawTopic("rawBytes")
+                        .publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
 
         pipelineIndexTopic = subTable.getIntegerTopic("pipelineIndex");
         pipelineIndexPublisher = pipelineIndexTopic.publish();

photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java

@@ -23,11 +23,12 @@ import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import java.util.ArrayList;
 import java.util.List;
-import java.util.Objects;
 import org.photonvision.common.dataflow.structures.Packet;
 
 public class PhotonTrackedTarget {
-    public static final int PACK_SIZE_BYTES = Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7);
+    private static final int MAX_CORNERS = 8;
+    public static final int PACK_SIZE_BYTES =
+            Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS);
 
     private double yaw;
     private double pitch;
@@ -37,7 +38,12 @@ public class PhotonTrackedTarget {
     private Transform3d bestCameraToTarget = new Transform3d();
     private Transform3d altCameraToTarget = new Transform3d();
     private double poseAmbiguity;
-    private List<TargetCorner> targetCorners;
+
+    // Corners from the min-area rectangle bounding the target
+    private List<TargetCorner> minAreaRectCorners;
+
+    // Corners from whatever corner detection method was used
+    private List<TargetCorner> detectedCorners;
 
     public PhotonTrackedTarget() {}
 
@@ -51,8 +57,14 @@ public class PhotonTrackedTarget {
             Transform3d pose,
             Transform3d altPose,
             double ambiguity,
-            List<TargetCorner> corners) {
-        assert corners.size() == 4;
+            List<TargetCorner> minAreaRectCorners,
+            List<TargetCorner> detectedCorners) {
+        assert minAreaRectCorners.size() == 4;
+
+        if (detectedCorners.size() > MAX_CORNERS) {
+            detectedCorners = detectedCorners.subList(0, MAX_CORNERS);
+        }
+
         this.yaw = yaw;
         this.pitch = pitch;
         this.area = area;
@@ -60,7 +72,8 @@ public class PhotonTrackedTarget {
         this.fiducialId = id;
         this.bestCameraToTarget = pose;
         this.altCameraToTarget = altPose;
-        this.targetCorners = corners;
+        this.minAreaRectCorners = minAreaRectCorners;
+        this.detectedCorners = detectedCorners;
         this.poseAmbiguity = ambiguity;
     }
 
@@ -94,11 +107,28 @@ public class PhotonTrackedTarget {
     }
 
     /**
-     * Return a list of the 4 corners in image space (origin top left, x left, y down), in no
+     * Return a list of the 4 corners in image space (origin top left, x right, y down), in no
      * particular order, of the minimum area bounding rectangle of this target
      */
-    public List<TargetCorner> getCorners() {
-        return targetCorners;
+    public List<TargetCorner> getMinAreaRectCorners() {
+        return minAreaRectCorners;
+    }
+
+    /**
+     * Return a list of the n corners in image space (origin top left, x right, y down), in no
+     * particular order, detected for this target.
+     *
+     * <p>For fiducials, the order is known and is always counter-clock wise around the tag, like so
+     *
+     * <p>spotless:off
+     * -> +X  3 ----- 2
+     * |      |       |
+     * V      |       |
+     * +Y     0 ----- 1
+     * spotless:on
+     */
+    public List<TargetCorner> getDetectedCorners() {
+        return detectedCorners;
     }
 
     /**
@@ -118,21 +148,54 @@ public class PhotonTrackedTarget {
     }
 
     @Override
-    public boolean equals(Object o) {
-        if (this == o) return true;
-        if (o == null || getClass() != o.getClass()) return false;
-        PhotonTrackedTarget that = (PhotonTrackedTarget) o;
-        return Double.compare(that.yaw, yaw) == 0
-                && Double.compare(that.pitch, pitch) == 0
-                && Double.compare(that.area, area) == 0
-                && Objects.equals(bestCameraToTarget, that.bestCameraToTarget)
-                && Objects.equals(altCameraToTarget, that.altCameraToTarget)
-                && Objects.equals(targetCorners, that.targetCorners);
+    public int hashCode() {
+        final int prime = 31;
+        int result = 1;
+        long temp;
+        temp = Double.doubleToLongBits(yaw);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(pitch);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(area);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(skew);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        result = prime * result + fiducialId;
+        result = prime * result + ((bestCameraToTarget == null) ? 0 : bestCameraToTarget.hashCode());
+        result = prime * result + ((altCameraToTarget == null) ? 0 : altCameraToTarget.hashCode());
+        temp = Double.doubleToLongBits(poseAmbiguity);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        result = prime * result + ((minAreaRectCorners == null) ? 0 : minAreaRectCorners.hashCode());
+        result = prime * result + ((detectedCorners == null) ? 0 : detectedCorners.hashCode());
+        return result;
     }
 
     @Override
-    public int hashCode() {
-        return Objects.hash(yaw, pitch, area, bestCameraToTarget, altCameraToTarget);
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (obj == null) return false;
+        if (getClass() != obj.getClass()) return false;
+        PhotonTrackedTarget other = (PhotonTrackedTarget) obj;
+        if (Double.doubleToLongBits(yaw) != Double.doubleToLongBits(other.yaw)) return false;
+        if (Double.doubleToLongBits(pitch) != Double.doubleToLongBits(other.pitch)) return false;
+        if (Double.doubleToLongBits(area) != Double.doubleToLongBits(other.area)) return false;
+        if (Double.doubleToLongBits(skew) != Double.doubleToLongBits(other.skew)) return false;
+        if (fiducialId != other.fiducialId) return false;
+        if (bestCameraToTarget == null) {
+            if (other.bestCameraToTarget != null) return false;
+        } else if (!bestCameraToTarget.equals(other.bestCameraToTarget)) return false;
+        if (altCameraToTarget == null) {
+            if (other.altCameraToTarget != null) return false;
+        } else if (!altCameraToTarget.equals(other.altCameraToTarget)) return false;
+        if (Double.doubleToLongBits(poseAmbiguity) != Double.doubleToLongBits(other.poseAmbiguity))
+            return false;
+        if (minAreaRectCorners == null) {
+            if (other.minAreaRectCorners != null) return false;
+        } else if (!minAreaRectCorners.equals(other.minAreaRectCorners)) return false;
+        if (detectedCorners == null) {
+            if (other.detectedCorners != null) return false;
+        } else if (!detectedCorners.equals(other.detectedCorners)) return false;
+        return true;
     }
 
     private static Transform3d decodeTransform(Packet packet) {
@@ -158,6 +221,25 @@ public class PhotonTrackedTarget {
         packet.encode(transform.getRotation().getQuaternion().getZ());
     }
 
+    private static void encodeList(Packet packet, List<TargetCorner> list) {
+        packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE));
+        for (int i = 0; i < list.size(); i++) {
+            packet.encode(list.get(i).x);
+            packet.encode(list.get(i).y);
+        }
+    }
+
+    private static List<TargetCorner> decodeList(Packet p) {
+        byte len = p.decodeByte();
+        var ret = new ArrayList<TargetCorner>();
+        for (int i = 0; i < len; i++) {
+            double cx = p.decodeDouble();
+            double cy = p.decodeDouble();
+            ret.add(new TargetCorner(cx, cy));
+        }
+        return ret;
+    }
+
     /**
      * Populates the fields of this class with information from the incoming packet.
      *
@@ -176,13 +258,15 @@ public class PhotonTrackedTarget {
 
         this.poseAmbiguity = packet.decodeDouble();
 
-        this.targetCorners = new ArrayList<>(4);
+        this.minAreaRectCorners = new ArrayList<>(4);
         for (int i = 0; i < 4; i++) {
             double cx = packet.decodeDouble();
             double cy = packet.decodeDouble();
-            targetCorners.add(new TargetCorner(cx, cy));
+            minAreaRectCorners.add(new TargetCorner(cx, cy));
         }
 
+        detectedCorners = decodeList(packet);
+
         return packet;
     }
 
@@ -203,10 +287,12 @@ public class PhotonTrackedTarget {
         packet.encode(poseAmbiguity);
 
         for (int i = 0; i < 4; i++) {
-            packet.encode(targetCorners.get(i).x);
-            packet.encode(targetCorners.get(i).y);
+            packet.encode(minAreaRectCorners.get(i).x);
+            packet.encode(minAreaRectCorners.get(i).y);
         }
 
+        encodeList(packet, detectedCorners);
+
         return packet;
     }
 
@@ -226,7 +312,7 @@ public class PhotonTrackedTarget {
                 + ", cameraToTarget="
                 + bestCameraToTarget
                 + ", targetCorners="
-                + targetCorners
+                + minAreaRectCorners
                 + '}';
     }
 }

photonlib-cpp-examples/aimandrange/.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": true,
     "currentLanguage": "cpp",
-    "projectYear": "2023Beta",
+    "projectYear": "2023",
     "teamNumber": 5
 }

photonlib-cpp-examples/aimandrange/build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1"
 
     id "com.dorongold.task-tree" version "2.1.0"
 }

photonlib-cpp-examples/aimattarget/.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": true,
     "currentLanguage": "cpp",
-    "projectYear": "2023Beta",
+    "projectYear": "2023",
     "teamNumber": 5
 }

photonlib-cpp-examples/aimattarget/build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1"
 
     id "com.dorongold.task-tree" version "2.1.0"
 }

photonlib-cpp-examples/getinrange/.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": true,
     "currentLanguage": "cpp",
-    "projectYear": "2023Beta",
+    "projectYear": "2023",
     "teamNumber": 5
 }

photonlib-cpp-examples/getinrange/build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1"
 
     id "com.dorongold.task-tree" version "2.1.0"
 }

photonlib-java-examples/aimandrange/.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": false,
     "currentLanguage": "java",
-    "projectYear": "2023Beta",
+    "projectYear": "2023",
     "teamNumber": 6995
 }

photonlib-java-examples/aimandrange/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

photonlib-java-examples/aimattarget/.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": false,
     "currentLanguage": "java",
-    "projectYear": "2023Beta",
+    "projectYear": "2023",
     "teamNumber": 6995
 }

photonlib-java-examples/aimattarget/build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

photonlib-java-examples/apriltagExample/.gitignore

@@ -0,0 +1,162 @@
+# This gitignore has been specially created by the WPILib team.
+# If you remove items from this file, intellisense might break.
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### Java ###
+# Compiled class file
+*.class
+
+# Log file
+*.log
+
+# BlueJ files
+*.ctxt
+
+# Mobile Tools for Java (J2ME)
+.mtj.tmp/
+
+# Package Files #
+*.jar
+*.war
+*.nar
+*.ear
+*.zip
+*.tar.gz
+*.rar
+
+# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
+hs_err_pid*
+
+### Linux ###
+*~
+
+# temporary files which can be created if a process still has a handle open of a deleted file
+.fuse_hidden*
+
+# KDE directory preferences
+.directory
+
+# Linux trash folder which might appear on any partition or disk
+.Trash-*
+
+# .nfs files are created when an open file is removed but is still being accessed
+.nfs*
+
+### macOS ###
+# General
+.DS_Store
+.AppleDouble
+.LSOverride
+
+# Icon must end with two \r
+Icon
+
+# Thumbnails
+._*
+
+# Files that might appear in the root of a volume
+.DocumentRevisions-V100
+.fseventsd
+.Spotlight-V100
+.TemporaryItems
+.Trashes
+.VolumeIcon.icns
+.com.apple.timemachine.donotpresent
+
+# Directories potentially created on remote AFP share
+.AppleDB
+.AppleDesktop
+Network Trash Folder
+Temporary Items
+.apdisk
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+
+### Windows ###
+# Windows thumbnail cache files
+Thumbs.db
+ehthumbs.db
+ehthumbs_vista.db
+
+# Dump file
+*.stackdump
+
+# Folder config file
+[Dd]esktop.ini
+
+# Recycle Bin used on file shares
+$RECYCLE.BIN/
+
+# Windows Installer files
+*.cab
+*.msi
+*.msix
+*.msm
+*.msp
+
+# Windows shortcuts
+*.lnk
+
+### Gradle ###
+.gradle
+/build/
+
+# Ignore Gradle GUI config
+gradle-app.setting
+
+# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored)
+!gradle-wrapper.jar
+
+# Cache of project
+.gradletasknamecache
+
+# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
+# gradle/wrapper/gradle-wrapper.properties
+
+# # VS Code Specific Java Settings
+# DO NOT REMOVE .classpath and .project
+.classpath
+.project
+.settings/
+bin/
+
+# Simulation GUI and other tools window save file
+*-window.json

photonlib-java-examples/apriltagExample/.wpilib/wpilib_preferences.json

@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": false,
+    "currentLanguage": "java",
+    "projectYear": "2023Beta",
+    "teamNumber": 1736
+}

photonlib-java-examples/apriltagExample/WPILib-License.md

@@ -0,0 +1,24 @@
+Copyright (c) 2009-2021 FIRST and other WPILib contributors
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

photonlib-java-examples/apriltagExample/build.gradle

@@ -0,0 +1,94 @@
+plugins {
+    id "java"
+    id "edu.wpi.first.GradleRIO" version "2023.1.1-beta-7"
+}
+
+sourceCompatibility = JavaVersion.VERSION_11
+targetCompatibility = JavaVersion.VERSION_11
+
+apply from: "${rootDir}/../shared/examples_common.gradle"
+
+def ROBOT_MAIN_CLASS = "frc.robot.Main"
+
+// Define my targets (RoboRIO) and artifacts (deployable files)
+// This is added by GradleRIO's backing project DeployUtils.
+deploy {
+    targets {
+        roborio(getTargetTypeClass('RoboRIO')) {
+            // Team number is loaded either from the .wpilib/wpilib_preferences.json
+            // or from command line. If not found an exception will be thrown.
+            // You can use getTeamOrDefault(team) instead of getTeamNumber if you
+            // want to store a team number in this file.
+            team = project.frc.getTeamOrDefault(5940)
+            debug = project.frc.getDebugOrDefault(false)
+
+            artifacts {
+                // First part is artifact name, 2nd is artifact type
+                // getTargetTypeClass is a shortcut to get the class type using a string
+
+                frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+                }
+
+                // Static files artifact
+                frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
+                    files = project.fileTree('src/main/deploy')
+                    directory = '/home/lvuser/deploy'
+                }
+            }
+        }
+    }
+}
+
+def deployArtifact = deploy.targets.roborio.artifacts.frcJava
+
+// Set to true to use debug for JNI.
+wpi.java.debugJni = false
+
+// Set this to true to enable desktop support.
+def includeDesktopSupport = true
+
+// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
+// Also defines JUnit 4.
+dependencies {
+    implementation wpi.java.deps.wpilib()
+    implementation wpi.java.vendor.java()
+
+    roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
+    roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
+
+    roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio)
+    roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio)
+
+    nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop)
+    nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop)
+    simulationDebug wpi.sim.enableDebug()
+
+    nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
+    nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
+    simulationRelease wpi.sim.enableRelease()
+
+    testImplementation 'junit:junit:4.13.1'
+}
+
+// Simulation configuration (e.g. environment variables).
+wpi.sim.addGui().defaultEnabled = true
+wpi.sim.addDriverstation()
+
+// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
+// in order to make them all available at runtime. Also adding the manifest so WPILib
+// knows where to look for our Robot Class.
+jar {
+    from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+    manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
+    duplicatesStrategy = DuplicatesStrategy.INCLUDE
+}
+
+// Configure jar and deploy tasks
+deployArtifact.jarTask = jar
+wpi.java.configureExecutableTasks(jar)
+wpi.java.configureTestTasks(test)
+
+// Configure string concat to always inline compile
+tasks.withType(JavaCompile) {
+    options.compilerArgs.add '-XDstringConcat=inline'
+}

photonlib-java-examples/apriltagExample/gradle/wrapper/gradle-wrapper.properties

@@ -0,0 +1,5 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=permwrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=permwrapper/dists

photonlib-java-examples/apriltagExample/gradlew

@@ -0,0 +1,240 @@
+#!/bin/sh
+
+#
+# Copyright © 2015-2021 the original authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+##############################################################################
+#
+#   Gradle start up script for POSIX generated by Gradle.
+#
+#   Important for running:
+#
+#   (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
+#       noncompliant, but you have some other compliant shell such as ksh or
+#       bash, then to run this script, type that shell name before the whole
+#       command line, like:
+#
+#           ksh Gradle
+#
+#       Busybox and similar reduced shells will NOT work, because this script
+#       requires all of these POSIX shell features:
+#         * functions;
+#         * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
+#           «${var#prefix}», «${var%suffix}», and «$( cmd )»;
+#         * compound commands having a testable exit status, especially «case»;
+#         * various built-in commands including «command», «set», and «ulimit».
+#
+#   Important for patching:
+#
+#   (2) This script targets any POSIX shell, so it avoids extensions provided
+#       by Bash, Ksh, etc; in particular arrays are avoided.
+#
+#       The "traditional" practice of packing multiple parameters into a
+#       space-separated string is a well documented source of bugs and security
+#       problems, so this is (mostly) avoided, by progressively accumulating
+#       options in "$@", and eventually passing that to Java.
+#
+#       Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
+#       and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
+#       see the in-line comments for details.
+#
+#       There are tweaks for specific operating systems such as AIX, CygWin,
+#       Darwin, MinGW, and NonStop.
+#
+#   (3) This script is generated from the Groovy template
+#       https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       within the Gradle project.
+#
+#       You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+app_path=$0
+
+# Need this for daisy-chained symlinks.
+while
+    APP_HOME=${app_path%"${app_path##*/}"}  # leaves a trailing /; empty if no leading path
+    [ -h "$app_path" ]
+do
+    ls=$( ls -ld "$app_path" )
+    link=${ls#*' -> '}
+    case $link in             #(
+      /*)   app_path=$link ;; #(
+      *)    app_path=$APP_HOME$link ;;
+    esac
+done
+
+APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit
+
+APP_NAME="Gradle"
+APP_BASE_NAME=${0##*/}
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+warn () {
+    echo "$*"
+} >&2
+
+die () {
+    echo
+    echo "$*"
+    echo
+    exit 1
+} >&2
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "$( uname )" in                #(
+  CYGWIN* )         cygwin=true  ;; #(
+  Darwin* )         darwin=true  ;; #(
+  MSYS* | MINGW* )  msys=true    ;; #(
+  NONSTOP* )        nonstop=true ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+    if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+        # IBM's JDK on AIX uses strange locations for the executables
+        JAVACMD=$JAVA_HOME/jre/sh/java
+    else
+        JAVACMD=$JAVA_HOME/bin/java
+    fi
+    if [ ! -x "$JAVACMD" ] ; then
+        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+else
+    JAVACMD=java
+    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+    case $MAX_FD in #(
+      max*)
+        MAX_FD=$( ulimit -H -n ) ||
+            warn "Could not query maximum file descriptor limit"
+    esac
+    case $MAX_FD in  #(
+      '' | soft) :;; #(
+      *)
+        ulimit -n "$MAX_FD" ||
+            warn "Could not set maximum file descriptor limit to $MAX_FD"
+    esac
+fi
+
+# Collect all arguments for the java command, stacking in reverse order:
+#   * args from the command line
+#   * the main class name
+#   * -classpath
+#   * -D...appname settings
+#   * --module-path (only if needed)
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
+
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if "$cygwin" || "$msys" ; then
+    APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
+    CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
+
+    JAVACMD=$( cygpath --unix "$JAVACMD" )
+
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    for arg do
+        if
+            case $arg in                                #(
+              -*)   false ;;                            # don't mess with options #(
+              /?*)  t=${arg#/} t=/${t%%/*}              # looks like a POSIX filepath
+                    [ -e "$t" ] ;;                      #(
+              *)    false ;;
+            esac
+        then
+            arg=$( cygpath --path --ignore --mixed "$arg" )
+        fi
+        # Roll the args list around exactly as many times as the number of
+        # args, so each arg winds up back in the position where it started, but
+        # possibly modified.
+        #
+        # NB: a `for` loop captures its iteration list before it begins, so
+        # changing the positional parameters here affects neither the number of
+        # iterations, nor the values presented in `arg`.
+        shift                   # remove old arg
+        set -- "$@" "$arg"      # push replacement arg
+    done
+fi
+
+# Collect all arguments for the java command;
+#   * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
+#     shell script including quotes and variable substitutions, so put them in
+#     double quotes to make sure that they get re-expanded; and
+#   * put everything else in single quotes, so that it's not re-expanded.
+
+set -- \
+        "-Dorg.gradle.appname=$APP_BASE_NAME" \
+        -classpath "$CLASSPATH" \
+        org.gradle.wrapper.GradleWrapperMain \
+        "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+    die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+#   readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+#   set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+        printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+        xargs -n1 |
+        sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+        tr '\n' ' '
+    )" '"$@"'
+
+exec "$JAVACMD" "$@"