photonlibpy: Fix some type check failures (#1548)

This fixes a variety of type check failures raised by both mypy and pyright. See #1548

.github/workflows/build.yml

@@ -100,11 +100,11 @@ jobs:
       - name: Gradle Coverage
         run: ./gradlew jacocoTestReport
       - name: Publish Coverage Report
-        uses: codecov/codecov-action@v3
+        uses: codecov/codecov-action@v4
         with:
           file: ./photon-server/build/reports/jacoco/test/jacocoTestReport.xml
       - name: Publish Core Coverage Report
-        uses: codecov/codecov-action@v3
+        uses: codecov/codecov-action@v4
         with:
           file: ./photon-core/build/reports/jacoco/test/jacocoTestReport.xml
   build-offline-docs:
@@ -133,6 +133,37 @@ jobs:
         with:
           name: built-docs
           path: docs/build/html
+
+  build-photonlib-vendorjson:
+    name: "Build Vendor JSON"
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+
+      - name: Install Java 17
+        uses: actions/setup-java@v4
+        with:
+          java-version: 17
+          distribution: temurin
+
+      # grab all tags
+      - run: git fetch --tags --force
+
+      # Generate the JSON and give it the ""standard""" name maven gives it
+      - run: |
+          chmod +x gradlew
+          ./gradlew photon-lib:generateVendorJson
+          export VERSION=$(git describe --tags --match=v*)
+          mv photon-lib/build/generated/vendordeps/photonlib.json photon-lib/build/generated/vendordeps/photonlib-$(git describe --tags --match=v*).json
+
+      # Upload it here so it shows up in releases
+      - uses: actions/upload-artifact@v4
+        with:
+          name: photonlib-vendor-json
+          path: photon-lib/build/generated/vendordeps/photonlib-*.json
+
   build-photonlib-host:
     env:
       MACOSX_DEPLOYMENT_TARGET: 13
@@ -165,6 +196,7 @@ jobs:
       - run: |
           chmod +x gradlew
           ./gradlew photon-targeting:build photon-lib:build -i
+        name: Build with Gradle
       - run: ./gradlew photon-lib:publish photon-targeting:publish
         name: Publish
         env:
@@ -506,6 +538,11 @@ jobs:
         with:
           merge-multiple: true
           pattern: photonlib-offline
+      # Download vendor json
+      - uses: actions/download-artifact@v4
+        with:
+          merge-multiple: true
+          pattern: photonlib-vendor-json
       # Download all images
       - uses: actions/download-artifact@v4
         with:
@@ -528,14 +565,14 @@ jobs:
       # Upload all jars and xz archives
       # Split into two uploads to work around max size limits in action-gh-releases
       # https://github.com/softprops/action-gh-release/issues/353
-      - uses: softprops/action-gh-release@v2.0.8
+      - uses: softprops/action-gh-release@v2.0.9
         with:
           files: |
             **/*orangepi5*.xz
         if: startsWith(github.ref, 'refs/tags/v')
         env:
           GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
-      - uses: softprops/action-gh-release@v2.0.8
+      - uses: softprops/action-gh-release@v2.0.9
         with:
           files: |
             **/!(*orangepi5*).xz
@@ -545,3 +582,18 @@ jobs:
         if: startsWith(github.ref, 'refs/tags/v')
         env:
           GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+
+  dispatch:
+    name: dispatch
+    needs: [build-photonlib-vendorjson]
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: peter-evans/repository-dispatch@v3
+        if: |
+          github.repository == 'PhotonVision/photonvision' &&
+          startsWith(github.ref, 'refs/tags/v')
+        with:
+          token: ${{ secrets.VENDOR_JSON_REPO_PUSH_TOKEN }}
+          repository: PhotonVision/vendor-json-repo
+          event-type: tag
+          client-payload: '{"run_id": "${{ github.run_id }}", "package_version": "${{ github.ref_name }}"}'

.github/workflows/lint-format.yml

@@ -26,7 +26,7 @@ jobs:
     name: "wpiformat"
     runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: Fetch all history and metadata
         run: |
           git fetch --prune --unshallow
@@ -37,7 +37,7 @@ jobs:
         with:
           python-version: 3.11
       - name: Install wpiformat
-        run: pip3 install wpiformat==2024.41
+        run: pip3 install wpiformat==2024.45
       - name: Run
         run: wpiformat
       - name: Check output
@@ -45,7 +45,7 @@ jobs:
       - name: Generate diff
         run: git diff HEAD > wpiformat-fixes.patch
         if: ${{ failure() }}
-      - uses: actions/upload-artifact@v3
+      - uses: actions/upload-artifact@v4
         with:
           name: wpiformat fixes
           path: wpiformat-fixes.patch
@@ -54,7 +54,7 @@ jobs:
     name: "Java Formatting"
     runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
         with:
           fetch-depth: 0
       - uses: actions/setup-java@v4
@@ -64,6 +64,7 @@ jobs:
       - run: |
           chmod +x gradlew
           ./gradlew spotlessCheck
+        name: Run spotless
 
   client-lint-format:
     name: "PhotonClient Lint and Formatting"
@@ -72,9 +73,9 @@ jobs:
         working-directory: photon-client
     runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: Setup Node.js
-        uses: actions/setup-node@v3
+        uses: actions/setup-node@v4
         with:
           node-version: 18
       - name: Install Dependencies
@@ -87,7 +88,7 @@ jobs:
     name: "Check server index.html not changed"
     runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: Fetch all history and metadata
         run: |
           git fetch --prune --unshallow

.github/workflows/photonvision-docs.yml

@@ -17,7 +17,7 @@ jobs:
     runs-on: ubuntu-22.04
 
     steps:
-    - uses: actions/checkout@v3
+    - uses: actions/checkout@v4
 
     - uses: actions/setup-python@v4
       with:

devTools/calibrationUtils.py

@@ -1,11 +1,12 @@
 import argparse
 import base64
-from dataclasses import dataclass
 import json
 import os
+from dataclasses import dataclass
+
 import cv2
-import numpy as np
 import mrcal
+import numpy as np
 from wpimath.geometry import Quaternion as _Quat
 
 

docs/source/docs/contributing/building-photon.md

@@ -284,3 +284,11 @@ Then, run the examples:
 > cd photonlib-python-examples
 > run.bat <example name>
 ```
+
+#### Downloading Pipeline Artifacts
+
+Using the [GitHub CLI](https://cli.github.com/), we can download artifacts from pipelines by run ID and name:
+
+```
+~/photonvision$ gh run download 11759699679 -n jar-Linux
+```

docs/source/docs/contributing/design-descriptions/time-sync.md

@@ -101,7 +101,7 @@ The message format forgoes CRCs (as these are provided by the Ethernet physical
 Clients may publish statistics to NetworkTables. If they do, they shall publish to a key that is globally unique per participant in the Time Synronization network. If a client implements this, it shall provide the following publishers:
 
 | Key | Type | Notes |
-| ------ | ------ | ---- | ----- |
+| ------ | ------ | ---- |
 | offset_us | Integer | The time offset that, when added to the client's local clock, provides server time |
 | ping_tx_count | Integer | The total number of TSP Ping packets transmitted |
 | ping_rx_count | Integer | The total number of TSP Ping packets received |

photon-core/build.gradle

@@ -4,6 +4,7 @@ plugins {
 
 import java.nio.file.Path
 
+ext.licenseFile = file("$rootDir/LICENSE")
 apply from: "${rootDir}/shared/common.gradle"
 
 wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get()

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

@@ -151,7 +151,11 @@ public class PhotonConfiguration {
         generalSubmap.put("availableModels", NeuralNetworkModelManager.getInstance().getModels());
         generalSubmap.put(
                 "supportedBackends", NeuralNetworkModelManager.getInstance().getSupportedBackends());
-        generalSubmap.put("hardwareModel", hardwareConfig.deviceName);
+        generalSubmap.put(
+                "hardwareModel",
+                hardwareConfig.deviceName.isEmpty()
+                        ? Platform.getHardwareModel()
+                        : hardwareConfig.deviceName);
         generalSubmap.put("hardwarePlatform", Platform.getPlatformName());
         settingsSubmap.put("general", generalSubmap);
         // AprilTagFieldLayout

photon-core/src/main/java/org/photonvision/common/logging/KernelLogLogger.java

@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.logging;
+
+import edu.wpi.first.util.RuntimeDetector;
+import org.photonvision.common.util.TimedTaskManager;
+import org.photonvision.jni.QueuedFileLogger;
+
+/**
+ * Listens for and reproduces Linux kernel logs, from /var/log/kern.log, into the Photon logger
+ * ecosystem
+ */
+public class KernelLogLogger {
+    private static KernelLogLogger INSTANCE;
+
+    public static KernelLogLogger getInstance() {
+        if (INSTANCE == null) {
+            INSTANCE = new KernelLogLogger();
+        }
+        return INSTANCE;
+    }
+
+    QueuedFileLogger listener = null;
+    Logger logger = new Logger(KernelLogLogger.class, LogGroup.General);
+
+    public KernelLogLogger() {
+        if (RuntimeDetector.isLinux()) {
+            listener = new QueuedFileLogger("/var/log/kern.log");
+        } else {
+            System.out.println("NOT for klogs");
+        }
+
+        // arbitrary frequency to grab logs. The underlying native buffer will grow unbounded without
+        // this, lol
+        TimedTaskManager.getInstance().addTask("outputPrintk", this::outputNewPrintks, 1000);
+    }
+
+    public void outputNewPrintks() {
+        if (listener == null) {
+            return;
+        }
+
+        for (var msg : listener.getNewlines()) {
+            // We currently set all logs to debug regardless of their actual level
+            logger.log(msg, LogLevel.DEBUG);
+        }
+    }
+}

photon-core/src/main/java/org/photonvision/common/logging/LogGroup.java

@@ -26,4 +26,5 @@ public enum LogGroup {
     Config,
     CSCore,
     NetworkTables,
+    System,
 }

photon-core/src/main/java/org/photonvision/common/logging/Logger.java

@@ -30,8 +30,34 @@ import org.photonvision.common.dataflow.DataChangeService;
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.util.TimedTaskManager;
 
-@SuppressWarnings("unused")
+/** TODO: get rid of static {} blocks and refactor to singleton pattern */
 public class Logger {
+    private static final HashMap<LogGroup, LogLevel> levelMap = new HashMap<>();
+    private static final List<LogAppender> currentAppenders = new ArrayList<>();
+
+    private static final UILogAppender uiLogAppender = new UILogAppender();
+
+    // // TODO why's the logger care about this? split it out
+    // private static KernelLogLogger klogListener = null;
+
+    static {
+        levelMap.put(LogGroup.Camera, LogLevel.INFO);
+        levelMap.put(LogGroup.General, LogLevel.INFO);
+        levelMap.put(LogGroup.WebServer, LogLevel.INFO);
+        levelMap.put(LogGroup.Data, LogLevel.INFO);
+        levelMap.put(LogGroup.VisionModule, LogLevel.INFO);
+        levelMap.put(LogGroup.Config, LogLevel.INFO);
+        levelMap.put(LogGroup.CSCore, LogLevel.TRACE);
+        levelMap.put(LogGroup.NetworkTables, LogLevel.DEBUG);
+        levelMap.put(LogGroup.System, LogLevel.DEBUG);
+
+        currentAppenders.add(new ConsoleLogAppender());
+        currentAppenders.add(uiLogAppender);
+        addFileAppender(PathManager.getInstance().getLogPath());
+
+        cleanLogs(PathManager.getInstance().getLogsDir());
+    }
+
     public static final String ANSI_RESET = "\u001B[0m";
     public static final String ANSI_BLACK = "\u001B[30m";
     public static final String ANSI_RED = "\u001B[31m";
@@ -50,8 +76,6 @@ public class Logger {
     private static final List<Pair<String, LogLevel>> uiBacklog = new ArrayList<>();
     private static boolean connected = false;
 
-    private static final UILogAppender uiLogAppender = new UILogAppender();
-
     private final String className;
     private final LogGroup group;
 
@@ -89,27 +113,6 @@ public class Logger {
         return builder.toString();
     }
 
-    private static final HashMap<LogGroup, LogLevel> levelMap = new HashMap<>();
-    private static final List<LogAppender> currentAppenders = new ArrayList<>();
-
-    static {
-        levelMap.put(LogGroup.Camera, LogLevel.INFO);
-        levelMap.put(LogGroup.General, LogLevel.INFO);
-        levelMap.put(LogGroup.WebServer, LogLevel.INFO);
-        levelMap.put(LogGroup.Data, LogLevel.INFO);
-        levelMap.put(LogGroup.VisionModule, LogLevel.INFO);
-        levelMap.put(LogGroup.Config, LogLevel.INFO);
-        levelMap.put(LogGroup.CSCore, LogLevel.TRACE);
-        levelMap.put(LogGroup.NetworkTables, LogLevel.DEBUG);
-    }
-
-    static {
-        currentAppenders.add(new ConsoleLogAppender());
-        currentAppenders.add(uiLogAppender);
-        addFileAppender(PathManager.getInstance().getLogPath());
-        cleanLogs(PathManager.getInstance().getLogsDir());
-    }
-
     @SuppressWarnings("ResultOfMethodCallIgnored")
     public static void addFileAppender(Path logFilePath) {
         var file = logFilePath.toFile();

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

@@ -96,9 +96,12 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
         var autoExpProp = findProperty("exposure_auto", "auto_exposure");
 
         exposureAbsProp = expProp.get();
-        autoExposureProp = autoExpProp.get();
         this.minExposure = exposureAbsProp.getMin();
         this.maxExposure = exposureAbsProp.getMax();
+
+        if (autoExpProp.isPresent()) {
+            autoExposureProp = autoExpProp.get();
+        }
     }
 
     public void setAllCamDefaults() {
@@ -169,7 +172,7 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
             softSet("auto_exposure_bias", 0);
             softSet("iso_sensitivity_auto", 0); // Disable auto ISO adjustment
             softSet("iso_sensitivity", 0); // Manual ISO adjustment
-            autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
+            if (autoExposureProp != null) autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
 
             // Most cameras leave exposure time absolute at the last value from their AE
             // algorithm.
@@ -199,7 +202,7 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
     public void setExposureRaw(double exposureRaw) {
         if (exposureRaw >= 0.0) {
             try {
-                autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
+                if (autoExposureProp != null) autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
 
                 int propVal = (int) MathUtil.clamp(exposureRaw, minExposure, maxExposure);
 

photon-lib/build.gradle

@@ -9,6 +9,7 @@ ext {
     includePhotonTargeting = true
     // Include the generated Version file
     generatedHeaders = "src/generate/native/include"
+    licenseFile = file("LICENSE")
 }
 
 apply plugin: 'cpp'
@@ -22,25 +23,7 @@ apply from: "${rootDir}/versioningHelper.gradle"
 
 nativeUtils {
     exportsConfigs {
-        "${nativeName}" {
-            // From https://github.com/wpilibsuite/allwpilib/blob/a32589831184969939fd3d63f449a2788a0a8542/wpimath/build.gradle#L72
-            // Copyright (c) FIRST and other WPILib contributors.
-            // Open Source Software; you can modify and/or share it under the terms of
-            // the WPILib BSD license file in the root directory of this project.
-            x64ExcludeSymbols = [
-                '_CT??_R0?AV_System_error',
-                '_CT??_R0?AVexception',
-                '_CT??_R0?AVfailure',
-                '_CT??_R0?AVruntime_error',
-                '_CT??_R0?AVsystem_error',
-                '_CTA5?AVfailure',
-                '_TI5?AVfailure',
-                '_CT??_R0?AVout_of_range',
-                '_CTA3?AVout_of_range',
-                '_TI3?AVout_of_range',
-                '_CT??_R0?AVbad_cast'
-            ]
-        }
+        "${nativeName}" {}
     }
 }
 

photon-lib/py/photonlibpy/__init__.py

@@ -15,7 +15,7 @@
 ## along with this program.  If not, see <https://www.gnu.org/licenses/>.
 ###############################################################################
 
-from .packet import Packet  # noqa
 from .estimatedRobotPose import EstimatedRobotPose  # noqa
-from .photonPoseEstimator import PhotonPoseEstimator, PoseStrategy  # noqa
+from .packet import Packet  # noqa
 from .photonCamera import PhotonCamera  # noqa
+from .photonPoseEstimator import PhotonPoseEstimator, PoseStrategy  # noqa

photon-lib/py/photonlibpy/estimation/__init__.py

@@ -0,0 +1,5 @@
+from .cameraTargetRelation import CameraTargetRelation
+from .openCVHelp import OpenCVHelp
+from .rotTrlTransform3d import RotTrlTransform3d
+from .targetModel import TargetModel
+from .visionEstimation import VisionEstimation

photon-lib/py/photonlibpy/estimation/cameraTargetRelation.py

@@ -0,0 +1,25 @@
+import math
+
+from wpimath.geometry import Pose3d, Rotation2d, Transform3d
+from wpimath.units import meters
+
+
+class CameraTargetRelation:
+    def __init__(self, cameraPose: Pose3d, targetPose: Pose3d):
+        self.camPose = cameraPose
+        self.camToTarg = Transform3d(cameraPose, targetPose)
+        self.camToTargDist = self.camToTarg.translation().norm()
+        self.camToTargDistXY: meters = math.hypot(
+            self.camToTarg.translation().X(), self.camToTarg.translation().Y()
+        )
+        self.camToTargYaw = Rotation2d(self.camToTarg.X(), self.camToTarg.Y())
+        self.camToTargPitch = Rotation2d(self.camToTargDistXY, -self.camToTarg.Z())
+        self.camToTargAngle = Rotation2d(
+            math.hypot(self.camToTargYaw.radians(), self.camToTargPitch.radians())
+        )
+        self.targToCam = Transform3d(targetPose, cameraPose)
+        self.targToCamYaw = Rotation2d(self.targToCam.X(), self.targToCam.Y())
+        self.targToCamPitch = Rotation2d(self.camToTargDistXY, -self.targToCam.Z())
+        self.targtoCamAngle = Rotation2d(
+            math.hypot(self.targToCamYaw.radians(), self.targToCamPitch.radians())
+        )

photon-lib/py/photonlibpy/estimation/openCVHelp.py

@@ -0,0 +1,200 @@
+import math
+from typing import Any, Tuple
+
+import cv2 as cv
+import numpy as np
+from wpimath.geometry import Rotation3d, Transform3d, Translation3d
+
+from ..targeting import PnpResult, TargetCorner
+from .rotTrlTransform3d import RotTrlTransform3d
+
+NWU_TO_EDN = Rotation3d(np.array([[0, -1, 0], [0, 0, -1], [1, 0, 0]]))
+EDN_TO_NWU = Rotation3d(np.array([[0, 0, 1], [-1, 0, 0], [0, -1, 0]]))
+
+
+class OpenCVHelp:
+    @staticmethod
+    def getMinAreaRect(points: np.ndarray) -> cv.RotatedRect:
+        return cv.RotatedRect(*cv.minAreaRect(points))
+
+    @staticmethod
+    def translationNWUtoEDN(trl: Translation3d) -> Translation3d:
+        return trl.rotateBy(NWU_TO_EDN)
+
+    @staticmethod
+    def rotationNWUtoEDN(rot: Rotation3d) -> Rotation3d:
+        return -NWU_TO_EDN + (rot + NWU_TO_EDN)
+
+    @staticmethod
+    def translationToTVec(translations: list[Translation3d]) -> np.ndarray:
+        retVal: list[list] = []
+        for translation in translations:
+            trl = OpenCVHelp.translationNWUtoEDN(translation)
+            retVal.append([trl.X(), trl.Y(), trl.Z()])
+        return np.array(
+            retVal,
+            dtype=np.float32,
+        )
+
+    @staticmethod
+    def rotationToRVec(rotation: Rotation3d) -> np.ndarray:
+        retVal: list[np.ndarray] = []
+        rot = OpenCVHelp.rotationNWUtoEDN(rotation)
+        rotVec = rot.getQuaternion().toRotationVector()
+        retVal.append(rotVec)
+        return np.array(
+            retVal,
+            dtype=np.float32,
+        )
+
+    @staticmethod
+    def avgPoint(points: list[Tuple[float, float]]) -> Tuple[float, float]:
+        x = 0.0
+        y = 0.0
+        for p in points:
+            x += p[0]
+            y += p[1]
+        return (x / len(points), y / len(points))
+
+    @staticmethod
+    def pointsToTargetCorners(points: np.ndarray) -> list[TargetCorner]:
+        corners = [TargetCorner(p[0, 0], p[0, 1]) for p in points]
+        return corners
+
+    @staticmethod
+    def cornersToPoints(corners: list[TargetCorner]) -> np.ndarray:
+        points = [[[c.x, c.y]] for c in corners]
+        return np.array(points)
+
+    @staticmethod
+    def projectPoints(
+        cameraMatrix: np.ndarray,
+        distCoeffs: np.ndarray,
+        camRt: RotTrlTransform3d,
+        objectTranslations: list[Translation3d],
+    ) -> np.ndarray:
+        objectPoints = OpenCVHelp.translationToTVec(objectTranslations)
+        rvec = OpenCVHelp.rotationToRVec(camRt.getRotation())
+        tvec = OpenCVHelp.translationToTVec(
+            [
+                camRt.getTranslation(),
+            ]
+        )
+
+        pts, _ = cv.projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs)
+        return pts
+
+    @staticmethod
+    def reorderCircular(
+        elements: list[Any] | np.ndarray, backwards: bool, shiftStart: int
+    ) -> list[Any]:
+        size = len(elements)
+        reordered = []
+        dir = -1 if backwards else 1
+        for i in range(size):
+            index = (i * dir + shiftStart * dir) % size
+            if index < 0:
+                index += size
+            reordered.append(elements[index])
+        return reordered
+
+    @staticmethod
+    def translationEDNToNWU(trl: Translation3d) -> Translation3d:
+        return trl.rotateBy(EDN_TO_NWU)
+
+    @staticmethod
+    def rotationEDNToNWU(rot: Rotation3d) -> Rotation3d:
+        return -EDN_TO_NWU + (rot + EDN_TO_NWU)
+
+    @staticmethod
+    def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d:
+        return OpenCVHelp.translationEDNToNWU(Translation3d(tvecInput))
+
+    @staticmethod
+    def rVecToRotation(rvecInput: np.ndarray) -> Rotation3d:
+        return OpenCVHelp.rotationEDNToNWU(Rotation3d(rvecInput))
+
+    @staticmethod
+    def solvePNP_Square(
+        cameraMatrix: np.ndarray,
+        distCoeffs: np.ndarray,
+        modelTrls: list[Translation3d],
+        imagePoints: np.ndarray,
+    ) -> PnpResult | None:
+        modelTrls = OpenCVHelp.reorderCircular(modelTrls, True, -1)
+        imagePoints = np.array(OpenCVHelp.reorderCircular(imagePoints, True, -1))
+        objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
+
+        alt: Transform3d | None = None
+        for tries in range(2):
+            retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
+                objectMat,
+                imagePoints,
+                cameraMatrix,
+                distCoeffs,
+                flags=cv.SOLVEPNP_IPPE_SQUARE,
+            )
+
+            best = Transform3d(
+                OpenCVHelp.tVecToTranslation(tvecs[0]),
+                OpenCVHelp.rVecToRotation(rvecs[0]),
+            )
+            if len(tvecs) > 1:
+                alt = Transform3d(
+                    OpenCVHelp.tVecToTranslation(tvecs[1]),
+                    OpenCVHelp.rVecToRotation(rvecs[1]),
+                )
+
+            if not math.isnan(reprojectionError[0, 0]):
+                break
+            else:
+                pt = imagePoints[0]
+                pt[0, 0] -= 0.001
+                pt[0, 1] -= 0.001
+                imagePoints[0] = pt
+
+        if math.isnan(reprojectionError[0, 0]):
+            print("SolvePNP_Square failed!")
+            return None
+
+        if alt:
+            return PnpResult(
+                best=best,
+                bestReprojErr=reprojectionError[0, 0],
+                alt=alt,
+                altReprojErr=reprojectionError[1, 0],
+                ambiguity=reprojectionError[0, 0] / reprojectionError[1, 0],
+            )
+        else:
+            # We have no alternative so set it to best as well
+            return PnpResult(
+                best=best,
+                bestReprojErr=reprojectionError[0],
+                alt=best,
+                altReprojErr=reprojectionError[0],
+            )
+
+    @staticmethod
+    def solvePNP_SQPNP(
+        cameraMatrix: np.ndarray,
+        distCoeffs: np.ndarray,
+        modelTrls: list[Translation3d],
+        imagePoints: np.ndarray,
+    ) -> PnpResult | None:
+        objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
+
+        retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
+            objectMat, imagePoints, cameraMatrix, distCoeffs, flags=cv.SOLVEPNP_SQPNP
+        )
+
+        error = reprojectionError[0, 0]
+        best = Transform3d(
+            OpenCVHelp.tVecToTranslation(tvecs[0]), OpenCVHelp.rVecToRotation(rvecs[0])
+        )
+
+        if math.isnan(error):
+            return None
+
+        # We have no alternative so set it to best as well
+        result = PnpResult(best=best, bestReprojErr=error, alt=best, altReprojErr=error)
+        return result

photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py

@@ -0,0 +1,32 @@
+from typing import Self
+
+from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
+
+
+class RotTrlTransform3d:
+    def __init__(
+        self, rot: Rotation3d = Rotation3d(), trl: Translation3d = Translation3d()
+    ):
+        self.rot = rot
+        self.trl = trl
+
+    def inverse(self) -> Self:
+        invRot = -self.rot
+        invTrl = -(self.trl.rotateBy(invRot))
+        return type(self)(invRot, invTrl)
+
+    def getTransform(self) -> Transform3d:
+        return Transform3d(self.trl, self.rot)
+
+    def getTranslation(self) -> Translation3d:
+        return self.trl
+
+    def getRotation(self) -> Rotation3d:
+        return self.rot
+
+    def apply(self, trlToApply: Translation3d) -> Translation3d:
+        return trlToApply.rotateBy(self.rot) + self.trl
+
+    @classmethod
+    def makeRelativeTo(cls, pose: Pose3d) -> Self:
+        return cls(pose.rotation(), pose.translation()).inverse()

photon-lib/py/photonlibpy/estimation/targetModel.py

@@ -0,0 +1,137 @@
+import math
+from typing import List, Self
+
+from wpimath.geometry import Pose3d, Rotation2d, Rotation3d, Translation3d
+from wpimath.units import meters
+
+from . import RotTrlTransform3d
+
+
+class TargetModel:
+    def __init__(
+        self,
+        *,
+        width: meters | None = None,
+        height: meters | None = None,
+        length: meters | None = None,
+        diameter: meters | None = None,
+        verts: List[Translation3d] | None = None
+    ):
+
+        if (
+            width is not None
+            and height is not None
+            and length is None
+            and diameter is None
+            and verts is None
+        ):
+            self.isPlanar = True
+            self.isSpherical = False
+            self.vertices = [
+                Translation3d(0.0, -width / 2.0, -height / 2.0),
+                Translation3d(0.0, width / 2.0, -height / 2.0),
+                Translation3d(0.0, width / 2.0, height / 2.0),
+                Translation3d(0.0, -width / 2.0, height / 2.0),
+            ]
+
+            return
+
+        elif (
+            length is not None
+            and width is not None
+            and height is not None
+            and diameter is None
+            and verts is None
+        ):
+            verts = [
+                Translation3d(length / 2.0, -width / 2.0, -height / 2.0),
+                Translation3d(length / 2.0, width / 2.0, -height / 2.0),
+                Translation3d(length / 2.0, width / 2.0, height / 2.0),
+                Translation3d(length / 2.0, -width / 2.0, height / 2.0),
+                Translation3d(-length / 2.0, -width / 2.0, height / 2.0),
+                Translation3d(-length / 2.0, width / 2.0, height / 2.0),
+                Translation3d(-length / 2.0, width / 2.0, -height / 2.0),
+                Translation3d(-length / 2.0, -width / 2.0, -height / 2.0),
+            ]
+            # Handle the rest of this in the "default" case
+        elif (
+            diameter is not None
+            and width is None
+            and height is None
+            and length is None
+            and verts is None
+        ):
+            self.isPlanar = False
+            self.isSpherical = True
+            self.vertices = [
+                Translation3d(0.0, -diameter / 2.0, 0.0),
+                Translation3d(0.0, 0.0, -diameter / 2.0),
+                Translation3d(0.0, diameter / 2.0, 0.0),
+                Translation3d(0.0, 0.0, diameter / 2.0),
+            ]
+            return
+        elif (
+            verts is not None
+            and width is None
+            and height is None
+            and length is None
+            and diameter is None
+        ):
+            # Handle this in the "default" case
+            pass
+        else:
+            raise Exception("Not a valid overload")
+
+        # TODO maybe remove this if there is a better/preferred way
+        # make the python type checking gods happy
+        assert verts is not None
+
+        self.isSpherical = False
+        if len(verts) <= 2:
+            self.vertices: List[Translation3d] = []
+            self.isPlanar = False
+        else:
+            cornersPlaner = True
+            for corner in verts:
+                if abs(corner.X() < 1e-4):
+                    cornersPlaner = False
+            self.isPlanar = cornersPlaner
+
+        self.vertices = verts
+
+    def getFieldVertices(self, targetPose: Pose3d) -> List[Translation3d]:
+        basisChange = RotTrlTransform3d(targetPose.rotation(), targetPose.translation())
+
+        retVal = []
+
+        for vert in self.vertices:
+            retVal.append(basisChange.apply(vert))
+
+        return retVal
+
+    @classmethod
+    def getOrientedPose(cls, tgtTrl: Translation3d, cameraTrl: Translation3d):
+        relCam = cameraTrl - tgtTrl
+        orientToCam = Rotation3d(
+            0.0,
+            Rotation2d(math.hypot(relCam.X(), relCam.Y()), relCam.Z()).radians(),
+            Rotation2d(relCam.X(), relCam.Y()).radians(),
+        )
+        return Pose3d(tgtTrl, orientToCam)
+
+    def getVertices(self) -> List[Translation3d]:
+        return self.vertices
+
+    def getIsPlanar(self) -> bool:
+        return self.isPlanar
+
+    def getIsSpherical(self) -> bool:
+        return self.isSpherical
+
+    @classmethod
+    def AprilTag36h11(cls) -> Self:
+        return cls(width=6.5 * 0.0254, height=6.5 * 0.0254)
+
+    @classmethod
+    def AprilTag16h5(cls) -> Self:
+        return cls(width=6.0 * 0.0254, height=6.0 * 0.0254)

photon-lib/py/photonlibpy/estimation/visionEstimation.py

@@ -0,0 +1,91 @@
+import numpy as np
+from robotpy_apriltag import AprilTag, AprilTagFieldLayout
+from wpimath.geometry import Pose3d, Transform3d, Translation3d
+
+from ..targeting import PhotonTrackedTarget, PnpResult, TargetCorner
+from . import OpenCVHelp, TargetModel
+
+
+class VisionEstimation:
+    @staticmethod
+    def getVisibleLayoutTags(
+        visTags: list[PhotonTrackedTarget], layout: AprilTagFieldLayout
+    ) -> list[AprilTag]:
+        retVal: list[AprilTag] = []
+        for tag in visTags:
+            id = tag.getFiducialId()
+            maybePose = layout.getTagPose(id)
+            if maybePose:
+                tag = AprilTag()
+                tag.ID = id
+                tag.pose = maybePose
+                retVal.append(tag)
+        return retVal
+
+    @staticmethod
+    def estimateCamPosePNP(
+        cameraMatrix: np.ndarray,
+        distCoeffs: np.ndarray,
+        visTags: list[PhotonTrackedTarget],
+        layout: AprilTagFieldLayout,
+        tagModel: TargetModel,
+    ) -> PnpResult | None:
+        if len(visTags) == 0:
+            return None
+
+        corners: list[TargetCorner] = []
+        knownTags: list[AprilTag] = []
+
+        for tgt in visTags:
+            id = tgt.getFiducialId()
+            maybePose = layout.getTagPose(id)
+            if maybePose:
+                tag = AprilTag()
+                tag.ID = id
+                tag.pose = maybePose
+                knownTags.append(tag)
+                currentCorners = tgt.getDetectedCorners()
+                if currentCorners:
+                    corners += currentCorners
+
+        if len(knownTags) == 0 or len(corners) == 0 or len(corners) % 4 != 0:
+            return None
+
+        points = OpenCVHelp.cornersToPoints(corners)
+
+        if len(knownTags) == 1:
+            camToTag = OpenCVHelp.solvePNP_Square(
+                cameraMatrix, distCoeffs, tagModel.getVertices(), points
+            )
+            if not camToTag:
+                return None
+
+            bestPose = knownTags[0].pose.transformBy(camToTag.best.inverse())
+            altPose = Pose3d()
+            if camToTag.ambiguity != 0:
+                altPose = knownTags[0].pose.transformBy(camToTag.alt.inverse())
+
+            o = Pose3d()
+            result = PnpResult(
+                best=Transform3d(o, bestPose),
+                alt=Transform3d(o, altPose),
+                ambiguity=camToTag.ambiguity,
+                bestReprojErr=camToTag.bestReprojErr,
+                altReprojErr=camToTag.altReprojErr,
+            )
+            return result
+        else:
+            objectTrls: list[Translation3d] = []
+            for tag in knownTags:
+                verts = tagModel.getFieldVertices(tag.pose)
+                objectTrls += verts
+
+            ret = OpenCVHelp.solvePNP_SQPNP(
+                cameraMatrix, distCoeffs, objectTrls, points
+            )
+            if ret:
+                # Invert best/alt transforms
+                ret.best = ret.best.inverse()
+                ret.alt = ret.alt.inverse()
+
+            return ret

photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py

@@ -20,15 +20,26 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
+from ..packet import Packet
 from ..targeting import *
 
 
 class MultiTargetPNPResultSerde:
-
     # Message definition md5sum. See photon_packet.adoc for details
     MESSAGE_VERSION = "541096947e9f3ca2d3f425ff7b04aa7b"
     MESSAGE_FORMAT = "PnpResult:ae4d655c0a3104d88df4f5db144c1e86 estimatedPose;int16 fiducialIDsUsed[?];"
 
+    @staticmethod
+    def pack(value: "MultiTargetPNPResult") -> "Packet":
+        ret = Packet()
+
+        # estimatedPose is of non-intrinsic type PnpResult
+        ret.encodeBytes(PnpResult.photonStruct.pack(value.estimatedPose).getData())
+
+        # fiducialIDsUsed is a custom VLA!
+        ret.encodeShortList(value.fiducialIDsUsed)
+        return ret
+
     @staticmethod
     def unpack(packet: "Packet") -> "MultiTargetPNPResult":
         ret = MultiTargetPNPResult()

photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py

@@ -20,15 +20,32 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
+from ..packet import Packet
 from ..targeting import *
 
 
 class PhotonPipelineMetadataSerde:
-
     # Message definition md5sum. See photon_packet.adoc for details
     MESSAGE_VERSION = "ac0a45f686457856fb30af77699ea356"
     MESSAGE_FORMAT = "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;int64 timeSinceLastPong;"
 
+    @staticmethod
+    def pack(value: "PhotonPipelineMetadata") -> "Packet":
+        ret = Packet()
+
+        # sequenceID is of intrinsic type int64
+        ret.encodeLong(value.sequenceID)
+
+        # captureTimestampMicros is of intrinsic type int64
+        ret.encodeLong(value.captureTimestampMicros)
+
+        # publishTimestampMicros is of intrinsic type int64
+        ret.encodeLong(value.publishTimestampMicros)
+
+        # timeSinceLastPong is of intrinsic type int64
+        ret.encodeLong(value.timeSinceLastPong)
+        return ret
+
     @staticmethod
     def unpack(packet: "Packet") -> "PhotonPipelineMetadata":
         ret = PhotonPipelineMetadata()

photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py

@@ -20,15 +20,31 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
+from ..packet import Packet
 from ..targeting import *
 
 
 class PhotonPipelineResultSerde:
-
     # Message definition md5sum. See photon_packet.adoc for details
     MESSAGE_VERSION = "4b2ff16a964b5e2bf04be0c1454d91c4"
     MESSAGE_FORMAT = "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"
 
+    @staticmethod
+    def pack(value: "PhotonPipelineResult") -> "Packet":
+        ret = Packet()
+
+        # metadata is of non-intrinsic type PhotonPipelineMetadata
+        ret.encodeBytes(
+            PhotonPipelineMetadata.photonStruct.pack(value.metadata).getData()
+        )
+
+        # targets is a custom VLA!
+        ret.encodeList(value.targets, PhotonTrackedTarget.photonStruct)
+
+        # multitagResult is optional! it better not be a VLA too
+        ret.encodeOptional(value.multitagResult, MultiTargetPNPResult.photonStruct)
+        return ret
+
     @staticmethod
     def unpack(packet: "Packet") -> "PhotonPipelineResult":
         ret = PhotonPipelineResult()

photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py

@@ -20,15 +20,54 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
+from ..packet import Packet
 from ..targeting import *
 
 
 class PhotonTrackedTargetSerde:
-
     # Message definition md5sum. See photon_packet.adoc for details
     MESSAGE_VERSION = "cc6dbb5c5c1e0fa808108019b20863f1"
     MESSAGE_FORMAT = "float64 yaw;float64 pitch;float64 area;float64 skew;int32 fiducialId;int32 objDetectId;float32 objDetectConf;Transform3d bestCameraToTarget;Transform3d altCameraToTarget;float64 poseAmbiguity;TargetCorner:16f6ac0dedc8eaccb951f4895d9e18b6 minAreaRectCorners[?];TargetCorner:16f6ac0dedc8eaccb951f4895d9e18b6 detectedCorners[?];"
 
+    @staticmethod
+    def pack(value: "PhotonTrackedTarget") -> "Packet":
+        ret = Packet()
+
+        # yaw is of intrinsic type float64
+        ret.encodeDouble(value.yaw)
+
+        # pitch is of intrinsic type float64
+        ret.encodeDouble(value.pitch)
+
+        # area is of intrinsic type float64
+        ret.encodeDouble(value.area)
+
+        # skew is of intrinsic type float64
+        ret.encodeDouble(value.skew)
+
+        # fiducialId is of intrinsic type int32
+        ret.encodeInt(value.fiducialId)
+
+        # objDetectId is of intrinsic type int32
+        ret.encodeInt(value.objDetectId)
+
+        # objDetectConf is of intrinsic type float32
+        ret.encodeFloat(value.objDetectConf)
+
+        ret.encodeTransform(value.bestCameraToTarget)
+
+        ret.encodeTransform(value.altCameraToTarget)
+
+        # poseAmbiguity is of intrinsic type float64
+        ret.encodeDouble(value.poseAmbiguity)
+
+        # minAreaRectCorners is a custom VLA!
+        ret.encodeList(value.minAreaRectCorners, TargetCorner.photonStruct)
+
+        # detectedCorners is a custom VLA!
+        ret.encodeList(value.detectedCorners, TargetCorner.photonStruct)
+        return ret
+
     @staticmethod
     def unpack(packet: "Packet") -> "PhotonTrackedTarget":
         ret = PhotonTrackedTarget()

photon-lib/py/photonlibpy/generated/PnpResultSerde.py

@@ -20,15 +20,33 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
+from ..packet import Packet
 from ..targeting import *
 
 
 class PnpResultSerde:
-
     # Message definition md5sum. See photon_packet.adoc for details
     MESSAGE_VERSION = "ae4d655c0a3104d88df4f5db144c1e86"
     MESSAGE_FORMAT = "Transform3d best;Transform3d alt;float64 bestReprojErr;float64 altReprojErr;float64 ambiguity;"
 
+    @staticmethod
+    def pack(value: "PnpResult") -> "Packet":
+        ret = Packet()
+
+        ret.encodeTransform(value.best)
+
+        ret.encodeTransform(value.alt)
+
+        # bestReprojErr is of intrinsic type float64
+        ret.encodeDouble(value.bestReprojErr)
+
+        # altReprojErr is of intrinsic type float64
+        ret.encodeDouble(value.altReprojErr)
+
+        # ambiguity is of intrinsic type float64
+        ret.encodeDouble(value.ambiguity)
+        return ret
+
     @staticmethod
     def unpack(packet: "Packet") -> "PnpResult":
         ret = PnpResult()

photon-lib/py/photonlibpy/generated/TargetCornerSerde.py

@@ -20,15 +20,26 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
+from ..packet import Packet
 from ..targeting import *
 
 
 class TargetCornerSerde:
-
     # Message definition md5sum. See photon_packet.adoc for details
     MESSAGE_VERSION = "16f6ac0dedc8eaccb951f4895d9e18b6"
     MESSAGE_FORMAT = "float64 x;float64 y;"
 
+    @staticmethod
+    def pack(value: "TargetCorner") -> "Packet":
+        ret = Packet()
+
+        # x is of intrinsic type float64
+        ret.encodeDouble(value.x)
+
+        # y is of intrinsic type float64
+        ret.encodeDouble(value.y)
+        return ret
+
     @staticmethod
     def unpack(packet: "Packet") -> "TargetCorner":
         ret = TargetCorner()

photon-lib/py/photonlibpy/generated/__init__.py

@@ -2,7 +2,6 @@
 
 from .MultiTargetPNPResultSerde import MultiTargetPNPResultSerde  # noqa
 from .PhotonPipelineMetadataSerde import PhotonPipelineMetadataSerde  # noqa
-from .PhotonPipelineMetadataSerde import PhotonPipelineMetadataSerde  # noqa
 from .PhotonPipelineResultSerde import PhotonPipelineResultSerde  # noqa
 from .PhotonTrackedTargetSerde import PhotonTrackedTargetSerde  # noqa
 from .PnpResultSerde import PnpResultSerde  # noqa

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

@@ -0,0 +1,64 @@
+import ntcore as nt
+from wpimath.geometry import Transform3d
+
+from ..generated.PhotonPipelineResultSerde import PhotonPipelineResultSerde
+
+PhotonPipelineResult_TYPE_STRING = (
+    "photonstruct:PhotonPipelineResult:" + PhotonPipelineResultSerde.MESSAGE_VERSION
+)
+
+
+class NTTopicSet:
+
+    def __init__(self) -> None:
+        self.subTable = nt.NetworkTableInstance.getDefault()
+
+    def updateEntries(self) -> None:
+        options = nt.PubSubOptions()
+        options.periodic = 0.01
+        options.sendAll = True
+        self.rawBytesEntry = self.subTable.getRawTopic("rawBytes").publish(
+            PhotonPipelineResult_TYPE_STRING, options
+        )
+        self.rawBytesEntry.getTopic().setProperty(
+            "message_uuid", PhotonPipelineResultSerde.MESSAGE_VERSION
+        )
+        self.pipelineIndexPublisher = self.subTable.getIntegerTopic(
+            "pipelineIndexState"
+        ).publish()
+        self.pipelineIndexRequestSub = self.subTable.getIntegerTopic(
+            "pipelineIndexRequest"
+        ).subscribe(0)
+
+        self.driverModePublisher = self.subTable.getBooleanTopic("driverMode").publish()
+        self.driverModeSubscriber = self.subTable.getBooleanTopic(
+            "driverModeRequest"
+        ).subscribe(False)
+
+        self.driverModeSubscriber.getTopic().publish().setDefault(False)
+
+        self.latencyMillisEntry = self.subTable.getDoubleTopic(
+            "latencyMillis"
+        ).publish()
+        self.hasTargetEntry = self.subTable.getBooleanTopic("hasTargets").publish()
+
+        self.targetPitchEntry = self.subTable.getDoubleTopic("targetPitch").publish()
+        self.targetAreaEntry = self.subTable.getDoubleTopic("targetArea").publish()
+        self.targetYawEntry = self.subTable.getDoubleTopic("targetYaw").publish()
+        self.targetPoseEntry = self.subTable.getStructTopic(
+            "targetPose", Transform3d
+        ).publish()
+        self.targetSkewEntry = self.subTable.getDoubleTopic("targetSkew").publish()
+
+        self.bestTargetPosX = self.subTable.getDoubleTopic("targetPixelsX").publish()
+        self.bestTargetPosY = self.subTable.getDoubleTopic("targetPixelsY").publish()
+
+        self.heartbeatTopic = self.subTable.getIntegerTopic("heartbeat")
+        self.heartbeatPublisher = self.heartbeatTopic.publish()
+
+        self.cameraIntrinsicsPublisher = self.subTable.getDoubleArrayTopic(
+            "cameraIntrinsics"
+        ).publish()
+        self.cameraDistortionPublisher = self.subTable.getDoubleArrayTopic(
+            "cameraDistortion"
+        ).publish()

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

@@ -0,0 +1 @@
+from .NTTopicSet import NTTopicSet

photon-lib/py/photonlibpy/packet.py

@@ -16,13 +16,21 @@
 ###############################################################################
 
 import struct
-from typing import Any, Optional, Type
-from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Quaternion
+from typing import Generic, Optional, Protocol, TypeVar
+
 import wpilib
+from wpimath.geometry import Quaternion, Rotation3d, Transform3d, Translation3d
+
+T = TypeVar("T")
+
+
+class Serde(Generic[T], Protocol):
+    def pack(self, value: T) -> "Packet": ...
+    def unpack(self, packet: "Packet") -> T: ...
 
 
 class Packet:
-    def __init__(self, data: bytes):
+    def __init__(self, data: bytes = b""):
         """
         * Constructs an empty packet.
         *
@@ -33,9 +41,9 @@ class Packet:
         self.readPos = 0
         self.outOfBytes = False
 
-    def clear(self):
+    def clear(self) -> None:
         """Clears the packet and resets the read and write positions."""
-        self.packetData = [0] * self.size
+        self.packetData = bytes(self.size)
         self.readPos = 0
         self.outOfBytes = False
 
@@ -157,7 +165,7 @@ class Packet:
             ret.append(self.decodeDouble())
         return ret
 
-    def decodeShortList(self) -> list[float]:
+    def decodeShortList(self) -> list[int]:
         """
         * Returns a decoded array of shorts from the packet.
         """
@@ -186,15 +194,122 @@ class Packet:
 
         return Transform3d(translation, rotation)
 
-    def decodeList(self, serde: Type):
+    def decodeList(self, serde: Serde[T]) -> list[T]:
         retList = []
         arr_len = self.decode8()
         for _ in range(arr_len):
             retList.append(serde.unpack(self))
         return retList
 
-    def decodeOptional(self, serde: Type) -> Optional[Any]:
+    def decodeOptional(self, serde: Serde[T]) -> Optional[T]:
         if self.decodeBoolean():
             return serde.unpack(self)
         else:
             return None
+
+    def _encodeGeneric(self, packFormat, value):
+        """
+        Append bytes to the packet data buffer.
+        """
+        self.packetData = self.packetData + struct.pack(packFormat, value)
+        self.size = len(self.packetData)
+
+    def encode8(self, value: int):
+        """
+        Encodes a single byte and appends it to the packet.
+        """
+        self._encodeGeneric("<b", value)
+
+    def encode16(self, value: int):
+        """
+        Encodes a short (2 bytes) and appends it to the packet.
+        """
+        self._encodeGeneric("<h", value)
+
+    def encodeInt(self, value: int):
+        """
+        Encodes an int (4 bytes) and appends it to the packet.
+        """
+        self._encodeGeneric("<l", value)
+
+    def encodeFloat(self, value: float):
+        """
+        Encodes a float (4 bytes) and appends it to the packet.
+        """
+        self._encodeGeneric("<f", value)
+
+    def encodeLong(self, value: int):
+        """
+        Encodes a long (8 bytes) and appends it to the packet.
+        """
+        self._encodeGeneric("<q", value)
+
+    def encodeDouble(self, value: float):
+        """
+        Encodes a double (8 bytes) and appends it to the packet.
+        """
+        self._encodeGeneric("<d", value)
+
+    def encodeBoolean(self, value: bool):
+        """
+        Encodes a boolean as a single byte and appends it to the packet.
+        """
+        self.encode8(1 if value else 0)
+
+    def encodeDoubleArray(self, values: list[float]):
+        """
+        Encodes an array of doubles and appends it to the packet.
+        """
+        self.encode8(len(values))
+        for value in values:
+            self.encodeDouble(value)
+
+    def encodeShortList(self, values: list[int]):
+        """
+        Encodes a list of shorts, with length prefixed as a single byte.
+        """
+        self.encode8(len(values))
+        for value in values:
+            self.encode16(value)
+
+    def encodeTransform(self, transform: Transform3d):
+        """
+        Encodes a Transform3d (translation and rotation) and appends it to the packet.
+        """
+        # Encode Translation3d part (x, y, z)
+        self.encodeDouble(transform.translation().x)
+        self.encodeDouble(transform.translation().y)
+        self.encodeDouble(transform.translation().z)
+
+        # Encode Rotation3d as Quaternion (w, x, y, z)
+        quaternion = transform.rotation().getQuaternion()
+        self.encodeDouble(quaternion.W())
+        self.encodeDouble(quaternion.X())
+        self.encodeDouble(quaternion.Y())
+        self.encodeDouble(quaternion.Z())
+
+    def encodeList(self, values: list[T], serde: Serde[T]):
+        """
+        Encodes a list of items using a specific serializer and appends it to the packet.
+        """
+        self.encode8(len(values))
+        for item in values:
+            packed = serde.pack(item)
+            self.packetData = self.packetData + packed.getData()
+            self.size = len(self.packetData)
+
+    def encodeOptional(self, value: Optional[T], serde: Serde[T]):
+        """
+        Encodes an optional value using a specific serializer.
+        """
+        if value is None:
+            self.encodeBoolean(False)
+        else:
+            self.encodeBoolean(True)
+            packed = serde.pack(value)
+            self.packetData = self.packetData + packed.getData()
+            self.size = len(self.packetData)
+
+    def encodeBytes(self, value: bytes):
+        self.packetData = self.packetData + value
+        self.size = len(self.packetData)

photon-lib/py/photonlibpy/photonCamera.py

@@ -17,15 +17,17 @@
 
 from enum import Enum
 from typing import List
+
 import ntcore
-from wpilib import RobotController, Timer
-import wpilib
-from .packet import Packet
-from .targeting.photonPipelineResult import PhotonPipelineResult
-from .version import PHOTONVISION_VERSION, PHOTONLIB_VERSION  # type: ignore[import-untyped]
 
 # magical import to make serde stuff work
 import photonlibpy.generated  # noqa
+import wpilib
+from wpilib import RobotController, Timer
+
+from .packet import Packet
+from .targeting.photonPipelineResult import PhotonPipelineResult
+from .version import PHOTONLIB_VERSION  # type: ignore[import-untyped]
 
 
 class VisionLEDMode(Enum):
@@ -231,12 +233,13 @@ class PhotonCamera:
 
         remoteUUID = self._rawBytesEntry.getTopic().getProperty("message_uuid")
 
-        if remoteUUID is None or len(remoteUUID) == 0:
+        if not remoteUUID:
             wpilib.reportWarning(
                 f"PhotonVision coprocessor at path {self._path} has not reported a message interface UUID - is your coprocessor's camera started?",
                 True,
             )
 
+        assert isinstance(remoteUUID, str)
         # ntcore hands us a JSON string with leading/trailing quotes - remove those
         remoteUUID = remoteUUID.replace('"', "")
 

photon-lib/py/photonlibpy/photonPoseEstimator.py

@@ -20,11 +20,11 @@ from typing import Optional
 
 import wpilib
 from robotpy_apriltag import AprilTagFieldLayout
-from wpimath.geometry import Transform3d, Pose3d, Pose2d
+from wpimath.geometry import Pose2d, Pose3d, Transform3d
 
-from .targeting.photonPipelineResult import PhotonPipelineResult
-from .photonCamera import PhotonCamera
 from .estimatedRobotPose import EstimatedRobotPose
+from .photonCamera import PhotonCamera
+from .targeting.photonPipelineResult import PhotonPipelineResult
 
 
 class PoseStrategy(enum.Enum):
@@ -269,8 +269,8 @@ class PhotonPoseEstimator:
     def _multiTagOnCoprocStrategy(
         self, result: PhotonPipelineResult
     ) -> Optional[EstimatedRobotPose]:
-        if result.multiTagResult.estimatedPose.isPresent:
-            best_tf = result.multiTagResult.estimatedPose.best
+        if result.multitagResult is not None:
+            best_tf = result.multitagResult.estimatedPose.best
             best = (
                 Pose3d()
                 .transformBy(best_tf)  # field-to-camera

photon-lib/py/photonlibpy/simulation/__init__.py

@@ -0,0 +1,5 @@
+from .photonCameraSim import PhotonCameraSim
+from .simCameraProperties import SimCameraProperties
+from .videoSimUtil import VideoSimUtil
+from .visionSystemSim import VisionSystemSim
+from .visionTargetSim import VisionTargetSim

photon-lib/py/photonlibpy/simulation/photonCameraSim.py

@@ -0,0 +1,408 @@
+import math
+import typing
+
+import cscore as cs
+import cv2 as cv
+import numpy as np
+import robotpy_apriltag
+import wpilib
+from wpimath.geometry import Pose3d, Transform3d
+from wpimath.units import meters, seconds
+
+from ..estimation import OpenCVHelp, RotTrlTransform3d, TargetModel, VisionEstimation
+from ..estimation.cameraTargetRelation import CameraTargetRelation
+from ..networktables.NTTopicSet import NTTopicSet
+from ..photonCamera import PhotonCamera
+from ..targeting import (
+    MultiTargetPNPResult,
+    PhotonPipelineMetadata,
+    PhotonPipelineResult,
+    PhotonTrackedTarget,
+    PnpResult,
+    TargetCorner,
+)
+from .simCameraProperties import SimCameraProperties
+from .visionTargetSim import VisionTargetSim
+
+
+class PhotonCameraSim:
+    kDefaultMinAreaPx: float = 100
+
+    def __init__(
+        self,
+        camera: PhotonCamera,
+        props: SimCameraProperties | None = None,
+        minTargetAreaPercent: float | None = None,
+        maxSightRange: meters | None = None,
+    ):
+
+        self.minTargetAreaPercent: float = 0.0
+        self.maxSightRange: float = 1.0e99
+        self.videoSimRawEnabled: bool = False
+        self.videoSimWireframeEnabled: bool = False
+        self.videoSimWireframeResolution: float = 0.1
+        self.videoSimProcEnabled: bool = True
+        self.ts = NTTopicSet()
+        self.heartbeatCounter: int = 0
+        self.nextNtEntryTime = int(wpilib.Timer.getFPGATimestamp() * 1e6)
+        self.tagLayout = robotpy_apriltag.loadAprilTagLayoutField(
+            robotpy_apriltag.AprilTagField.k2024Crescendo
+        )
+
+        if (
+            camera is not None
+            and props is None
+            and minTargetAreaPercent is None
+            and maxSightRange is None
+        ):
+            props = SimCameraProperties.PERFECT_90DEG()
+        elif (
+            camera is not None
+            and props is not None
+            and minTargetAreaPercent is not None
+            and maxSightRange is not None
+        ):
+            pass
+        elif (
+            camera is not None
+            and props is not None
+            and minTargetAreaPercent is None
+            and maxSightRange is None
+        ):
+            pass
+        else:
+            raise Exception("Invalid Constructor Called")
+
+        self.cam = camera
+        self.prop = props
+        self.setMinTargetAreaPixels(PhotonCameraSim.kDefaultMinAreaPx)
+
+        # TODO Check fps is right
+        self.videoSimRaw = cs.CvSource(
+            self.cam.getName() + "-raw",
+            cs.VideoMode.PixelFormat.kGray,
+            self.prop.getResWidth(),
+            self.prop.getResHeight(),
+            1,
+        )
+        self.videoSimFrameRaw = np.zeros(
+            (self.prop.getResWidth(), self.prop.getResHeight())
+        )
+
+        # TODO Check fps is right
+        self.videoSimProcessed = cs.CvSource(
+            self.cam.getName() + "-processed",
+            cs.VideoMode.PixelFormat.kGray,
+            self.prop.getResWidth(),
+            self.prop.getResHeight(),
+            1,
+        )
+        self.videoSimFrameProcessed = np.zeros(
+            (self.prop.getResWidth(), self.prop.getResHeight())
+        )
+
+        self.ts.subTable = self.cam._cameraTable
+        self.ts.updateEntries()
+
+        # Handle this last explicitly for this function signature because the other constructor is called in the initialiser list
+        if (
+            camera is not None
+            and props is not None
+            and minTargetAreaPercent is not None
+            and maxSightRange is not None
+        ):
+            self.minTargetAreaPercent = minTargetAreaPercent
+            self.maxSightRange = maxSightRange
+
+    def getCamera(self) -> PhotonCamera:
+        return self.cam
+
+    def getMinTargetAreaPercent(self) -> float:
+        return self.minTargetAreaPercent
+
+    def getMinTargetAreaPixels(self) -> float:
+        return self.minTargetAreaPercent / 100.0 * self.prop.getResArea()
+
+    def getMaxSightRange(self) -> meters:
+        return self.maxSightRange
+
+    def getVideoSimRaw(self) -> cs.CvSource:
+        return self.videoSimRaw
+
+    def getVideoSimFrameRaw(self) -> np.ndarray:
+        return self.videoSimFrameRaw
+
+    def canSeeTargetPose(self, camPose: Pose3d, target: VisionTargetSim) -> bool:
+        rel = CameraTargetRelation(camPose, target.getPose())
+        return (
+            (
+                abs(rel.camToTargYaw.degrees())
+                < self.prop.getHorizFOV().degrees() / 2.0
+                and abs(rel.camToTargPitch.degrees())
+                < self.prop.getVertFOV().degrees() / 2.0
+            )
+            and (
+                not target.getModel().getIsPlanar()
+                or abs(rel.targtoCamAngle.degrees()) < 90
+            )
+            and rel.camToTarg.translation().norm() <= self.maxSightRange
+        )
+
+    def canSeeCorner(self, points: np.ndarray) -> bool:
+        assert points.shape[1] == 1
+        assert points.shape[2] == 2
+        for pt in points:
+            x = pt[0, 0]
+            y = pt[0, 1]
+            if (
+                x < 0
+                or x > self.prop.getResWidth()
+                or y < 0
+                or y > self.prop.getResHeight()
+            ):
+                return False
+
+        return True
+
+    def consumeNextEntryTime(self) -> float | None:
+        now = int(wpilib.Timer.getFPGATimestamp() * 1e6)
+        timestamp = 0
+        iter = 0
+        while now >= self.nextNtEntryTime:
+            timestamp = int(self.nextNtEntryTime)
+            frameTime = int(self.prop.estSecUntilNextFrame() * 1e6)
+            self.nextNtEntryTime += frameTime
+
+            iter += 1
+            if iter > 50:
+                timestamp = now
+                self.nextNtEntryTime = now + frameTime
+                break
+
+        if timestamp != 0:
+            return timestamp
+
+        return None
+
+    def setMinTargetAreaPercent(self, areaPercent: float) -> None:
+        self.minTargetAreaPercent = areaPercent
+
+    def setMinTargetAreaPixels(self, areaPx: float) -> None:
+        self.minTargetAreaPercent = areaPx / self.prop.getResArea() * 100.0
+
+    def setMaxSightRange(self, range: meters) -> None:
+        self.maxSightRange = range
+
+    def enableRawStream(self, enabled: bool) -> None:
+        raise Exception("Raw stream not implemented")
+        # self.videoSimRawEnabled = enabled
+
+    def enableDrawWireframe(self, enabled: bool) -> None:
+        raise Exception("Wireframe not implemented")
+        # self.videoSimWireframeEnabled = enabled
+
+    def setWireframeResolution(self, resolution: float) -> None:
+        self.videoSimWireframeResolution = resolution
+
+    def enableProcessedStream(self, enabled: bool) -> None:
+        raise Exception("Processed stream not implemented")
+        # self.videoSimProcEnabled = enabled
+
+    def process(
+        self, latency: seconds, cameraPose: Pose3d, targets: list[VisionTargetSim]
+    ) -> PhotonPipelineResult:
+        # Sort targets by distance to camera - furthest to closest
+        def distance(target: VisionTargetSim):
+            return target.getPose().translation().distance(cameraPose.translation())
+
+        targets.sort(key=distance, reverse=True)
+
+        visibleTgts: list[
+            typing.Tuple[VisionTargetSim, list[typing.Tuple[float, float]]]
+        ] = []
+        detectableTgts: list[PhotonTrackedTarget] = []
+
+        camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
+
+        for tgt in targets:
+            if not self.canSeeTargetPose(cameraPose, tgt):
+                continue
+
+            fieldCorners = tgt.getFieldVertices()
+            isSpherical = tgt.getModel().getIsSpherical()
+            if isSpherical:
+                model = tgt.getModel()
+                fieldCorners = model.getFieldVertices(
+                    TargetModel.getOrientedPose(
+                        tgt.getPose().translation(), cameraPose.translation()
+                    )
+                )
+
+            imagePoints = OpenCVHelp.projectPoints(
+                self.prop.getIntrinsics(),
+                self.prop.getDistCoeffs(),
+                camRt,
+                fieldCorners,
+            )
+
+            if isSpherical:
+                center = OpenCVHelp.avgPoint(imagePoints)
+                l: int = 0
+                for i in range(4):
+                    if imagePoints[i, 0, 0] < imagePoints[l, 0, 0].x:
+                        l = i
+
+                lc = imagePoints[l]
+                angles = [
+                    0.0,
+                ] * 4
+                t = (l + 1) % 4
+                b = (l + 1) % 4
+                for i in range(4):
+                    if i == l:
+                        continue
+                    ic = imagePoints[i]
+                    angles[i] = math.atan2(lc[0, 1] - ic[0, 1], ic[0, 0] - lc[0, 0])
+                    if angles[i] >= angles[t]:
+                        t = i
+                    if angles[i] <= angles[b]:
+                        b = i
+                for i in range(4):
+                    if i != t and i != l and i != b:
+                        r = i
+                rect = cv.RotatedRect(
+                    center,
+                    (
+                        imagePoints[r, 0, 0] - lc[0, 0],
+                        imagePoints[b, 0, 1] - imagePoints[t, 0, 1],
+                    ),
+                    -angles[r],
+                )
+                imagePoints = rect.points()
+
+            visibleTgts.append((tgt, imagePoints))
+            noisyTargetCorners = self.prop.estPixelNoise(imagePoints)
+            minAreaRect = OpenCVHelp.getMinAreaRect(noisyTargetCorners)
+            minAreaRectPts = minAreaRect.points()
+            centerPt = minAreaRect.center
+            centerRot = self.prop.getPixelRot(centerPt)
+            areaPercent = self.prop.getContourAreaPercent(noisyTargetCorners)
+
+            if (
+                not self.canSeeCorner(noisyTargetCorners)
+                or not areaPercent >= self.minTargetAreaPercent
+            ):
+                continue
+
+            pnpSim: PnpResult | None = None
+            if tgt.fiducialId >= 0 and len(tgt.getFieldVertices()) == 4:
+                pnpSim = OpenCVHelp.solvePNP_Square(
+                    self.prop.getIntrinsics(),
+                    self.prop.getDistCoeffs(),
+                    tgt.getModel().getVertices(),
+                    noisyTargetCorners,
+                )
+
+            smallVec: list[TargetCorner] = []
+            for corner in minAreaRectPts:
+                smallVec.append(TargetCorner(corner[0], corner[1]))
+
+            cornersFloat = OpenCVHelp.pointsToTargetCorners(noisyTargetCorners)
+
+            detectableTgts.append(
+                PhotonTrackedTarget(
+                    yaw=math.degrees(-centerRot.Z()),
+                    pitch=math.degrees(-centerRot.Y()),
+                    area=areaPercent,
+                    skew=math.degrees(centerRot.X()),
+                    fiducialId=tgt.fiducialId,
+                    detectedCorners=cornersFloat,
+                    minAreaRectCorners=smallVec,
+                    bestCameraToTarget=pnpSim.best if pnpSim else Transform3d(),
+                    altCameraToTarget=pnpSim.alt if pnpSim else Transform3d(),
+                    poseAmbiguity=pnpSim.ambiguity if pnpSim else -1,
+                )
+            )
+
+        # Video streams disabled for now
+        if self.enableRawStream:
+            # VideoSimUtil::UpdateVideoProp(videoSimRaw, prop);
+            # cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()};
+            # cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1);
+            # blankFrame.assignTo(videoSimFrameRaw);
+            pass
+        if self.enableProcessedStream:
+            # VideoSimUtil::UpdateVideoProp(videoSimProcessed, prop);
+            pass
+
+        multiTagResults: MultiTargetPNPResult | None = None
+
+        visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(
+            detectableTgts, self.tagLayout
+        )
+
+        if len(visibleLayoutTags) > 1:
+            usedIds = [tag.ID for tag in visibleLayoutTags]
+            usedIds.sort()
+            pnpResult = VisionEstimation.estimateCamPosePNP(
+                self.prop.getIntrinsics(),
+                self.prop.getDistCoeffs(),
+                detectableTgts,
+                self.tagLayout,
+                TargetModel.AprilTag36h11(),
+            )
+            if pnpResult is not None:
+                multiTagResults = MultiTargetPNPResult(pnpResult, usedIds)
+
+        self.heartbeatCounter += 1
+        return PhotonPipelineResult(
+            metadata=PhotonPipelineMetadata(
+                self.heartbeatCounter, int(latency * 1e6), 1000000
+            ),
+            targets=detectableTgts,
+            multitagResult=multiTagResults,
+        )
+
+    def submitProcessedFrame(
+        self, result: PhotonPipelineResult, receiveTimestamp: float | None
+    ):
+        if receiveTimestamp is None:
+            receiveTimestamp = wpilib.Timer.getFPGATimestamp() * 1e6
+        receiveTimestamp = int(receiveTimestamp)
+
+        self.ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp)
+
+        newPacket = PhotonPipelineResult.photonStruct.pack(result)
+        self.ts.rawBytesEntry.set(newPacket.getData(), receiveTimestamp)
+
+        hasTargets = result.hasTargets()
+        self.ts.hasTargetEntry.set(hasTargets, receiveTimestamp)
+        if not hasTargets:
+            self.ts.targetPitchEntry.set(0.0, receiveTimestamp)
+            self.ts.targetYawEntry.set(0.0, receiveTimestamp)
+            self.ts.targetAreaEntry.set(0.0, receiveTimestamp)
+            self.ts.targetPoseEntry.set(Transform3d(), receiveTimestamp)
+            self.ts.targetSkewEntry.set(0.0, receiveTimestamp)
+        else:
+            bestTarget = result.getBestTarget()
+
+            self.ts.targetPitchEntry.set(bestTarget.getPitch(), receiveTimestamp)
+            self.ts.targetYawEntry.set(bestTarget.getYaw(), receiveTimestamp)
+            self.ts.targetAreaEntry.set(bestTarget.getArea(), receiveTimestamp)
+            self.ts.targetSkewEntry.set(bestTarget.getSkew(), receiveTimestamp)
+
+            self.ts.targetPoseEntry.set(
+                bestTarget.getBestCameraToTarget(), receiveTimestamp
+            )
+
+            intrinsics = self.prop.getIntrinsics()
+            intrinsicsView = intrinsics.flatten().tolist()
+            self.ts.cameraIntrinsicsPublisher.set(intrinsicsView, receiveTimestamp)
+
+            distortion = self.prop.getDistCoeffs()
+            distortionView = distortion.flatten().tolist()
+            self.ts.cameraDistortionPublisher.set(distortionView, receiveTimestamp)
+
+            self.ts.heartbeatPublisher.set(self.heartbeatCounter, receiveTimestamp)
+
+            self.ts.subTable.getInstance().flush()

photon-lib/py/photonlibpy/simulation/simCameraProperties.py

@@ -0,0 +1,661 @@
+import logging
+import math
+import typing
+
+import cv2 as cv
+import numpy as np
+from wpimath.geometry import Rotation2d, Rotation3d, Translation3d
+from wpimath.units import hertz, seconds
+
+from ..estimation import RotTrlTransform3d
+
+
+class SimCameraProperties:
+    def __init__(self, path: str | None = None, width: int = 0, height: int = 0):
+        self.resWidth: int = -1
+        self.resHeight: int = -1
+        self.camIntrinsics: np.ndarray = np.zeros((3, 3))  # [3,3]
+        self.distCoeffs: np.ndarray = np.zeros((8, 1))  # [8,1]
+        self.avgErrorPx: float = 0.0
+        self.errorStdDevPx: float = 0.0
+        self.frameSpeed: seconds = 0.0
+        self.exposureTime: seconds = 0.0
+        self.avgLatency: seconds = 0.0
+        self.latencyStdDev: seconds = 0.0
+        self.viewplanes: list[np.ndarray] = []  # [3,1]
+
+        if path is None:
+            self.setCalibration(960, 720, fovDiag=Rotation2d(math.radians(90.0)))
+        else:
+            raise Exception("not yet implemented")
+
+    def setCalibration(
+        self,
+        width: int,
+        height: int,
+        *,
+        fovDiag: Rotation2d | None = None,
+        newCamIntrinsics: np.ndarray | None = None,
+        newDistCoeffs: np.ndarray | None = None,
+    ):
+        # Should be an inverted XOR on the args to differentiate between the signatures
+
+        has_fov_args = fovDiag is not None
+        has_matrix_args = newCamIntrinsics is not None and newDistCoeffs is not None
+
+        if (has_fov_args and has_matrix_args) or (
+            not has_matrix_args and not has_fov_args
+        ):
+            raise Exception("not a correct function sig")
+
+        if has_fov_args:
+            if fovDiag.degrees() < 1.0 or fovDiag.degrees() > 179.0:
+                fovDiag = Rotation2d.fromDegrees(
+                    max(min(fovDiag.degrees(), 179.0), 1.0)
+                )
+                logging.error(
+                    "Requested invalid FOV! Clamping between (1, 179) degrees..."
+                )
+
+            resDiag = math.sqrt(width * width + height * height)
+            diagRatio = math.tan(fovDiag.radians() / 2.0)
+            fovWidth = Rotation2d(math.atan((diagRatio * (width / resDiag)) * 2))
+            fovHeight = Rotation2d(math.atan(diagRatio * (height / resDiag)) * 2)
+
+            newDistCoeffs = np.zeros((8, 1))
+
+            cx = width / 2.0 - 0.5
+            cy = height / 2.0 - 0.5
+
+            fx = cx / math.tan(fovWidth.radians() / 2.0)
+            fy = cy / math.tan(fovHeight.radians() / 2.0)
+
+            newCamIntrinsics = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]])
+
+        # really convince python we are doing the right thing
+        assert newCamIntrinsics is not None
+        assert newDistCoeffs is not None
+
+        self.resWidth = width
+        self.resHeight = height
+        self.camIntrinsics = newCamIntrinsics
+        self.distCoeffs = newDistCoeffs
+
+        p = [
+            Translation3d(
+                1.0,
+                Rotation3d(
+                    0.0,
+                    0.0,
+                    (self.getPixelYaw(0) + Rotation2d(math.pi / 2.0)).radians(),
+                ),
+            ),
+            Translation3d(
+                1.0,
+                Rotation3d(
+                    0.0,
+                    0.0,
+                    (self.getPixelYaw(width) + Rotation2d(math.pi / 2.0)).radians(),
+                ),
+            ),
+            Translation3d(
+                1.0,
+                Rotation3d(
+                    0.0,
+                    0.0,
+                    (self.getPixelPitch(0) + Rotation2d(math.pi / 2.0)).radians(),
+                ),
+            ),
+            Translation3d(
+                1.0,
+                Rotation3d(
+                    0.0,
+                    0.0,
+                    (self.getPixelPitch(height) + Rotation2d(math.pi / 2.0)).radians(),
+                ),
+            ),
+        ]
+
+        self.viewplanes = []
+
+        for i in p:
+            self.viewplanes.append(np.array([i.X(), i.Y(), i.Z()]))
+
+    def setCalibError(self, newAvgErrorPx: float, newErrorStdDevPx: float):
+        self.avgErrorPx = newAvgErrorPx
+        self.errorStdDevPx = newErrorStdDevPx
+
+    def setFPS(self, fps: hertz):
+        self.frameSpeed = max(1.0 / fps, self.exposureTime)
+
+    def setExposureTime(self, newExposureTime: seconds):
+        self.exposureTime = newExposureTime
+        self.frameSpeed = max(self.frameSpeed, self.exposureTime)
+
+    def setAvgLatency(self, newAvgLatency: seconds):
+        self.vgLatency = newAvgLatency
+
+    def setLatencyStdDev(self, newLatencyStdDev: seconds):
+        self.latencyStdDev = newLatencyStdDev
+
+    def getResWidth(self) -> int:
+        return self.resWidth
+
+    def getResHeight(self) -> int:
+        return self.resHeight
+
+    def getResArea(self) -> int:
+        return self.resWidth * self.resHeight
+
+    def getAspectRatio(self) -> float:
+        return 1.0 * self.resWidth / self.resHeight
+
+    def getIntrinsics(self) -> np.ndarray:
+        return self.camIntrinsics
+
+    def getDistCoeffs(self) -> np.ndarray:
+        return self.distCoeffs
+
+    def getFPS(self) -> hertz:
+        return 1.0 / self.frameSpeed
+
+    def getFrameSpeed(self) -> seconds:
+        return self.frameSpeed
+
+    def getExposureTime(self) -> seconds:
+        return self.exposureTime
+
+    def getAverageLatency(self) -> seconds:
+        return self.avgLatency
+
+    def getLatencyStdDev(self) -> seconds:
+        return self.latencyStdDev
+
+    def getContourAreaPercent(self, points: list[typing.Tuple[float, float]]) -> float:
+        return (
+            cv.contourArea(cv.convexHull(np.array(points))) / self.getResArea() * 100.0
+        )
+
+    def getPixelYaw(self, pixelX: float) -> Rotation2d:
+        fx = self.camIntrinsics[0, 0]
+        cx = self.camIntrinsics[0, 2]
+        xOffset = cx - pixelX
+        return Rotation2d(fx, xOffset)
+
+    def getPixelPitch(self, pixelY: float) -> Rotation2d:
+        fy = self.camIntrinsics[1, 1]
+        cy = self.camIntrinsics[1, 2]
+        yOffset = cy - pixelY
+        return Rotation2d(fy, -yOffset)
+
+    def getPixelRot(self, point: typing.Tuple[int, int]) -> Rotation3d:
+        return Rotation3d(
+            0.0,
+            self.getPixelPitch(point[1]).radians(),
+            self.getPixelYaw(point[0]).radians(),
+        )
+
+    def getCorrectedPixelRot(self, point: typing.Tuple[float, float]) -> Rotation3d:
+        fx = self.camIntrinsics[0, 0]
+        cx = self.camIntrinsics[0, 2]
+        xOffset = cx - point[0]
+
+        fy = self.camIntrinsics[1, 1]
+        cy = self.camIntrinsics[1, 2]
+        yOffset = cy - point[1]
+
+        yaw = Rotation2d(fx, xOffset)
+        pitch = Rotation2d(fy / math.cos(math.atan(xOffset / fx)), -yOffset)
+        return Rotation3d(0.0, pitch.radians(), yaw.radians())
+
+    def getHorizFOV(self) -> Rotation2d:
+        left = self.getPixelYaw(0)
+        right = self.getPixelYaw(self.resWidth)
+        return left - right
+
+    def getVertFOV(self) -> Rotation2d:
+        above = self.getPixelPitch(0)
+        below = self.getPixelPitch(self.resHeight)
+        return below - above
+
+    def getDiagFOV(self) -> Rotation2d:
+        return Rotation2d(
+            math.hypot(self.getHorizFOV().radians(), self.getVertFOV().radians())
+        )
+
+    def getVisibleLine(
+        self, camRt: RotTrlTransform3d, a: Translation3d, b: Translation3d
+    ) -> typing.Tuple[float | None, float | None]:
+        relA = camRt.apply(a)
+        relB = camRt.apply(b)
+
+        if relA.X() <= 0.0 and relB.X() <= 0.0:
+            return (None, None)
+
+        av = np.array([relA.X(), relA.Y(), relA.Z()])
+        bv = np.array([relB.X(), relB.Y(), relB.Z()])
+        abv = bv - av
+
+        aVisible = True
+        bVisible = True
+
+        for normal in self.viewplanes:
+            aVisibility = av.dot(normal)
+            if aVisibility < 0:
+                aVisible = False
+
+            bVisibility = bv.dot(normal)
+            if bVisibility < 0:
+                bVisible = False
+            if aVisibility <= 0 and bVisibility <= 0:
+                return (None, None)
+
+        if aVisible and bVisible:
+            return (0.0, 1.0)
+
+        intersections = [float("nan"), float("nan"), float("nan"), float("nan")]
+
+        # Optionally 3x1 vector
+        ipts: typing.List[np.ndarray | None] = [None, None, None, None]
+
+        for i, normal in enumerate(self.viewplanes):
+            a_projn = (av.dot(normal) / normal.dot(normal)) * normal
+
+            if abs(abv.dot(normal)) < 1.0e-5:
+                continue
+            intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn))
+
+            apv = intersections[i] * abv
+            intersectpt = av + apv
+            ipts[i] = intersectpt
+
+            for j in range(1, len(self.viewplanes)):
+                if j == 0:
+                    continue
+                oi = (i + j) % len(self.viewplanes)
+                onormal = self.viewplanes[oi]
+                if intersectpt.dot(onormal) < 0:
+                    intersections[i] = float("nan")
+                    ipts[i] = None
+                    break
+
+            if not ipts[i]:
+                continue
+
+            for j in range(i - 1, 0 - 1):
+                oipt = ipts[j]
+                if not oipt:
+                    continue
+
+                diff = oipt - intersectpt
+                if abs(diff).max() < 1e-4:
+                    intersections[i] = float("nan")
+                    ipts[i] = None
+                    break
+
+        inter1 = float("nan")
+        inter2 = float("nan")
+        for inter in intersections:
+            if not math.isnan(inter):
+                if math.isnan(inter1):
+                    inter1 = inter
+                else:
+                    inter2 = inter
+
+        if not math.isnan(inter2):
+            max_ = max(inter1, inter2)
+            min_ = min(inter1, inter2)
+            if aVisible:
+                min_ = 0
+            if bVisible:
+                max_ = 1
+            return (min_, max_)
+        elif not math.isnan(inter1):
+            if aVisible:
+                return (0, inter1)
+            if bVisible:
+                return (inter1, 1)
+            return (inter1, None)
+        else:
+            return (None, None)
+
+    def estPixelNoise(self, points: np.ndarray) -> np.ndarray:
+        assert points.shape[1] == 1, points.shape
+        assert points.shape[2] == 2, points.shape
+        if self.avgErrorPx == 0 and self.errorStdDevPx == 0:
+            return points
+
+        noisyPts: list[list] = []
+        for p in points:
+            error = np.random.normal(self.avgErrorPx, self.errorStdDevPx, 1)[0]
+            errorAngle = np.random.uniform(-math.pi, math.pi)
+            noisyPts.append(
+                [
+                    [
+                        float(p[0, 0] + error * math.cos(errorAngle)),
+                        float(p[0, 1] + error * math.sin(errorAngle)),
+                    ]
+                ]
+            )
+        retval = np.array(noisyPts, dtype=np.float32)
+        assert points.shape == retval.shape, retval
+        return retval
+
+    def estLatency(self) -> seconds:
+        return max(
+            float(np.random.normal(self.avgLatency, self.latencyStdDev, 1)[0]),
+            0.0,
+        )
+
+    def estSecUntilNextFrame(self) -> seconds:
+        return self.frameSpeed + max(0.0, self.estLatency() - self.frameSpeed)
+
+    @classmethod
+    def PERFECT_90DEG(cls) -> typing.Self:
+        return cls()
+
+    @classmethod
+    def PI4_LIFECAM_320_240(cls) -> typing.Self:
+        prop = cls()
+        prop.setCalibration(
+            320,
+            240,
+            newCamIntrinsics=np.array(
+                [
+                    [328.2733242048587, 0.0, 164.8190261141906],
+                    [0.0, 318.0609794305216, 123.8633838438093],
+                    [0.0, 0.0, 1.0],
+                ]
+            ),
+            newDistCoeffs=np.array(
+                [
+                    [
+                        0.09957946553445934,
+                        -0.9166265114485799,
+                        0.0019519890627236526,
+                        -0.0036071725380870333,
+                        1.5627234622420942,
+                        0,
+                        0,
+                        0,
+                    ]
+                ]
+            ),
+        )
+        prop.setCalibError(0.21, 0.0124)
+        prop.setFPS(30.0)
+        prop.setAvgLatency(30.0e-3)
+        prop.setLatencyStdDev(10.0e-3)
+        return prop
+
+    @classmethod
+    def PI4_LIFECAM_640_480(cls) -> typing.Self:
+        prop = cls()
+        prop.setCalibration(
+            640,
+            480,
+            newCamIntrinsics=np.array(
+                [
+                    [669.1428078983059, 0.0, 322.53377249329213],
+                    [0.0, 646.9843137061716, 241.26567383784163],
+                    [0.0, 0.0, 1.0],
+                ]
+            ),
+            newDistCoeffs=np.array(
+                [
+                    [
+                        0.12788470750464645,
+                        -1.2350335805796528,
+                        0.0024990767286192732,
+                        -0.0026958287600230705,
+                        2.2951386729115537,
+                        0,
+                        0,
+                        0,
+                    ]
+                ]
+            ),
+        )
+        prop.setCalibError(0.26, 0.046)
+        prop.setFPS(15.0)
+        prop.setAvgLatency(65.0e-3)
+        prop.setLatencyStdDev(15.0e-3)
+        return prop
+
+    @classmethod
+    def LL2_640_480(cls) -> typing.Self:
+        prop = cls()
+        prop.setCalibration(
+            640,
+            480,
+            newCamIntrinsics=np.array(
+                [
+                    [511.22843367007755, 0.0, 323.62049380211096],
+                    [0.0, 514.5452336723849, 261.8827920543568],
+                    [0.0, 0.0, 1.0],
+                ]
+            ),
+            newDistCoeffs=np.array(
+                [
+                    [
+                        0.1917469998873756,
+                        -0.5142936883324216,
+                        0.012461562046896614,
+                        0.0014084973492408186,
+                        0.35160648971214437,
+                        0,
+                        0,
+                        0,
+                    ]
+                ]
+            ),
+        )
+        prop.setCalibError(0.25, 0.05)
+        prop.setFPS(15.0)
+        prop.setAvgLatency(35.0e-3)
+        prop.setLatencyStdDev(8.0e-3)
+        return prop
+
+    @classmethod
+    def LL2_960_720(cls) -> typing.Self:
+        prop = cls()
+        prop.setCalibration(
+            960,
+            720,
+            newCamIntrinsics=np.array(
+                [
+                    [769.6873145148892, 0.0, 486.1096609458122],
+                    [0.0, 773.8164483705323, 384.66071662358354],
+                    [0.0, 0.0, 1.0],
+                ]
+            ),
+            newDistCoeffs=np.array(
+                [
+                    [
+                        0.189462064814501,
+                        -0.49903003669627627,
+                        0.007468423590519429,
+                        0.002496885298683693,
+                        0.3443122090208624,
+                        0,
+                        0,
+                        0,
+                    ]
+                ]
+            ),
+        )
+        prop.setCalibError(0.35, 0.10)
+        prop.setFPS(10.0)
+        prop.setAvgLatency(50.0e-3)
+        prop.setLatencyStdDev(15.0e-3)
+        return prop
+
+    @classmethod
+    def LL2_1280_720(cls) -> typing.Self:
+        prop = cls()
+        prop.setCalibration(
+            1280,
+            720,
+            newCamIntrinsics=np.array(
+                [
+                    [1011.3749416937393, 0.0, 645.4955139388737],
+                    [0.0, 1008.5391755084075, 508.32877656020196],
+                    [0.0, 0.0, 1.0],
+                ]
+            ),
+            newDistCoeffs=np.array(
+                [
+                    [
+                        0.13730101577061535,
+                        -0.2904345656989261,
+                        8.32475714507539e-4,
+                        -3.694397782014239e-4,
+                        0.09487962227027584,
+                        0,
+                        0,
+                        0,
+                    ]
+                ]
+            ),
+        )
+        prop.setCalibError(0.37, 0.06)
+        prop.setFPS(7.0)
+        prop.setAvgLatency(60.0e-3)
+        prop.setLatencyStdDev(20.0e-3)
+        return prop
+
+    @classmethod
+    def OV9281_640_480(cls) -> typing.Self:
+        prop = cls()
+        prop.setCalibration(
+            640,
+            480,
+            newCamIntrinsics=np.array(
+                [
+                    [627.1573807284262, 0, 307.79423851611824],
+                    [0, 626.6621595938243, 219.02625533911998],
+                    [0, 0, 1],
+                ]
+            ),
+            newDistCoeffs=np.array(
+                [
+                    [
+                        0.054834081023049625,
+                        -0.15994111706817074,
+                        -0.0017587106009926158,
+                        -0.0014671022483263552,
+                        0.049742166267499596,
+                        0,
+                        0,
+                        0,
+                    ],
+                ]
+            ),
+        )
+        prop.setCalibError(0.25, 0.05)
+        prop.setFPS(30.0)
+        prop.setAvgLatency(60.0e-3)
+        prop.setLatencyStdDev(20.0e-3)
+        return prop
+
+    @classmethod
+    def OV9281_800_600(cls) -> typing.Self:
+        prop = cls()
+        prop.setCalibration(
+            800,
+            600,
+            newCamIntrinsics=np.array(
+                [
+                    [783.9467259105329, 0, 384.7427981451478],
+                    [0, 783.3276994922804, 273.7828191739],
+                    [0, 0, 1],
+                ]
+            ),
+            newDistCoeffs=np.array(
+                [
+                    [
+                        0.054834081023049625,
+                        -0.15994111706817074,
+                        -0.0017587106009926158,
+                        -0.0014671022483263552,
+                        0.049742166267499596,
+                        0,
+                        0,
+                        0,
+                    ],
+                ]
+            ),
+        )
+        prop.setCalibError(0.25, 0.05)
+        prop.setFPS(25.0)
+        prop.setAvgLatency(60.0e-3)
+        prop.setLatencyStdDev(20.0e-3)
+        return prop
+
+    @classmethod
+    def OV9281_1280_720(cls) -> typing.Self:
+        prop = cls()
+        prop.setCalibration(
+            1280,
+            720,
+            newCamIntrinsics=np.array(
+                [
+                    [940.7360710926395, 0, 615.5884770322365],
+                    [0, 939.9932393907364, 328.53938300868],
+                    [0, 0, 1],
+                ]
+            ),
+            newDistCoeffs=np.array(
+                [
+                    [
+                        0.054834081023049625,
+                        -0.15994111706817074,
+                        -0.0017587106009926158,
+                        -0.0014671022483263552,
+                        0.049742166267499596,
+                        0,
+                        0,
+                        0,
+                    ],
+                ]
+            ),
+        )
+        prop.setCalibError(0.25, 0.05)
+        prop.setFPS(15.0)
+        prop.setAvgLatency(60.0e-3)
+        prop.setLatencyStdDev(20.0e-3)
+        return prop
+
+    @classmethod
+    def OV9281_1920_1080(cls) -> typing.Self:
+        prop = cls()
+        prop.setCalibration(
+            1920,
+            1080,
+            newCamIntrinsics=np.array(
+                [
+                    [1411.1041066389591, 0, 923.3827155483548],
+                    [0, 1409.9898590861046, 492.80907451301994],
+                    [0, 0, 1],
+                ]
+            ),
+            newDistCoeffs=np.array(
+                [
+                    [
+                        0.054834081023049625,
+                        -0.15994111706817074,
+                        -0.0017587106009926158,
+                        -0.0014671022483263552,
+                        0.049742166267499596,
+                        0,
+                        0,
+                        0,
+                    ],
+                ]
+            ),
+        )
+        prop.setCalibError(0.25, 0.05)
+        prop.setFPS(10.0)
+        prop.setAvgLatency(60.0e-3)
+        prop.setLatencyStdDev(20.0e-3)
+        return prop

photon-lib/py/photonlibpy/simulation/videoSimUtil.py

@@ -0,0 +1,2 @@
+class VideoSimUtil:
+    pass

photon-lib/py/photonlibpy/simulation/visionSystemSim.py

@@ -0,0 +1,237 @@
+import typing
+
+import wpilib
+from robotpy_apriltag import AprilTagFieldLayout
+from wpilib import Field2d
+from wpimath.geometry import Pose2d, Pose3d, Transform3d
+
+# TODO(auscompgeek): update import path when RobotPy re-exports are fixed
+from wpimath.interpolation._interpolation import TimeInterpolatablePose3dBuffer
+from wpimath.units import seconds
+
+from ..estimation import TargetModel
+from .photonCameraSim import PhotonCameraSim
+from .visionTargetSim import VisionTargetSim
+
+
+class VisionSystemSim:
+    def __init__(self, visionSystemName: str):
+        self.dbgField: Field2d = Field2d()
+        self.bufferLength: seconds = 1.5
+
+        self.camSimMap: typing.Dict[str, PhotonCameraSim] = {}
+        self.camTrfMap: typing.Dict[PhotonCameraSim, TimeInterpolatablePose3dBuffer] = (
+            {}
+        )
+        self.robotPoseBuffer: TimeInterpolatablePose3dBuffer = (
+            TimeInterpolatablePose3dBuffer(self.bufferLength)
+        )
+        self.targetSets: typing.Dict[str, list[VisionTargetSim]] = {}
+
+        self.tableName: str = "VisionSystemSim-" + visionSystemName
+        wpilib.SmartDashboard.putData(self.tableName + "/Sim Field", self.dbgField)
+
+    def getCameraSim(self, name: str) -> PhotonCameraSim | None:
+        return self.camSimMap.get(name, None)
+
+    def getCameraSims(self) -> list[PhotonCameraSim]:
+        return [*self.camSimMap.values()]
+
+    def addCamera(self, cameraSim: PhotonCameraSim, robotToCamera: Transform3d) -> None:
+        name = cameraSim.getCamera().getName()
+        if name not in self.camSimMap:
+            self.camSimMap[name] = cameraSim
+            self.camTrfMap[cameraSim] = TimeInterpolatablePose3dBuffer(
+                self.bufferLength
+            )
+            self.camTrfMap[cameraSim].addSample(
+                wpilib.Timer.getFPGATimestamp(), Pose3d() + robotToCamera
+            )
+
+    def clearCameras(self) -> None:
+        self.camSimMap.clear()
+        self.camTrfMap.clear()
+
+    def removeCamera(self, cameraSim: PhotonCameraSim) -> bool:
+        name = cameraSim.getCamera().getName()
+        if name in self.camSimMap:
+            del self.camSimMap[name]
+            return True
+        else:
+            return False
+
+    def getRobotToCamera(
+        self,
+        cameraSim: PhotonCameraSim,
+        time: seconds = wpilib.Timer.getFPGATimestamp(),
+    ) -> Transform3d | None:
+        if cameraSim in self.camTrfMap:
+            trfBuffer = self.camTrfMap[cameraSim]
+            sample = trfBuffer.sample(time)
+            if sample is None:
+                return None
+            else:
+                return Transform3d(Pose3d(), sample)
+        else:
+            return None
+
+    def getCameraPose(
+        self,
+        cameraSim: PhotonCameraSim,
+        time: seconds = wpilib.Timer.getFPGATimestamp(),
+    ) -> Pose3d | None:
+        robotToCamera = self.getRobotToCamera(cameraSim, time)
+        if robotToCamera is None:
+            return None
+        else:
+            return self.getRobotPose(time) + robotToCamera
+
+    def adjustCamera(
+        self, cameraSim: PhotonCameraSim, robotToCamera: Transform3d
+    ) -> bool:
+        if cameraSim in self.camTrfMap:
+            self.camTrfMap[cameraSim].addSample(
+                wpilib.Timer.getFPGATimestamp(), Pose3d() + robotToCamera
+            )
+            return True
+        else:
+            return False
+
+    def resetCameraTransforms(self, cameraSim: PhotonCameraSim | None = None) -> None:
+        now = wpilib.Timer.getFPGATimestamp()
+
+        def resetSingleCamera(self, cameraSim: PhotonCameraSim) -> bool:
+            if cameraSim in self.camTrfMap:
+                trfBuffer = self.camTrfMap[cameraSim]
+                lastTrf = Transform3d(Pose3d(), trfBuffer.sample(now))
+                trfBuffer.clear()
+                self.adjustCamera(cameraSim, lastTrf)
+                return True
+            else:
+                return False
+
+        if cameraSim is None:
+            for camera in self.camTrfMap.keys():
+                resetSingleCamera(self, camera)
+        else:
+            resetSingleCamera(self, cameraSim)
+
+    def getVisionTargets(self, targetType: str | None = None) -> list[VisionTargetSim]:
+        if targetType is None:
+            all: list[VisionTargetSim] = []
+            for targets in self.targetSets.values():
+                for target in targets:
+                    all.append(target)
+            return all
+        else:
+            return self.targetSets[targetType]
+
+    def addVisionTargets(
+        self, targets: list[VisionTargetSim], targetType: str = "targets"
+    ) -> None:
+        if targetType not in self.targetSets:
+            self.targetSets[targetType] = targets
+        else:
+            self.targetSets[targetType] += targets
+
+    def addAprilTags(self, layout: AprilTagFieldLayout) -> None:
+        targets: list[VisionTargetSim] = []
+        for tag in layout.getTags():
+            tag_pose = layout.getTagPose(tag.ID)
+            # TODO this was done to make the python gods happy. Confirm that this is desired or if types dont matter
+            assert tag_pose is not None
+            targets.append(
+                VisionTargetSim(tag_pose, TargetModel.AprilTag36h11(), tag.ID)
+            )
+        self.addVisionTargets(targets, "apriltag")
+
+    def clearVisionTargets(self) -> None:
+        self.targetSets.clear()
+
+    def clearAprilTags(self) -> None:
+        self.removeVisionTargetType("apriltag")
+
+    def removeVisionTargetType(self, targetType: str) -> None:
+        del self.targetSets[targetType]
+
+    def removeVisionTargets(
+        self, targets: list[VisionTargetSim]
+    ) -> list[VisionTargetSim]:
+        removedList: list[VisionTargetSim] = []
+        for target in targets:
+            for _, currentTargets in self.targetSets.items():
+                if target in currentTargets:
+                    removedList.append(target)
+                    currentTargets.remove(target)
+        return removedList
+
+    def getRobotPose(
+        self, timestamp: seconds = wpilib.Timer.getFPGATimestamp()
+    ) -> Pose3d:
+        return self.robotPoseBuffer.sample(timestamp)
+
+    def resetRobotPose(self, robotPose: Pose2d | Pose3d) -> None:
+        if type(robotPose) is Pose2d:
+            robotPose = Pose3d(robotPose)
+        assert type(robotPose) is Pose3d
+
+        self.robotPoseBuffer.clear()
+        self.robotPoseBuffer.addSample(wpilib.Timer.getFPGATimestamp(), robotPose)
+
+    def getDebugField(self) -> Field2d:
+        return self.dbgField
+
+    def update(self, robotPose: Pose2d | Pose3d) -> None:
+        if type(robotPose) is Pose2d:
+            robotPose = Pose3d(robotPose)
+        assert type(robotPose) is Pose3d
+
+        for targetType, targets in self.targetSets.items():
+            posesToAdd: list[Pose2d] = []
+            for target in targets:
+                posesToAdd.append(target.getPose().toPose2d())
+            self.dbgField.getObject(targetType).setPoses(posesToAdd)
+
+        now = wpilib.Timer.getFPGATimestamp()
+        self.robotPoseBuffer.addSample(now, robotPose)
+        self.dbgField.setRobotPose(robotPose.toPose2d())
+
+        allTargets: list[VisionTargetSim] = []
+        for targets in self.targetSets.values():
+            for target in targets:
+                allTargets.append(target)
+
+        visTgtPoses2d: list[Pose2d] = []
+        cameraPoses2d: list[Pose2d] = []
+        processed = False
+        for camSim in self.camSimMap.values():
+            optTimestamp = camSim.consumeNextEntryTime()
+            if optTimestamp is None:
+                continue
+            else:
+                processed = True
+
+            timestampNt = optTimestamp
+            latency = camSim.prop.estLatency()
+            timestampCapture = timestampNt * 1.0e-6 - latency
+
+            lateRobotPose = self.getRobotPose(timestampCapture)
+            lateCameraPose = lateRobotPose + self.getRobotToCamera(
+                camSim, timestampCapture
+            )
+            cameraPoses2d.append(lateCameraPose.toPose2d())
+
+            camResult = camSim.process(latency, lateCameraPose, allTargets)
+            camSim.submitProcessedFrame(camResult, timestampNt)
+            for target in camResult.getTargets():
+                trf = target.getBestCameraToTarget()
+                if trf == Transform3d():
+                    continue
+
+                visTgtPoses2d.append(lateCameraPose.transformBy(trf).toPose2d())
+
+        if processed:
+            self.dbgField.getObject("visibleTargetPoses").setPoses(visTgtPoses2d)
+
+        if len(cameraPoses2d) != 0:
+            self.dbgField.getObject("cameras").setPoses(cameraPoses2d)

photon-lib/py/photonlibpy/simulation/visionTargetSim.py

@@ -0,0 +1,50 @@
+import math
+
+from wpimath.geometry import Pose3d, Translation3d
+
+from ..estimation.targetModel import TargetModel
+
+
+class VisionTargetSim:
+    def __init__(self, pose: Pose3d, model: TargetModel, id: int = -1):
+        self.pose: Pose3d = pose
+        self.model: TargetModel = model
+        self.fiducialId: int = id
+        self.objDetClassId: int = -1
+        self.objDetConf: float = -1.0
+
+    def __lt__(self, right) -> bool:
+        return self.pose.translation().norm() < right.pose.translation().norm()
+
+    def __eq__(self, other) -> bool:
+        # Use 1 inch and 1 degree tolerance
+        return (
+            abs(self.pose.translation().X() - other.getPose().translation().X())
+            < 0.0254
+            and abs(self.pose.translation().Y() - other.getPose().translation().Y())
+            < 0.0254
+            and abs(self.pose.translation().Z() - other.getPose().translation().Z())
+            < 0.0254
+            and abs(self.pose.rotation().X() - other.getPose().rotation().X())
+            < math.radians(1)
+            and abs(self.pose.rotation().Y() - other.getPose().rotation().Y())
+            < math.radians(1)
+            and abs(self.pose.rotation().Z() - other.getPose().rotation().Z())
+            < math.radians(1)
+            and self.model.getIsPlanar() == other.getModel().getIsPlanar()
+        )
+
+    def setPose(self, newPose: Pose3d) -> None:
+        self.pose = newPose
+
+    def setModel(self, newModel: TargetModel) -> None:
+        self.model = newModel
+
+    def getPose(self) -> Pose3d:
+        return self.pose
+
+    def getModel(self) -> TargetModel:
+        return self.model
+
+    def getFieldVertices(self) -> list[Translation3d]:
+        return self.model.getFieldVertices(self.pose)

photon-lib/py/photonlibpy/targeting/TargetCorner.py

@@ -1,4 +1,8 @@
 from dataclasses import dataclass
+from typing import TYPE_CHECKING, ClassVar
+
+if TYPE_CHECKING:
+    from .. import generated
 
 
 @dataclass
@@ -6,4 +10,4 @@ class TargetCorner:
     x: float = 0
     y: float = 9
 
-    photonStruct: "TargetCornerSerde" = None
+    photonStruct: ClassVar["generated.TargetCornerSerde"]

photon-lib/py/photonlibpy/targeting/__init__.py

@@ -1,6 +1,6 @@
 # no one but us chickens
 
-from .TargetCorner import TargetCorner  # noqa
 from .multiTargetPNPResult import MultiTargetPNPResult, PnpResult  # noqa
 from .photonPipelineResult import PhotonPipelineMetadata, PhotonPipelineResult  # noqa
 from .photonTrackedTarget import PhotonTrackedTarget  # noqa
+from .TargetCorner import TargetCorner  # noqa

photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py

@@ -1,17 +1,23 @@
 from dataclasses import dataclass, field
+from typing import TYPE_CHECKING, ClassVar
+
 from wpimath.geometry import Transform3d
+
 from ..packet import Packet
 
+if TYPE_CHECKING:
+    from .. import generated
+
 
 @dataclass
 class PnpResult:
     best: Transform3d = field(default_factory=Transform3d)
     alt: Transform3d = field(default_factory=Transform3d)
     ambiguity: float = 0.0
-    bestReprojError: float = 0.0
-    altReprojError: float = 0.0
+    bestReprojErr: float = 0.0
+    altReprojErr: float = 0.0
 
-    photonStruct: "PNPResultSerde" = None
+    photonStruct: ClassVar["generated.PnpResultSerde"]
 
 
 @dataclass
@@ -31,4 +37,4 @@ class MultiTargetPNPResult:
                 self.fiducialIDsUsed.append(fidId)
         return packet
 
-    photonStruct: "MultiTargetPNPResultSerde" = None
+    photonStruct: ClassVar["generated.MultiTargetPNPResultSerde"]

photon-lib/py/photonlibpy/targeting/photonPipelineResult.py

@@ -1,9 +1,12 @@
 from dataclasses import dataclass, field
-from typing import Optional
+from typing import TYPE_CHECKING, ClassVar, Optional
 
 from .multiTargetPNPResult import MultiTargetPNPResult
 from .photonTrackedTarget import PhotonTrackedTarget
 
+if TYPE_CHECKING:
+    from .. import generated
+
 
 @dataclass
 class PhotonPipelineMetadata:
@@ -15,7 +18,9 @@ class PhotonPipelineMetadata:
     # Mirror of the heartbeat entry -- monotonically increasing
     sequenceID: int = -1
 
-    photonStruct: "PhotonPipelineMetadataSerde" = None
+    timeSinceLastPong: int = -1
+
+    photonStruct: ClassVar["generated.PhotonPipelineMetadataSerde"]
 
 
 @dataclass
@@ -27,7 +32,7 @@ class PhotonPipelineResult:
     # Python users beware! We don't currently run a Time Sync Server, so these timestamps are in
     # an arbitrary timebase. This is not true in C++ or Java.
     metadata: PhotonPipelineMetadata = field(default_factory=PhotonPipelineMetadata)
-    multiTagResult: Optional[MultiTargetPNPResult] = None
+    multitagResult: Optional[MultiTargetPNPResult] = None
 
     def getLatencyMillis(self) -> float:
         return (
@@ -55,7 +60,7 @@ class PhotonPipelineResult:
     def hasTargets(self) -> bool:
         return len(self.targets) > 0
 
-    def getBestTarget(self) -> PhotonTrackedTarget:
+    def getBestTarget(self) -> Optional[PhotonTrackedTarget]:
         """
         Returns the best target in this pipeline result. If there are no targets, this method will
         return null. The best target is determined by the target sort mode in the PhotonVision UI.
@@ -64,4 +69,4 @@ class PhotonPipelineResult:
             return None
         return self.getTargets()[0]
 
-    photonStruct: "PhotonPipelineResultSerde" = None
+    photonStruct: ClassVar["generated.PhotonPipelineResultSerde"]

photon-lib/py/photonlibpy/targeting/photonTrackedTarget.py

@@ -1,8 +1,14 @@
 from dataclasses import dataclass, field
+from typing import TYPE_CHECKING, ClassVar
+
 from wpimath.geometry import Transform3d
+
 from ..packet import Packet
 from .TargetCorner import TargetCorner
 
+if TYPE_CHECKING:
+    from .. import generated
+
 
 @dataclass
 class PhotonTrackedTarget:
@@ -13,9 +19,11 @@ class PhotonTrackedTarget:
     fiducialId: int = -1
     bestCameraToTarget: Transform3d = field(default_factory=Transform3d)
     altCameraToTarget: Transform3d = field(default_factory=Transform3d)
-    minAreaRectCorners: list[TargetCorner] | None = None
-    detectedCorners: list[TargetCorner] | None = None
+    minAreaRectCorners: list[TargetCorner] = field(default_factory=list[TargetCorner])
+    detectedCorners: list[TargetCorner] = field(default_factory=list[TargetCorner])
     poseAmbiguity: float = 0.0
+    objDetectId: int = -1
+    objDetectConf: float = 0.0
 
     def getYaw(self) -> float:
         return self.yaw
@@ -35,10 +43,10 @@ class PhotonTrackedTarget:
     def getPoseAmbiguity(self) -> float:
         return self.poseAmbiguity
 
-    def getMinAreaRectCorners(self) -> list[TargetCorner] | None:
+    def getMinAreaRectCorners(self) -> list[TargetCorner]:
         return self.minAreaRectCorners
 
-    def getDetectedCorners(self) -> list[TargetCorner] | None:
+    def getDetectedCorners(self) -> list[TargetCorner]:
         return self.detectedCorners
 
     def getBestCameraToTarget(self) -> Transform3d:
@@ -55,4 +63,4 @@ class PhotonTrackedTarget:
             retList.append(TargetCorner(cx, cy))
         return retList
 
-    photonStruct: "PhotonTrackedTargetSerde" = None
+    photonStruct: ClassVar["generated.PhotonTrackedTargetSerde"]

photon-lib/py/setup.py

@@ -1,5 +1,7 @@
-from setuptools import setup, find_packages
-import subprocess, re
+import re
+import subprocess
+
+from setuptools import find_packages, setup
 
 gitDescribeResult = (
     subprocess.check_output(["git", "describe", "--tags", "--match=v*", "--always"])
@@ -55,10 +57,14 @@ setup(
     packages=find_packages(),
     version=versionString,
     install_requires=[
+        "numpy~=1.25",
         "wpilib<2025,>=2024.0.0b2",
         "robotpy-wpimath<2025,>=2024.0.0b2",
         "robotpy-apriltag<2025,>=2024.0.0b2",
+        "robotpy-cscore<2025,>=2024.0.0.b2",
         "pyntcore<2025,>=2024.0.0b2",
+        "robotpy-opencv;platform_machine=='roborio'",
+        "opencv-python;platform_machine!='roborio'",
     ],
     description=descriptionStr,
     url="https://photonvision.org",

photon-lib/py/test/photonPoseEstimator_test.py

@@ -15,247 +15,260 @@
 ## along with this program.  If not, see <https://www.gnu.org/licenses/>.
 ###############################################################################
 
-# from photonlibpy import MultiTargetPNPResult, PnpResult
-# from photonlibpy import PhotonPipelineResult
-# from photonlibpy import PhotonPoseEstimator, PoseStrategy
-# from photonlibpy import PhotonTrackedTarget, TargetCorner, PhotonPipelineMetadata
-# from robotpy_apriltag import AprilTag, AprilTagFieldLayout
-# from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
+from photonlibpy import PhotonPoseEstimator, PoseStrategy
+from photonlibpy.targeting import (
+    PhotonPipelineMetadata,
+    PhotonTrackedTarget,
+    TargetCorner,
+)
+from photonlibpy.targeting.multiTargetPNPResult import MultiTargetPNPResult, PnpResult
+from photonlibpy.targeting.photonPipelineResult import PhotonPipelineResult
+from robotpy_apriltag import AprilTag, AprilTagFieldLayout
+from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
 
 
-# class PhotonCameraInjector:
-#     result: PhotonPipelineResult
+class PhotonCameraInjector:
+    result: PhotonPipelineResult
 
-#     def getLatestResult(self) -> PhotonPipelineResult:
-#         return self.result
+    def getLatestResult(self) -> PhotonPipelineResult:
+        return self.result
 
 
-# def setupCommon() -> AprilTagFieldLayout:
-#     tagList = []
-#     tagPoses = (
-#         Pose3d(3, 3, 3, Rotation3d()),
-#         Pose3d(5, 5, 5, Rotation3d()),
-#     )
-#     for id_, pose in enumerate(tagPoses):
-#         aprilTag = AprilTag()
-#         aprilTag.ID = id_
-#         aprilTag.pose = pose
-#         tagList.append(aprilTag)
+def setupCommon() -> AprilTagFieldLayout:
+    tagList = []
+    tagPoses = (
+        Pose3d(3, 3, 3, Rotation3d()),
+        Pose3d(5, 5, 5, Rotation3d()),
+    )
+    for id_, pose in enumerate(tagPoses):
+        aprilTag = AprilTag()
+        aprilTag.ID = id_
+        aprilTag.pose = pose
+        tagList.append(aprilTag)
 
-#     fieldLength = 54 / 3.281  # 54 ft -> meters
-#     fieldWidth = 27 / 3.281  # 24 ft -> meters
+    fieldLength = 54 / 3.281  # 54 ft -> meters
+    fieldWidth = 27 / 3.281  # 24 ft -> meters
 
-#     return AprilTagFieldLayout(tagList, fieldLength, fieldWidth)
+    return AprilTagFieldLayout(tagList, fieldLength, fieldWidth)
 
 
-# def test_lowestAmbiguityStrategy():
-#     aprilTags = setupCommon()
+def test_lowestAmbiguityStrategy():
+    aprilTags = setupCommon()
 
-#     cameraOne = PhotonCameraInjector()
-#     cameraOne.result = PhotonPipelineResult(
-#         11 * 1e6,
-#         [
-#             PhotonTrackedTarget(
-#                 3.0,
-#                 -4.0,
-#                 9.0,
-#                 4.0,
-#                 0,
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 0.7,
-#             ),
-#             PhotonTrackedTarget(
-#                 3.0,
-#                 -4.0,
-#                 9.1,
-#                 6.7,
-#                 1,
-#                 Transform3d(Translation3d(4, 2, 3), Rotation3d(0, 0, 0)),
-#                 Transform3d(Translation3d(4, 2, 3), Rotation3d(1, 5, 3)),
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 0.3,
-#             ),
-#             PhotonTrackedTarget(
-#                 9.0,
-#                 -2.0,
-#                 19.0,
-#                 3.0,
-#                 0,
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 0.4,
-#             ),
-#         ],
-#         None,
-#         metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0),
-#     )
+    cameraOne = PhotonCameraInjector()
+    cameraOne.result = PhotonPipelineResult(
+        int(11 * 1e6),
+        [
+            PhotonTrackedTarget(
+                3.0,
+                -4.0,
+                9.0,
+                4.0,
+                0,
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                0.7,
+            ),
+            PhotonTrackedTarget(
+                3.0,
+                -4.0,
+                9.1,
+                6.7,
+                1,
+                Transform3d(Translation3d(4, 2, 3), Rotation3d(0, 0, 0)),
+                Transform3d(Translation3d(4, 2, 3), Rotation3d(1, 5, 3)),
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                0.3,
+            ),
+            PhotonTrackedTarget(
+                9.0,
+                -2.0,
+                19.0,
+                3.0,
+                0,
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                0.4,
+            ),
+        ],
+        metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0),
+        multitagResult=None,
+    )
 
-#     estimator = PhotonPoseEstimator(
-#         aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
-#     )
+    estimator = PhotonPoseEstimator(
+        aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
+    )
 
-#     estimatedPose = estimator.update()
-#     pose = estimatedPose.estimatedPose
+    estimatedPose = estimator.update()
 
-#     assertEquals(11 - 0.002, estimatedPose.timestampSeconds, 1e-3)
-#     assertEquals(1, pose.x, 0.01)
-#     assertEquals(3, pose.y, 0.01)
-#     assertEquals(2, pose.z, 0.01)
+    assert estimatedPose is not None
+
+    pose = estimatedPose.estimatedPose
+
+    assertEquals(11 - 0.002, estimatedPose.timestampSeconds, 1e-3)
+    assertEquals(1, pose.x, 0.01)
+    assertEquals(3, pose.y, 0.01)
+    assertEquals(2, pose.z, 0.01)
 
 
-# def test_multiTagOnCoprocStrategy():
-#     cameraOne = PhotonCameraInjector()
-#     cameraOne.result = PhotonPipelineResult(
-#         11 * 1e6,
-#         # There needs to be at least one target present for pose estimation to work
-#         # Doesn't matter which/how many targets for this test
-#         [
-#             PhotonTrackedTarget(
-#                 3.0,
-#                 -4.0,
-#                 9.0,
-#                 4.0,
-#                 0,
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 0.7,
-#             )
-#         ],
-#         multiTagResult=MultiTargetPNPResult(
-#             PnpResult(True, Transform3d(1, 3, 2, Rotation3d()))
-#         ),
-#         metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0),
-#     )
+def test_multiTagOnCoprocStrategy():
+    cameraOne = PhotonCameraInjector()
+    cameraOne.result = PhotonPipelineResult(
+        int(11 * 1e6),
+        # There needs to be at least one target present for pose estimation to work
+        # Doesn't matter which/how many targets for this test
+        [
+            PhotonTrackedTarget(
+                3.0,
+                -4.0,
+                9.0,
+                4.0,
+                0,
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                0.7,
+            )
+        ],
+        metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0),
+        multitagResult=MultiTargetPNPResult(
+            PnpResult(Transform3d(1, 3, 2, Rotation3d()))
+        ),
+    )
 
-#     estimator = PhotonPoseEstimator(
-#         AprilTagFieldLayout(),
-#         PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
-#         cameraOne,
-#         Transform3d(),
-#     )
+    estimator = PhotonPoseEstimator(
+        AprilTagFieldLayout(),
+        PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
+        cameraOne,
+        Transform3d(),
+    )
 
-#     estimatedPose = estimator.update()
-#     pose = estimatedPose.estimatedPose
+    estimatedPose = estimator.update()
 
-#     assertEquals(11 - 2e-3, estimatedPose.timestampSeconds, 1e-3)
-#     assertEquals(1, pose.x, 0.01)
-#     assertEquals(3, pose.y, 0.01)
-#     assertEquals(2, pose.z, 0.01)
+    assert estimatedPose is not None
+
+    pose = estimatedPose.estimatedPose
+
+    assertEquals(11 - 2e-3, estimatedPose.timestampSeconds, 1e-3)
+    assertEquals(1, pose.x, 0.01)
+    assertEquals(3, pose.y, 0.01)
+    assertEquals(2, pose.z, 0.01)
 
 
-# def test_cacheIsInvalidated():
-#     aprilTags = setupCommon()
+def test_cacheIsInvalidated():
+    aprilTags = setupCommon()
 
-#     cameraOne = PhotonCameraInjector()
-#     result = PhotonPipelineResult(
-#         20 * 1e6,
-#         [
-#             PhotonTrackedTarget(
-#                 3.0,
-#                 -4.0,
-#                 9.0,
-#                 4.0,
-#                 0,
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 [
-#                     TargetCorner(1, 2),
-#                     TargetCorner(3, 4),
-#                     TargetCorner(5, 6),
-#                     TargetCorner(7, 8),
-#                 ],
-#                 0.7,
-#             )
-#         ],
-#         metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0),
-#     )
+    cameraOne = PhotonCameraInjector()
+    result = PhotonPipelineResult(
+        int(20 * 1e6),
+        [
+            PhotonTrackedTarget(
+                3.0,
+                -4.0,
+                9.0,
+                4.0,
+                0,
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                [
+                    TargetCorner(1, 2),
+                    TargetCorner(3, 4),
+                    TargetCorner(5, 6),
+                    TargetCorner(7, 8),
+                ],
+                0.7,
+            )
+        ],
+        metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0),
+    )
 
-#     estimator = PhotonPoseEstimator(
-#         aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
-#     )
+    estimator = PhotonPoseEstimator(
+        aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
+    )
 
-#     # Empty result, expect empty result
-#     cameraOne.result = PhotonPipelineResult(0)
-#     estimatedPose = estimator.update()
-#     assert estimatedPose is None
+    # Empty result, expect empty result
+    cameraOne.result = PhotonPipelineResult(0)
+    estimatedPose = estimator.update()
+    assert estimatedPose is None
 
-#     # Set actual result
-#     cameraOne.result = result
-#     estimatedPose = estimator.update()
-#     assert estimatedPose is not None
-#     assertEquals(20, estimatedPose.timestampSeconds, 0.01)
-#     assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
+    # Set actual result
+    cameraOne.result = result
+    estimatedPose = estimator.update()
+    assert estimatedPose is not None
+    assertEquals(20, estimatedPose.timestampSeconds, 0.01)
+    assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
 
-#     # And again -- pose cache should mean this is empty
-#     cameraOne.result = result
-#     estimatedPose = estimator.update()
-#     assert estimatedPose is None
-#     # Expect the old timestamp to still be here
-#     assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
+    # And again -- pose cache should mean this is empty
+    cameraOne.result = result
+    estimatedPose = estimator.update()
+    assert estimatedPose is None
+    # Expect the old timestamp to still be here
+    assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
 
-#     # Set new field layout -- right after, the pose cache timestamp should be -1
-#     estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0)
-#     assertEquals(-1, estimator._poseCacheTimestampSeconds)
-#     # Update should cache the current timestamp (20) again
-#     cameraOne.result = result
-#     estimatedPose = estimator.update()
-#     assertEquals(20, estimatedPose.timestampSeconds, 0.01)
-#     assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
+    # Set new field layout -- right after, the pose cache timestamp should be -1
+    estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0)
+    assertEquals(-1, estimator._poseCacheTimestampSeconds)
+    # Update should cache the current timestamp (20) again
+    cameraOne.result = result
+    estimatedPose = estimator.update()
+
+    assert estimatedPose is not None
+
+    assertEquals(20, estimatedPose.timestampSeconds, 0.01)
+    assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
 
 
-# def assertEquals(expected, actual, epsilon=0.0):
-#     assert abs(expected - actual) <= epsilon
+def assertEquals(expected, actual, epsilon=0.0):
+    assert abs(expected - actual) <= epsilon

photon-lib/py/test/photonlibpy_test.py

@@ -16,8 +16,9 @@
 ###############################################################################
 
 from time import sleep
-from photonlibpy import PhotonCamera
+
 import ntcore
+from photonlibpy import PhotonCamera
 from photonlibpy.photonCamera import setVersionCheckEnabled
 
 

photon-lib/py/test/visionSystemSim_test.py

@@ -0,0 +1,484 @@
+import math
+
+import ntcore as nt
+import pytest
+from photonlibpy.estimation import TargetModel, VisionEstimation
+from photonlibpy.photonCamera import PhotonCamera, setVersionCheckEnabled
+from photonlibpy.simulation import PhotonCameraSim, VisionSystemSim, VisionTargetSim
+from robotpy_apriltag import AprilTag, AprilTagFieldLayout
+from wpimath.geometry import (
+    Pose2d,
+    Pose3d,
+    Rotation2d,
+    Rotation3d,
+    Transform3d,
+    Translation2d,
+    Translation3d,
+)
+from wpimath.units import feetToMeters, meters
+
+
+@pytest.fixture(autouse=True)
+def setupCommon() -> None:
+
+    nt.NetworkTableInstance.getDefault().startServer()
+    setVersionCheckEnabled(False)
+
+
+def test_VisibilityCupidShuffle():
+
+    targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi))
+
+    visionSysSim = VisionSystemSim("Test")
+    camera = PhotonCamera("camera")
+    cameraSim = PhotonCameraSim(camera)
+    visionSysSim.addCamera(cameraSim, Transform3d())
+
+    cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
+
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(targetPose, TargetModel(width=1.0, height=1.0), 4774)]
+    )
+
+    # To the right, to the right
+    robotPose = Pose2d(Translation2d(5.0, 0.0), Rotation2d.fromDegrees(-70.0))
+    visionSysSim.update(robotPose)
+    assert not camera.getLatestResult().hasTargets()
+
+    # To the right, to the right
+    robotPose = Pose2d(Translation2d(5.0, 0.0), Rotation2d.fromDegrees(-95.0))
+    visionSysSim.update(robotPose)
+    assert not camera.getLatestResult().hasTargets()
+
+    # To the left, to the left
+    robotPose = Pose2d(Translation2d(5.0, 0.0), Rotation2d.fromDegrees(90.0))
+    visionSysSim.update(robotPose)
+    assert not camera.getLatestResult().hasTargets()
+
+    # To the left, to the left
+    robotPose = Pose2d(Translation2d(5.0, 0.0), Rotation2d.fromDegrees(65.0))
+    visionSysSim.update(robotPose)
+    assert not camera.getLatestResult().hasTargets()
+
+    # Now kick, now kick
+    robotPose = Pose2d(Translation2d(2.0, 0.0), Rotation2d.fromDegrees(5.0))
+    visionSysSim.update(robotPose)
+    assert camera.getLatestResult().hasTargets()
+
+    # Now kick, now kick
+    robotPose = Pose2d(Translation2d(2.0, 0.0), Rotation2d.fromDegrees(-5.0))
+    visionSysSim.update(robotPose)
+    assert camera.getLatestResult().hasTargets()
+
+    # Now walk it by yourself
+    robotPose = Pose2d(Translation2d(2.0, 0.0), Rotation2d.fromDegrees(-179.0))
+    visionSysSim.update(robotPose)
+    assert not camera.getLatestResult().hasTargets()
+
+    # Now walk it by yourself
+    visionSysSim.adjustCamera(
+        cameraSim, Transform3d(Translation3d(), Rotation3d(0, 0, math.pi))
+    )
+    visionSysSim.update(robotPose)
+    assert camera.getLatestResult().hasTargets()
+
+
+def test_NotVisibleVert1():
+
+    targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi))
+
+    visionSysSim = VisionSystemSim("Test")
+    camera = PhotonCamera("camera")
+    cameraSim = PhotonCameraSim(camera)
+    visionSysSim.addCamera(cameraSim, Transform3d())
+
+    cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
+
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(targetPose, TargetModel(width=3.0, height=3.0), 4774)]
+    )
+
+    robotPose = Pose2d(Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0))
+
+    visionSysSim.update(robotPose)
+    assert camera.getLatestResult().hasTargets()
+
+    visionSysSim.adjustCamera(
+        cameraSim,
+        Transform3d(Translation3d(0.0, 0.0, 5000.0), Rotation3d(0.0, 0.0, math.pi)),
+    )
+    visionSysSim.update(robotPose)
+    assert not camera.getLatestResult().hasTargets()
+
+
+def test_NotVisibleVert2():
+
+    targetPose = Pose3d(Translation3d(15.98, 0.0, 2.0), Rotation3d(0, 0, math.pi))
+
+    robotToCamera = Transform3d(
+        Translation3d(0.0, 0.0, 1.0), Rotation3d(0.0, -math.pi / 4.0, 0.0)
+    )
+
+    visionSysSim = VisionSystemSim("Test")
+    camera = PhotonCamera("camera")
+    cameraSim = PhotonCameraSim(camera)
+    visionSysSim.addCamera(cameraSim, robotToCamera)
+
+    cameraSim.prop.setCalibration(4774, 4774, fovDiag=Rotation2d.fromDegrees(80.0))
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(targetPose, TargetModel(width=0.5, height=0.5), 4774)]
+    )
+
+    robotPose = Pose2d(Translation2d(13.98, 0.0), Rotation2d.fromDegrees(5.0))
+    visionSysSim.update(robotPose)
+    assert camera.getLatestResult().hasTargets()
+
+    robotPose = Pose2d(Translation2d(0.0, 0.0), Rotation2d.fromDegrees(5.0))
+    visionSysSim.update(robotPose)
+    assert not camera.getLatestResult().hasTargets()
+
+
+def test_NotVisibleTargetSize():
+    targetPose = Pose3d(Translation3d(15.98, 0.0, 1.0), Rotation3d(0, 0, math.pi))
+
+    visionSysSim = VisionSystemSim("Test")
+    camera = PhotonCamera("camera")
+    cameraSim = PhotonCameraSim(camera)
+    visionSysSim.addCamera(cameraSim, Transform3d())
+
+    cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
+    cameraSim.setMinTargetAreaPixels(20.0)
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(targetPose, TargetModel(width=0.1, height=0.1), 4774)]
+    )
+
+    robotPose = Pose2d(Translation2d(12.0, 0.0), Rotation2d.fromDegrees(5.0))
+    visionSysSim.update(robotPose)
+    assert camera.getLatestResult().hasTargets()
+
+    robotPose = Pose2d(Translation2d(0.0, 0.0), Rotation2d.fromDegrees(5.0))
+    visionSysSim.update(robotPose)
+    assert not camera.getLatestResult().hasTargets()
+
+
+def test_NotVisibleTooFarLeds():
+
+    targetPose = Pose3d(Translation3d(15.98, 0.0, 1.0), Rotation3d(0, 0, math.pi))
+
+    visionSysSim = VisionSystemSim("Test")
+    camera = PhotonCamera("camera")
+    cameraSim = PhotonCameraSim(camera)
+    visionSysSim.addCamera(cameraSim, Transform3d())
+
+    cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
+    cameraSim.setMinTargetAreaPixels(1.0)
+    cameraSim.setMaxSightRange(10.0)
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(targetPose, TargetModel(width=1.0, height=1.0), 4774)]
+    )
+
+    robotPose = Pose2d(Translation2d(10.0, 0.0), Rotation2d.fromDegrees(5.0))
+    visionSysSim.update(robotPose)
+    assert camera.getLatestResult().hasTargets()
+
+    robotPose = Pose2d(Translation2d(0.0, 0.0), Rotation2d.fromDegrees(5.0))
+    visionSysSim.update(robotPose)
+    assert not camera.getLatestResult().hasTargets()
+
+
+@pytest.mark.parametrize(
+    "expected_yaw", [-10.0, -5.0, -2.0, -1.0, 0.0, 5.0, 7.0, 10.23]
+)
+def test_YawAngles(expected_yaw):
+
+    targetPose = Pose3d(
+        Translation3d(15.98, 0.0, 1.0), Rotation3d(0.0, 0.0, 3.0 * math.pi / 4.0)
+    )
+
+    visionSysSim = VisionSystemSim("Test")
+    camera = PhotonCamera("camera")
+    cameraSim = PhotonCameraSim(camera)
+
+    visionSysSim.addCamera(cameraSim, Transform3d())
+    cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
+    cameraSim.setMinTargetAreaPixels(0.0)
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(targetPose, TargetModel(width=0.5, height=0.5), 4774)]
+    )
+
+    robotPose = Pose2d(Translation2d(10.0, 0.0), Rotation2d.fromDegrees(expected_yaw))
+    visionSysSim.update(robotPose)
+
+    result = camera.getLatestResult()
+
+    assert result.hasTargets()
+    assert result.getBestTarget().getYaw() == pytest.approx(expected_yaw, abs=0.25)
+
+
+@pytest.mark.parametrize(
+    "expected_pitch", [-10.0, -5.0, -2.0, -1.0, 0.0, 5.0, 7.0, 10.23]
+)
+def test_PitchAngles(expected_pitch):
+
+    targetPose = Pose3d(
+        Translation3d(15.98, 0.0, 0.0), Rotation3d(0, 0, 3.0 * math.pi / 4.0)
+    )
+    robotPose = Pose2d(
+        Translation2d(10.0, 0.0), Rotation2d.fromDegrees(-expected_pitch)
+    )
+    visionSysSim = VisionSystemSim("Test")
+    camera = PhotonCamera("camera")
+    cameraSim = PhotonCameraSim(camera)
+    visionSysSim.addCamera(cameraSim, Transform3d())
+    cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(120.0))
+    cameraSim.setMinTargetAreaPixels(0.0)
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(targetPose, TargetModel(width=0.5, height=0.5), 4774)]
+    )
+    visionSysSim.adjustCamera(
+        cameraSim,
+        Transform3d(
+            Translation3d(), Rotation3d(0.0, math.radians(expected_pitch), 0.0)
+        ),
+    )
+    visionSysSim.update(robotPose)
+
+    result = camera.getLatestResult()
+
+    assert result.hasTargets()
+    assert result.getBestTarget().getPitch() == pytest.approx(expected_pitch, abs=0.25)
+
+
+@pytest.mark.parametrize(
+    "distParam, pitchParam, heightParam",
+    [
+        (5, -15.98, 0),
+        (6, -15.98, 1),
+        (10, -15.98, 0),
+        (15, -15.98, 2),
+        (19.95, -15.98, 0),
+        (20, -15.98, 0),
+        (5, -42, 1),
+        (6, -42, 0),
+        (10, -42, 2),
+        (15, -42, 0.5),
+        (19.42, -15.98, 0),
+        (20, -42, 0),
+        (5, -55, 2),
+        (6, -55, 0),
+        (10, -54, 2.2),
+        (15, -53, 0),
+        (19.52, -15.98, 1.1),
+    ],
+)
+def test_distanceCalc(distParam, pitchParam, heightParam):
+    distParam = feetToMeters(distParam)
+    pitchParam = math.radians(pitchParam)
+    heightParam = feetToMeters(heightParam)
+
+    targetPose = Pose3d(
+        Translation3d(15.98, 0.0, 1.0), Rotation3d(0.0, 0.0, 0.98 * math.pi)
+    )
+    robotPose = Pose3d(Translation3d(15.98 - distParam, 0.0, 0.0), Rotation3d())
+    robotToCamera = Transform3d(
+        Translation3d(0.0, 0.0, heightParam), Rotation3d(0.0, pitchParam, 0.0)
+    )
+
+    visionSysSim = VisionSystemSim(
+        "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife"
+    )
+    camera = PhotonCamera("camera")
+    cameraSim = PhotonCameraSim(camera)
+
+    visionSysSim.addCamera(cameraSim, Transform3d())
+    cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(160.0))
+    cameraSim.setMinTargetAreaPixels(0.0)
+    visionSysSim.adjustCamera(cameraSim, robotToCamera)
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(targetPose, TargetModel(width=0.5, height=0.5), 4774)]
+    )
+    visionSysSim.update(robotPose)
+
+    result = camera.getLatestResult()
+
+    assert result.hasTargets()
+
+    target = result.getBestTarget()
+
+    assert target.getYaw() == pytest.approx(0.0, abs=0.5)
+
+    # TODO Enable when PhotonUtils is ported
+    # dist = PhotonUtils.calculateDistanceToTarget(
+    #    robotToCamera.Z(), targetPose.Z(), -pitchParam, math.degrees(target.getPitch())
+    # )
+    # assert dist == pytest.approx(distParam, abs=0.25)
+
+
+def test_MultipleTargets():
+    targetPoseL = Pose3d(Translation3d(15.98, 2.0, 0.0), Rotation3d(0.0, 0.0, math.pi))
+    targetPoseC = Pose3d(Translation3d(15.98, 0.0, 0.0), Rotation3d(0.0, 0.0, math.pi))
+    targetPoseR = Pose3d(Translation3d(15.98, -2.0, 0.0), Rotation3d(0.0, 0.0, math.pi))
+
+    visionSysSim = VisionSystemSim("Test")
+    camera = PhotonCamera("camera")
+    cameraSim = PhotonCameraSim(camera)
+    visionSysSim.addCamera(cameraSim, Transform3d())
+    cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(80.0))
+    cameraSim.setMinTargetAreaPixels(20.0)
+
+    visionSysSim.addVisionTargets(
+        [
+            VisionTargetSim(
+                targetPoseL.transformBy(
+                    Transform3d(Translation3d(0, 0, 0), Rotation3d())
+                ),
+                TargetModel.AprilTag16h5(),
+                1,
+            ),
+            VisionTargetSim(
+                targetPoseC.transformBy(
+                    Transform3d(Translation3d(0, 0, 0), Rotation3d())
+                ),
+                TargetModel.AprilTag16h5(),
+                2,
+            ),
+            VisionTargetSim(
+                targetPoseR.transformBy(
+                    Transform3d(Translation3d(0, 0, 0), Rotation3d())
+                ),
+                TargetModel.AprilTag16h5(),
+                3,
+            ),
+            VisionTargetSim(
+                targetPoseL.transformBy(
+                    Transform3d(Translation3d(0, 0, 1), Rotation3d())
+                ),
+                TargetModel.AprilTag16h5(),
+                4,
+            ),
+            VisionTargetSim(
+                targetPoseC.transformBy(
+                    Transform3d(Translation3d(0, 0, 1), Rotation3d())
+                ),
+                TargetModel.AprilTag16h5(),
+                5,
+            ),
+            VisionTargetSim(
+                targetPoseR.transformBy(
+                    Transform3d(Translation3d(0, 0, 1), Rotation3d())
+                ),
+                TargetModel.AprilTag16h5(),
+                6,
+            ),
+            VisionTargetSim(
+                targetPoseL.transformBy(
+                    Transform3d(Translation3d(0, 0, 0.5), Rotation3d())
+                ),
+                TargetModel.AprilTag16h5(),
+                7,
+            ),
+            VisionTargetSim(
+                targetPoseC.transformBy(
+                    Transform3d(Translation3d(0, 0, 0.5), Rotation3d())
+                ),
+                TargetModel.AprilTag16h5(),
+                8,
+            ),
+            VisionTargetSim(
+                targetPoseL.transformBy(
+                    Transform3d(Translation3d(0, 0, 0.75), Rotation3d())
+                ),
+                TargetModel.AprilTag16h5(),
+                9,
+            ),
+            VisionTargetSim(
+                targetPoseR.transformBy(
+                    Transform3d(Translation3d(0, 0, 0.75), Rotation3d())
+                ),
+                TargetModel.AprilTag16h5(),
+                10,
+            ),
+            VisionTargetSim(
+                targetPoseL.transformBy(
+                    Transform3d(Translation3d(0, 0, 0.25), Rotation3d())
+                ),
+                TargetModel.AprilTag16h5(),
+                11,
+            ),
+        ]
+    )
+    robotPose = Pose2d(Translation2d(6.0, 0.0), Rotation2d.fromDegrees(0.25))
+    visionSysSim.update(robotPose)
+    res = camera.getLatestResult()
+    assert res.hasTargets()
+    tgtList = res.getTargets()
+    assert len(tgtList) == 11
+
+
+def test_PoseEstimation():
+    visionSysSim = VisionSystemSim("Test")
+    camera = PhotonCamera("camera")
+    cameraSim = PhotonCameraSim(camera)
+    visionSysSim.addCamera(cameraSim, Transform3d())
+    cameraSim.prop.setCalibration(640, 480, fovDiag=Rotation2d.fromDegrees(90.0))
+    cameraSim.setMinTargetAreaPixels(20.0)
+
+    tagList: list[AprilTag] = []
+    at0 = AprilTag()
+    at0.ID = 0
+    at0.pose = Pose3d(12.0, 3.0, 1.0, Rotation3d(0.0, 0.0, math.pi))
+    tagList.append(at0)
+    at1 = AprilTag()
+    at1.ID = 1
+    at1.pose = Pose3d(12.0, 1.0, -1.0, Rotation3d(0.0, 0.0, math.pi))
+    tagList.append(at1)
+    at2 = AprilTag()
+    at2.ID = 2
+    at2.pose = Pose3d(11.0, 0.0, 2.0, Rotation3d(0.0, 0.0, math.pi))
+    tagList.append(at2)
+
+    fieldLength: meters = 54.0
+    fieldWidth: meters = 27.0
+    layout = AprilTagFieldLayout(tagList, fieldLength, fieldWidth)
+    robotPose = Pose2d(Translation2d(5.0, 1.0), Rotation2d.fromDegrees(5.0))
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(tagList[0].pose, TargetModel.AprilTag16h5(), 0)]
+    )
+
+    visionSysSim.update(robotPose)
+
+    camEigen = cameraSim.prop.getIntrinsics()
+    distEigen = cameraSim.prop.getDistCoeffs()
+
+    camResults = camera.getLatestResult()
+    targets = camResults.getTargets()
+    results = VisionEstimation.estimateCamPosePNP(
+        camEigen, distEigen, targets, layout, TargetModel.AprilTag16h5()
+    )
+    assert results is not None
+    pose: Pose3d = Pose3d() + results.best
+    assert pose.X() == pytest.approx(5.0, abs=0.01)
+    assert pose.Y() == pytest.approx(1.0, abs=0.01)
+    assert pose.Z() == pytest.approx(0.0, abs=0.01)
+    assert pose.rotation().Z() == pytest.approx(math.radians(5.0), abs=0.01)
+
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(tagList[1].pose, TargetModel.AprilTag16h5(), 1)]
+    )
+    visionSysSim.addVisionTargets(
+        [VisionTargetSim(tagList[2].pose, TargetModel.AprilTag16h5(), 2)]
+    )
+    visionSysSim.update(robotPose)
+
+    camResults2 = camera.getLatestResult()
+    targets2 = camResults2.getTargets()
+    results2 = VisionEstimation.estimateCamPosePNP(
+        camEigen, distEigen, targets2, layout, TargetModel.AprilTag16h5()
+    )
+    assert results2 is not None
+    pose2 = Pose3d() + results2.best
+
+    assert pose2.X() == pytest.approx(robotPose.X(), abs=0.01)
+    assert pose2.Y() == pytest.approx(robotPose.Y(), abs=0.01)
+    assert pose2.Z() == pytest.approx(0.0, abs=0.01)
+    assert pose2.rotation().Z() == pytest.approx(math.radians(5.0), abs=0.01)

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

@@ -60,10 +60,21 @@ inline constexpr std::string_view bfw =
     ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
     "\n\n";
 
-// bit of a hack -- start a TimeSync server on port 5810 (hard-coded)
-static std::mutex g_timeSyncServerMutex;
-static bool g_timeSyncServerStarted;
-static wpi::tsp::TimeSyncServer timesyncServer{5810};
+// bit of a hack -- start a TimeSync server on port 5810 (hard-coded). We want
+// to avoid calling this from static initialization
+static void InitTspServer() {
+  // We dont impose requirements about not calling the PhotonCamera constructor
+  // from different threads, so i guess we need this?
+  static std::mutex g_timeSyncServerMutex;
+  static bool g_timeSyncServerStarted{false};
+  static wpi::tsp::TimeSyncServer timesyncServer{5810};
+
+  std::lock_guard lock{g_timeSyncServerMutex};
+  if (!g_timeSyncServerStarted) {
+    timesyncServer.Start();
+    g_timeSyncServerStarted = true;
+  }
+}
 
 namespace photon {
 
@@ -117,13 +128,10 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
   HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount);
   InstanceCount++;
 
-  {
-    std::lock_guard lock{g_timeSyncServerMutex};
-    if (!g_timeSyncServerStarted) {
-      timesyncServer.Start();
-      g_timeSyncServerStarted = true;
-    }
-  }
+  // The Robot class is actually created here:
+  // https://github.com/wpilibsuite/allwpilib/blob/811b1309683e930a1ce69fae818f943ff161b7a5/wpilibc/src/main/native/include/frc/RobotBase.h#L33
+  // so we should be fine to call this from the ctor
+  InitTspServer();
 }
 
 PhotonCamera::PhotonCamera(const std::string_view cameraName)

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

@@ -471,3 +471,79 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
   ASSERT_NEAR(units::degree_t{5}.convert<units::radians>().to<double>(),
               pose2.Rotation().Z().to<double>(), 0.01);
 }
+
+TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) {
+  frc::Transform3d robotToCamera{frc::Translation3d{6_in, 6_in, 6_in},
+                                 frc::Rotation3d{0_deg, -30_deg, 25.5_deg}};
+
+  photon::VisionSystemSim visionSysSim{"Test"};
+  photon::PhotonCamera camera{"cameraRotated"};
+  photon::PhotonCameraSim cameraSim{&camera};
+  visionSysSim.AddCamera(&cameraSim, robotToCamera);
+  cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg});
+  cameraSim.SetMinTargetAreaPixels(20.0);
+
+  std::vector<frc::AprilTag> tagList;
+  tagList.emplace_back(frc::AprilTag{
+      0, frc::Pose3d{12_m, 3_m, 1_m,
+                     frc::Rotation3d{0_rad, 0_rad,
+                                     units::radian_t{std::numbers::pi}}}});
+  tagList.emplace_back(frc::AprilTag{
+      1, frc::Pose3d{12_m, 1_m, -1_m,
+                     frc::Rotation3d{0_rad, 0_rad,
+                                     units::radian_t{std::numbers::pi}}}});
+  tagList.emplace_back(frc::AprilTag{
+      2, frc::Pose3d{11_m, 0_m, 2_m,
+                     frc::Rotation3d{0_rad, 0_rad,
+                                     units::radian_t{std::numbers::pi}}}});
+  units::meter_t fieldLength{54};
+  units::meter_t fieldWidth{27};
+  frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth};
+  frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{-5_deg}};
+  visionSysSim.AddVisionTargets(
+      {photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag36h11, 0}});
+  visionSysSim.Update(robotPose);
+
+  Eigen::Matrix<double, 3, 3> camEigen = camera.GetCameraMatrix().value();
+  Eigen::Matrix<double, 8, 1> distEigen = camera.GetDistCoeffs().value();
+
+  auto camResults = camera.GetLatestResult();
+  auto targetSpan = camResults.GetTargets();
+  std::vector<photon::PhotonTrackedTarget> targets;
+  for (photon::PhotonTrackedTarget tar : targetSpan) {
+    targets.push_back(tar);
+  }
+  auto results = photon::VisionEstimation::EstimateCamPosePNP(
+      camEigen, distEigen, targets, layout, photon::kAprilTag36h11);
+  ASSERT_TRUE(results);
+  frc::Pose3d pose = frc::Pose3d{} + results->best;
+  pose = pose.TransformBy(robotToCamera.Inverse());
+  ASSERT_NEAR(5, pose.X().to<double>(), 0.01);
+  ASSERT_NEAR(1, pose.Y().to<double>(), 0.01);
+  ASSERT_NEAR(0, pose.Z().to<double>(), 0.01);
+  ASSERT_NEAR(units::degree_t{-5}.convert<units::radians>().to<double>(),
+              pose.Rotation().Z().to<double>(), 0.01);
+
+  visionSysSim.AddVisionTargets(
+      {photon::VisionTargetSim{tagList[1].pose, photon::kAprilTag36h11, 1}});
+  visionSysSim.AddVisionTargets(
+      {photon::VisionTargetSim{tagList[2].pose, photon::kAprilTag36h11, 2}});
+  visionSysSim.Update(robotPose);
+
+  auto camResults2 = camera.GetLatestResult();
+  auto targetSpan2 = camResults2.GetTargets();
+  std::vector<photon::PhotonTrackedTarget> targets2;
+  for (photon::PhotonTrackedTarget tar : targetSpan2) {
+    targets2.push_back(tar);
+  }
+  auto results2 = photon::VisionEstimation::EstimateCamPosePNP(
+      camEigen, distEigen, targets2, layout, photon::kAprilTag36h11);
+  ASSERT_TRUE(results2);
+  frc::Pose3d pose2 = frc::Pose3d{} + results2->best;
+  pose2 = pose2.TransformBy(robotToCamera.Inverse());
+  ASSERT_NEAR(robotPose.X().to<double>(), pose2.X().to<double>(), 0.01);
+  ASSERT_NEAR(robotPose.Y().to<double>(), pose2.Y().to<double>(), 0.01);
+  ASSERT_NEAR(0, pose2.Z().to<double>(), 0.01);
+  ASSERT_NEAR(units::degree_t{-5}.convert<units::radians>().to<double>(),
+              pose2.Rotation().Z().to<double>(), 0.01);
+}

photon-serde/generate_messages.py

@@ -46,6 +46,7 @@ class MessageType(TypedDict):
     # C++ helpers
     cpp_include: str
     # python shim types
+    python_encode_shim: str
     python_decode_shim: str
     # Java import name
     java_import: str

photon-serde/message_data_types.yaml

@@ -5,29 +5,35 @@ bool:
   java_type: bool
   cpp_type: bool
   java_decode_method: decodeBoolean
+  java_encode_shim: encodeBoolean
 int16:
   len: 2
   java_type: short
   cpp_type: int16_t
   java_decode_method: decodeShort
   java_list_decode_method: decodeShortList
+  java_encode_shim: encodeShort
 int32:
   len: 4
   java_type: int
   cpp_type: int32_t
   java_decode_method: decodeInt
+  java_encode_shim: encodeInt
 int64:
   len: 8
   java_type: long
   cpp_type: int64_t
   java_decode_method: decodeLong
+  java_encode_shim: encodeLong
 float32:
   len: 4
   java_type: float
   cpp_type: float
   java_decode_method: decodeFloat
+  java_encode_shim: encodeFloat
 float64:
   len: 8
   java_type: double
   cpp_type: double
   java_decode_method: decodeDouble
+  java_encode_shim: encodeDouble

photon-serde/messages.yaml

@@ -17,6 +17,7 @@
   cpp_type: frc::Transform3d
   cpp_include: "<frc/geometry/Transform3d.h>"
   python_decode_shim: packet.decodeTransform
+  python_encode_shim: encodeTransform
   java_import: edu.wpi.first.math.geometry.Transform3d
   # shim since we expect fields to at least exist
   fields: []

photon-serde/templates/ThingSerde.py.jinja

@@ -21,6 +21,7 @@
 ###############################################################################
 
 from ..targeting import *
+from ..packet import Packet
 
 class {{ name }}Serde:
 
@@ -28,6 +29,34 @@ class {{ name }}Serde:
     MESSAGE_VERSION = "{{ message_hash }}"
     MESSAGE_FORMAT = "{{ message_fmt }}"
 
+    @staticmethod
+    def pack(value: '{{ name }}' ) -> 'Packet':
+        ret = Packet()
+{% for field in fields -%}
+{%- if field.type | is_shimmed %}
+        ret.{{ get_message_by_name(field.type).python_encode_shim}}(value.{{ field.name }})
+{%- elif field.optional == True %}
+        # {{ field.name }} is optional! it better not be a VLA too
+        ret.encodeOptional(value.{{ field.name }}, {{ field.type }}.photonStruct)
+{%- elif field.vla == True and not field.type | is_intrinsic %}
+        # {{ field.name }} is a custom VLA!
+        ret.encodeList(value.{{ field.name }}, {{ field.type }}.photonStruct)
+{%- elif field.vla == True and field.type | is_intrinsic %}
+        # {{ field.name }} is a custom VLA!
+        ret.encode{{ type_map[field.type].java_type.title() }}List(value.{{ field.name }})
+{%- elif field.type | is_intrinsic %}
+        # {{ field.name }} is of intrinsic type {{ field.type }}
+        ret.{{ type_map[field.type].java_encode_shim }}(value.{{field.name}})
+{%- else %}
+        # {{ field.name }} is of non-intrinsic type {{ field.type }}
+        ret.encodeBytes({{ field.type }}.photonStruct.pack(value.{{field.name}}).getData())
+{%- endif %}
+{%- if not loop.last %}
+{% endif -%}
+{% endfor%}
+        return ret
+
+
     @staticmethod
     def unpack(packet: 'Packet') -> '{{ name }}':
         ret = {{ name }}()

photon-server/build.gradle

@@ -11,6 +11,9 @@ apply from: "${rootDir}/shared/common.gradle"
 dependencies {
     implementation project(':photon-core')
 
+    // Zip
+    implementation 'org.zeroturnaround:zt-zip:1.14'
+
     // Needed for Javalin Runtime Logging
     implementation "org.slf4j:slf4j-simple:2.0.7"
 }
@@ -22,6 +25,10 @@ application {
     mainClass = 'org.photonvision.Main'
 }
 
+jar {
+    from file("$rootDir/LICENSE")
+}
+
 shadowJar {
     archiveBaseName = "photonvision"
     archiveVersion = project.version as String

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

@@ -33,6 +33,7 @@ import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
 import org.photonvision.common.hardware.HardwareManager;
 import org.photonvision.common.hardware.PiVersion;
 import org.photonvision.common.hardware.Platform;
+import org.photonvision.common.logging.KernelLogLogger;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.LogLevel;
 import org.photonvision.common.logging.Logger;
@@ -437,6 +438,10 @@ public class Main {
         Logger.setLevel(LogGroup.General, logLevel);
         logger.info("Logging initialized in debug mode.");
 
+        // Add Linux kernel log->Photon logger
+        KernelLogLogger.getInstance();
+
+        // Add CSCore->Photon logger
         PvCSCoreLogger.getInstance();
 
         logger.debug("Loading ConfigManager...");

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

@@ -53,6 +53,7 @@ import org.photonvision.common.util.file.ProgramDirectoryUtilities;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.camera.CameraQuirk;
 import org.photonvision.vision.processes.VisionModuleManager;
+import org.zeroturnaround.zip.ZipUtil;
 
 public class RequestHandler {
     // Treat all 2XX calls as "INFO"
@@ -422,20 +423,37 @@ public class RequestHandler {
         try {
             ShellExec shell = new ShellExec();
             var tempPath = Files.createTempFile("photonvision-journalctl", ".txt");
-            shell.executeBashCommand("journalctl -u photonvision.service > " + tempPath.toAbsolutePath());
+            var tempPath2 = Files.createTempFile("photonvision-kernelogs", ".txt");
+            // In the command below:
+            //   dmesg = output all kernel logs since current boot
+            //   cat /var/log/kern.log = output all kernal logs since first boot
+            shell.executeBashCommand(
+                    "journalctl -u photonvision.service > "
+                            + tempPath.toAbsolutePath()
+                            + " && dmesg > "
+                            + tempPath2.toAbsolutePath());
 
             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());
-                ctx.contentType("text/plain");
-                ctx.header("Content-Disposition", "attachment; filename=\"photonvision-journalctl.txt\"");
-                ctx.status(200);
+                // Wrote to the temp file! Zip and yeet it to the client
+
+                var out = Files.createTempFile("photonvision-logs", "zip").toFile();
+
+                try {
+                    ZipUtil.packEntries(new File[] {tempPath.toFile(), tempPath2.toFile()}, out);
+                } catch (Exception e) {
+                    e.printStackTrace();
+                }
+
+                var stream = new FileInputStream(out);
+                ctx.contentType("application/zip");
+                ctx.header("Content-Disposition", "attachment; filename=\"photonvision-logs.zip\"");
                 ctx.result(stream);
-                logger.info("Uploading settings with size " + stream.available());
+                ctx.status(200);
+                logger.info("Outputting log ZIP with size " + stream.available());
             } else {
                 ctx.status(500);
                 ctx.result("The journalctl service was unable to export logs");

photon-targeting/build.gradle

@@ -8,6 +8,7 @@ apply plugin: 'edu.wpi.first.NativeUtils'
 apply plugin: 'edu.wpi.first.WpilibTools'
 apply plugin: 'edu.wpi.first.GradleJni'
 
+ext.licenseFile = file("$rootDir/LICENSE")
 apply from: "${rootDir}/shared/config.gradle"
 apply from: "${rootDir}/shared/javacommon.gradle"
 
@@ -220,3 +221,6 @@ nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
+nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, wpi.versions.opencvVersion.get())
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")

photon-targeting/src/main/java/org/photonvision/common/hardware/Platform.java

@@ -22,37 +22,63 @@ import java.io.BufferedReader;
 import java.io.IOException;
 import java.nio.file.Files;
 import java.nio.file.Paths;
+import java.util.function.Supplier;
 
 @SuppressWarnings("unused")
 public enum Platform {
     // WPILib Supported (JNI)
-    WINDOWS_64("Windows x64", "winx64", false, OSType.WINDOWS, true),
-    LINUX_32("Linux x86", "linuxx64", false, OSType.LINUX, true),
-    LINUX_64("Linux x64", "linuxx64", false, OSType.LINUX, true),
+    WINDOWS_64("Windows x64", Platform::getUnknownModel, "winx64", false, OSType.WINDOWS, true),
+    LINUX_32("Linux x86", Platform::getUnknownModel, "linuxx64", false, OSType.LINUX, true),
+    LINUX_64("Linux x64", Platform::getUnknownModel, "linuxx64", false, OSType.LINUX, true),
     LINUX_RASPBIAN32(
             "Linux Raspbian 32-bit",
+            Platform::getLinuxDeviceTreeModel,
             "linuxarm32",
             true,
             OSType.LINUX,
             true), // Raspberry Pi 3/4 with a 32-bit image
     LINUX_RASPBIAN64(
             "Linux Raspbian 64-bit",
+            Platform::getLinuxDeviceTreeModel,
             "linuxarm64",
             true,
             OSType.LINUX,
             true), // Raspberry Pi 3/4 with a 64-bit image
-    LINUX_RK3588_64("Linux AARCH 64-bit with RK3588", "linuxarm64", false, OSType.LINUX, true),
+    LINUX_RK3588_64(
+            "Linux AARCH 64-bit with RK3588",
+            Platform::getLinuxDeviceTreeModel,
+            "linuxarm64",
+            false,
+            OSType.LINUX,
+            true),
     LINUX_AARCH64(
-            "Linux AARCH64", "linuxarm64", false, OSType.LINUX, true), // Jetson Nano, Jetson TX2
+            "Linux AARCH64",
+            Platform::getLinuxDeviceTreeModel,
+            "linuxarm64",
+            false,
+            OSType.LINUX,
+            true), // Jetson Nano, Jetson TX2
 
     // PhotonVision Supported (Manual build/install)
-    LINUX_ARM64("Linux ARM64", "linuxarm64", false, OSType.LINUX, true), // ODROID C2, N2
+    LINUX_ARM64(
+            "Linux ARM64",
+            Platform::getLinuxDeviceTreeModel,
+            "linuxarm64",
+            false,
+            OSType.LINUX,
+            true), // ODROID C2, N2
 
     // Completely unsupported
-    WINDOWS_32("Windows x86", "windowsx64", false, OSType.WINDOWS, false),
-    MACOS("Mac OS", "osxuniversal", false, OSType.MACOS, false),
-    LINUX_ARM32("Linux ARM32", "linuxarm32", false, OSType.LINUX, false), // ODROID XU4, C1+
-    UNKNOWN("Unsupported Platform", "", false, OSType.UNKNOWN, false);
+    WINDOWS_32("Windows x86", Platform::getUnknownModel, "windowsx64", false, OSType.WINDOWS, false),
+    MACOS("Mac OS", Platform::getUnknownModel, "osxuniversal", false, OSType.MACOS, false),
+    LINUX_ARM32(
+            "Linux ARM32",
+            Platform::getUnknownModel,
+            "linuxarm32",
+            false,
+            OSType.LINUX,
+            false), // ODROID XU4, C1+
+    UNKNOWN("Unsupported Platform", Platform::getUnknownModel, "", false, OSType.UNKNOWN, false);
 
     public enum OSType {
         WINDOWS,
@@ -62,6 +88,7 @@ public enum Platform {
     }
 
     public final String description;
+    public final String hardwareModel;
     public final String nativeLibraryFolderName;
     public final boolean isPi;
     public final OSType osType;
@@ -72,11 +99,13 @@ public enum Platform {
 
     Platform(
             String description,
+            Supplier<String> getHardwareModel,
             String nativeLibFolderName,
             boolean isPi,
             OSType osType,
             boolean isSupported) {
         this.description = description;
+        this.hardwareModel = getHardwareModel.get();
         this.isPi = isPi;
         this.osType = osType;
         this.isSupported = isSupported;
@@ -107,6 +136,10 @@ public enum Platform {
         }
     }
 
+    public static String getHardwareModel() {
+        return currentPlatform.hardwareModel;
+    }
+
     public static String getNativeLibraryFolderName() {
         return currentPlatform.nativeLibraryFolderName;
     }
@@ -122,6 +155,7 @@ public enum Platform {
     private static final String OS_ARCH = System.getProperty("os.arch");
     private static final String UnknownPlatformString =
             String.format("Unknown Platform. OS: %s, Architecture: %s", OS_NAME, OS_ARCH);
+    private static final String UnknownDeviceModelString = "Unknown";
 
     public static Platform getCurrentPlatform() {
         if (RuntimeDetector.isWindows()) {
@@ -199,6 +233,22 @@ public enum Platform {
         return fileHasText("/proc/device-tree/model", "NVIDIA Jetson");
     }
 
+    static String getLinuxDeviceTreeModel() {
+        var deviceTreeModelPath = Paths.get("/proc/device-tree/model");
+        try {
+            if (Files.exists(deviceTreeModelPath)) {
+                return Files.readString(deviceTreeModelPath).trim();
+            }
+        } catch (Exception ex) {
+            return UnknownDeviceModelString;
+        }
+        return UnknownDeviceModelString;
+    }
+
+    static String getUnknownModel() {
+        return UnknownDeviceModelString;
+    }
+
     // Checks for various names of linux OS
     private static boolean isStretch() {
         // TODO - this is a total guess

photon-targeting/src/main/java/org/photonvision/jni/QueuedFileLogger.java

@@ -0,0 +1,60 @@
+/*
+ * 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.jni;
+
+public class QueuedFileLogger {
+    long m_handle = 0;
+
+    public QueuedFileLogger(String path) {
+        m_handle = QueuedFileLogger.create(path);
+    }
+
+    public String[] getNewlines() {
+        String newBuffer = null;
+
+        synchronized (this) {
+            if (m_handle == 0) {
+                System.err.println("QueuedFileLogger use after free");
+                return new String[0];
+            }
+
+            newBuffer = QueuedFileLogger.getNewLines(m_handle);
+        }
+
+        if (newBuffer == null) {
+            return new String[0];
+        }
+
+        return newBuffer.split("\n");
+    }
+
+    public void stop() {
+        synchronized (this) {
+            if (m_handle != 0) {
+                QueuedFileLogger.destroy(m_handle);
+                m_handle = 0;
+            }
+        }
+    }
+
+    private static native long create(String path);
+
+    private static native void destroy(long handle);
+
+    private static native String getNewLines(long handle);
+}

photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp

@@ -156,17 +156,11 @@ wpi::tsp::TimeSyncClient::TimeSyncClient(std::string_view server,
                                          std::function<uint64_t()> timeProvider)
     : m_logger(::ClientLoggerFunc),
       m_timeProvider(timeProvider),
-      m_udp{wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)},
-      m_pingTimer{wpi::uv::Timer::Create(m_loopRunner.GetLoop())},
+      m_udp{},
+      m_pingTimer{},
       m_serverIP{server},
       m_serverPort{remote_port},
       m_loopDelay(ping_delay) {
-  struct sockaddr_in serverAddr;
-  uv::NameToAddr(m_serverIP, m_serverPort, &serverAddr);
-
-  m_loopRunner.ExecSync(
-      [this, serverAddr](uv::Loop&) { m_udp->Connect(serverAddr); });
-
   // fmt::println("Starting client (with server address {}:{})", server,
   //              remote_port);
 }
@@ -175,6 +169,13 @@ void wpi::tsp::TimeSyncClient::Start() {
   // fmt::println("Connecting received");
 
   m_loopRunner.ExecSync([this](uv::Loop&) {
+    struct sockaddr_in serverAddr;
+    uv::NameToAddr(m_serverIP, m_serverPort, &serverAddr);
+
+    m_udp = {wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)};
+    m_pingTimer = {wpi::uv::Timer::Create(m_loopRunner.GetLoop())};
+
+    m_udp->Connect(serverAddr);
     m_udp->received.connect(&wpi::tsp::TimeSyncClient::UdpCallback, this);
     m_udp->StartRecv();
   });

photon-targeting/src/main/native/cpp/net/TimeSyncServer.cpp

@@ -101,13 +101,13 @@ wpi::tsp::TimeSyncServer::TimeSyncServer(int port,
                                          std::function<uint64_t()> timeProvider)
     : m_logger{::ServerLoggerFunc},
       m_timeProvider{timeProvider},
-      m_udp{wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)} {
-  m_loopRunner.ExecSync(
-      [this, port](uv::Loop&) { m_udp->Bind("0.0.0.0", port); });
-}
+      m_udp{},
+      m_port(port) {}
 
 void wpi::tsp::TimeSyncServer::Start() {
   m_loopRunner.ExecSync([this](uv::Loop&) {
+    m_udp = {wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)};
+    m_udp->Bind("0.0.0.0", m_port);
     m_udp->received.connect(&wpi::tsp::TimeSyncServer::UdpCallback, this);
     m_udp->StartRecv();
   });

photon-targeting/src/main/native/include/net/TimeSyncServer.h

@@ -53,6 +53,7 @@ class TimeSyncServer {
   wpi::Logger m_logger;
   std::function<uint64_t()> m_timeProvider;
   SharedUdpPtr m_udp;
+  int m_port;
 
   std::thread m_listener;
 

photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h

@@ -201,9 +201,8 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) {
   cv::Mat wrapped{rvecInput.rows, rvecInput.cols, CV_32F};
   rvecInput.convertTo(wrapped, CV_32F);
   data = wrapped.at<cv::Vec3f>(cv::Point{0, 0});
-  return RotationEDNToNWU(frc::Rotation3d{units::radian_t{data[0]},
-                                          units::radian_t{data[1]},
-                                          units::radian_t{data[2]}});
+  return RotationEDNToNWU(
+      frc::Rotation3d{Eigen::Vector3d{data[0], data[1], data[2]}});
 }
 
 [[maybe_unused]] static std::optional<photon::PnpResult> SolvePNP_Square(

photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h

@@ -26,11 +26,14 @@
 namespace photon {
 class RotTrlTransform3d {
  public:
-  RotTrlTransform3d(const frc::Rotation3d& rot, const frc::Translation3d& trl)
-      : trl(trl), rot(rot) {}
+  RotTrlTransform3d(const frc::Rotation3d& newRot,
+                    const frc::Translation3d& newTrl)
+      : trl{newTrl}, rot{newRot} {}
+
   RotTrlTransform3d(const frc::Pose3d& initial, const frc::Pose3d& last)
-      : trl(last.Translation() - initial.Translation().RotateBy(rot)),
-        rot(last.Rotation() - initial.Rotation()) {}
+      : trl{last.Translation() - initial.Translation().RotateBy(
+                                     last.Rotation() - initial.Rotation())},
+        rot{last.Rotation() - initial.Rotation()} {}
   explicit RotTrlTransform3d(const frc::Transform3d& trf)
       : RotTrlTransform3d(trf.Rotation(), trf.Translation()) {}
   RotTrlTransform3d()

photon-targeting/src/main/native/jni/FileLoggerExtrasJNI.cpp

@@ -0,0 +1,103 @@
+/*
+ * 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/>.
+ */
+
+#include <functional>
+#include <string>
+#include <vector>
+
+#include <wpi/FileLogger.h>
+
+#include "jni_utils.h"
+#include "org_photonvision_jni_QueuedFileLogger.h"
+
+struct QueuedFileLogger {
+  // ew ew ew ew ew ew ew ew
+  std::vector<char> m_data{};
+
+  std::mutex m_mutex;
+
+  wpi::FileLogger logger;
+
+  explicit QueuedFileLogger(std::string_view file)
+      : logger{file, std::bind(&QueuedFileLogger::callback, this,
+                               std::placeholders::_1)} {
+    // fmt::println("Watching {}", file);
+  }
+
+  void callback(std::string_view newline) {
+    std::lock_guard lock{m_mutex};
+    // fmt::println("FileLogger got: {}", newline);
+    m_data.insert(m_data.end(), newline.begin(), newline.end());
+  }
+
+  std::vector<char> SwapData() {
+    std::vector<char> ret;
+    {
+      std::lock_guard lock{m_mutex};
+      ret.swap(m_data);
+    }
+
+    return ret;
+  }
+};
+
+extern "C" {
+
+/*
+ * Class:     org_photonvision_jni_QueuedFileLogger
+ * Method:    create
+ * Signature: (Ljava/lang/String;)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_photonvision_jni_QueuedFileLogger_create
+  (JNIEnv* env, jclass, jstring name)
+{
+  const char* c_name{env->GetStringUTFChars(name, 0)};
+  std::string cpp_name{c_name};
+  jlong ret{reinterpret_cast<jlong>(new QueuedFileLogger(cpp_name))};
+  env->ReleaseStringUTFChars(name, c_name);
+  return ret;
+}
+
+/*
+ * Class:     org_photonvision_jni_QueuedFileLogger
+ * Method:    destroy
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_org_photonvision_jni_QueuedFileLogger_destroy
+  (JNIEnv*, jclass, jlong handle)
+{
+  CHECK_PTR(handle);
+  delete reinterpret_cast<QueuedFileLogger*>(handle);
+}
+
+/*
+ * Class:     org_photonvision_jni_QueuedFileLogger
+ * Method:    getNewLines
+ * Signature: (J)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_org_photonvision_jni_QueuedFileLogger_getNewLines
+  (JNIEnv* env, jclass, jlong handle)
+{
+  CHECK_PTR_RETURN(handle, nullptr);
+  QueuedFileLogger* logger = reinterpret_cast<QueuedFileLogger*>(handle);
+
+  return env->NewStringUTF(logger->SwapData().data());
+}
+}  // extern "C"

photon-targeting/src/main/native/jni/TimeSyncClientJNI.cpp

@@ -20,21 +20,11 @@
 #include <cstdio>
 #include <string>
 
+#include "jni_utils.h"
 #include "net/TimeSyncClient.h"
 
 using namespace wpi::tsp;
 
-#define CHECK_PTR(ptr)                                               \
-  if (!ptr) {                                                        \
-    fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \
-    return;                                                          \
-  }
-#define CHECK_PTR_RETURN(ptr, default)                               \
-  if (!ptr) {                                                        \
-    fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \
-    return default;                                                  \
-  }
-
 /**
  * Finds a class and keeps it as a global reference.
  *

photon-targeting/src/main/native/jni/TimeSyncServerJNI.cpp

@@ -20,21 +20,11 @@
 
 #include <cstdio>
 
+#include "jni_utils.h"
 #include "net/TimeSyncServer.h"
 
 using namespace wpi::tsp;
 
-#define CHECK_PTR(ptr)                                               \
-  if (!ptr) {                                                        \
-    fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \
-    return;                                                          \
-  }
-#define CHECK_PTR_RETURN(ptr, default)                               \
-  if (!ptr) {                                                        \
-    fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \
-    return default;                                                  \
-  }
-
 extern "C" {
 
 /*

photon-targeting/src/main/native/jni/jni_utils.h

@@ -0,0 +1,29 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#define CHECK_PTR(ptr)                                               \
+  if (!ptr) {                                                        \
+    fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \
+    return;                                                          \
+  }
+#define CHECK_PTR_RETURN(ptr, default)                               \
+  if (!ptr) {                                                        \
+    fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \
+    return default;                                                  \
+  }

photon-targeting/src/test/java/net/TimeSyncTest.java

@@ -36,6 +36,8 @@ public class TimeSyncTest {
         if (!PhotonTargetingJniLoader.load()) {
             fail();
         }
+
+        HAL.initialize(1000, 0);
     }
 
     @AfterAll
@@ -45,8 +47,6 @@ public class TimeSyncTest {
 
     @Test
     public void smoketest() throws InterruptedException {
-        HAL.initialize(1000, 0);
-
         // NetworkTableInstance.getDefault().stopClient();
         // NetworkTableInstance.getDefault().startServer();
 

photon-targeting/src/test/java/wpiutil_extras/FileLoggerTest.java

@@ -0,0 +1,62 @@
+/*
+ * 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 wpiutil_extras;
+
+import static org.junit.jupiter.api.Assertions.fail;
+
+import edu.wpi.first.hal.HAL;
+import java.io.IOException;
+import org.junit.jupiter.api.AfterAll;
+import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.Test;
+import org.photonvision.jni.PhotonTargetingJniLoader;
+import org.photonvision.jni.QueuedFileLogger;
+import org.photonvision.jni.WpilibLoader;
+
+public class FileLoggerTest {
+    @BeforeAll
+    public static void load_wpilib() throws UnsatisfiedLinkError, IOException {
+        if (!WpilibLoader.loadLibraries()) {
+            fail();
+        }
+        if (!PhotonTargetingJniLoader.load()) {
+            fail();
+        }
+
+        HAL.initialize(1000, 0);
+    }
+
+    @AfterAll
+    public static void teardown() {
+        HAL.shutdown();
+    }
+
+    @Test
+    public void smoketest() throws InterruptedException {
+        var logger = new QueuedFileLogger("/var/log/kern.log");
+        for (int i = 0; i < 100; i++) {
+            Thread.sleep(1000);
+
+            for (var line : logger.getNewlines()) {
+                System.out.println(" ->:" + line);
+            }
+        }
+
+        logger.stop();
+    }
+}

photonlib-python-examples/aimandrange/drivetrain.py

@@ -24,11 +24,12 @@
 
 
 import math
+
+import swervemodule
 import wpilib
 import wpilib.simulation
 import wpimath.geometry
 import wpimath.kinematics
-import swervemodule
 
 kMaxSpeed = 3.0  # 3 meters per second
 kMaxAngularSpeed = math.pi  # 1/2 rotation per second

photonlib-python-examples/aimandrange/robot.py

@@ -24,9 +24,9 @@
 ###################################################################################
 
 import math
-import wpilib
-import drivetrain
 
+import drivetrain
+import wpilib
 from photonlibpy import PhotonCamera
 
 VISION_TURN_kP = 0.01

photonlib-python-examples/aimandrange/swervemodule.py

@@ -23,12 +23,13 @@
 ###################################################################################
 
 import math
+
 import wpilib
 import wpilib.simulation
-import wpimath.kinematics
+import wpimath.controller
 import wpimath.filter
 import wpimath.geometry
-import wpimath.controller
+import wpimath.kinematics
 import wpimath.trajectory
 import wpimath.units
 

photonlib-python-examples/aimattarget/drivetrain.py

@@ -24,11 +24,12 @@
 
 
 import math
+
+import swervemodule
 import wpilib
 import wpilib.simulation
 import wpimath.geometry
 import wpimath.kinematics
-import swervemodule
 
 kMaxSpeed = 3.0  # 3 meters per second
 kMaxAngularSpeed = math.pi  # 1/2 rotation per second

photonlib-python-examples/aimattarget/robot.py

@@ -24,9 +24,8 @@
 ###################################################################################
 
 
-import wpilib
 import drivetrain
-
+import wpilib
 from photonlibpy import PhotonCamera
 
 VISION_TURN_kP = 0.01

photonlib-python-examples/aimattarget/swervemodule.py

@@ -23,12 +23,13 @@
 ###################################################################################
 
 import math
+
 import wpilib
 import wpilib.simulation
-import wpimath.kinematics
+import wpimath.controller
 import wpimath.filter
 import wpimath.geometry
-import wpimath.controller
+import wpimath.kinematics
 import wpimath.trajectory
 import wpimath.units
 

photonlib-python-examples/poseest/drivetrain.py

@@ -24,12 +24,13 @@
 
 
 import math
+
+import swervemodule
 import wpilib
 import wpilib.simulation
+import wpimath.estimator
 import wpimath.geometry
 import wpimath.kinematics
-import wpimath.estimator
-import swervemodule
 
 kMaxSpeed = 3.0  # 3 meters per second
 kMaxAngularSpeed = math.pi  # 1/2 rotation per second

photonlib-python-examples/poseest/robot.py

@@ -24,12 +24,11 @@
 ###################################################################################
 
 
+import drivetrain
 import wpilib
 import wpimath.geometry
-from robotpy_apriltag import AprilTagField, loadAprilTagLayoutField
-import drivetrain
-
 from photonlibpy import PhotonCamera, PhotonPoseEstimator, PoseStrategy
+from robotpy_apriltag import AprilTagField, loadAprilTagLayoutField
 
 kRobotToCam = wpimath.geometry.Transform3d(
     wpimath.geometry.Translation3d(0.5, 0.0, 0.5),

photonlib-python-examples/poseest/swervemodule.py

@@ -23,12 +23,13 @@
 ###################################################################################
 
 import math
+
 import wpilib
 import wpilib.simulation
-import wpimath.kinematics
+import wpimath.controller
 import wpimath.filter
 import wpimath.geometry
-import wpimath.controller
+import wpimath.kinematics
 import wpimath.trajectory
 import wpimath.units
 

scripts/catnt.py

@@ -1,6 +1,7 @@
-from time import sleep
-import ntcore
 import argparse
+from time import sleep
+
+import ntcore
 from tabulate import tabulate
 
 

scripts/install.sh

@@ -1,336 +1,7 @@
 #!/bin/bash
-
-needs_arg() {
-    if [ -z "$OPTARG" ]; then
-      die "Argument is required for --$OPT option" \
-          "See './install.sh -h' for more information."
-    fi;
-}
-
-die() {
-  for arg in "$@"; do
-    echo "$arg" 1>&2
-  done
-  exit 1
-}
-
-debug() {
-  if [ -z "$QUIET" ] ; then
-    for arg in "$@"; do
-      echo "$arg"
-    done
-  fi
-}
-
-package_is_installed(){
-    dpkg-query -W -f='${Status}' "$1" 2>/dev/null | grep -q "ok installed"
-}
-
-install_if_missing() {
-  if package_is_installed "$1" ; then
-    debug "Found existing $1. Skipping..."
-    return
-  fi
-
-  debug "Installing $1..."
-  apt-get install --yes "$1"
-  debug "$1 installation complete."
-}
-
-get_photonvision_releases() {
-  # Return cached input
-  if [ -n "$PHOTON_VISION_RELEASES" ] ; then
-    echo "$PHOTON_VISION_RELEASES"
-    return
-  fi
-
-  # Use curl if available, otherwise fallback to wget
-  if command -v curl > /dev/null 2>&1 ; then
-    PHOTON_VISION_RELEASES="$(curl -sk https://api.github.com/repos/photonvision/photonvision/releases)"
-  else
-    PHOTON_VISION_RELEASES="$(wget -qO- https://api.github.com/repos/photonvision/photonvision/releases)"
-  fi
-
-  echo "$PHOTON_VISION_RELEASES"
-}
-
-get_versions() {
-  if [ -z "$PHOTON_VISION_VERSIONS" ] ; then
-    PHOTON_VISION_VERSIONS=$(get_photonvision_releases | \
-      sed -En 's/\"tag_name\": \"v([0-9]+\.[0-9]+\.[0-9]+)(-(beta|alpha)(-[0-9])?(\.[0-9]+)?)?\",/\1\2/p' | \
-      sed 's/^[[:space:]]*//')
-  fi
-
-  echo "$PHOTON_VISION_VERSIONS"
-}
-
-is_version_available() {
-  local target_version="$1"
-
-  # latest is a special case
-  if [ "$target_version" = "latest" ]; then
-    return 0
-  fi
-
-  # Check if multiple lines are match. You can only match 1.
-  if [ "$(get_versions | grep -cFx "$target_version")" -ne 1 ] ; then
-    return 1
-  fi
-
-  return 0
-}
-
-help() {
-  cat << EOF
-This script installs Photonvision.
-It must be run as root.
-
-Syntax: sudo ./install.sh [options]
-  options:
-  -h, --help
-      Display this help message.
-  -l, --list-versions
-      Lists all available versions of PhotonVision.
-  -v <version>, --version=<version>
-      Specifies which version of PhotonVision to install.
-      If not specified, the latest stable release is installed.
-      Ignores leading 'v's.
-  -a <arch>, --arch=<arch>
-      Install PhotonVision for the specified architecture.
-      Supported values: aarch64, x86_64
-  -m [option], --install-nm=[option]
-      Controls NetworkManager installation (Ubuntu only).
-      Options: "yes", "no", "ask".
-      Default: "ask" (unless -q or --quiet is specified, then "no").
-      "ask" prompts for installation. Ignored on other distros.
-  -n, --no-networking
-      Disable networking. This will also prevent installation of
-      NetworkManager, overriding -m,--install-nm.
-  -q, --quiet
-      Silent install, automatically accepts all defaults. For
-      non-interactive use. Makes -m,--install-nm default to "no".
-
-EOF
-}
-
-INSTALL_NETWORK_MANAGER="ask"
-VERSION="latest"
-
-while getopts "hlv:a:mnq-:" OPT; do
-  if [ "$OPT" = "-" ]; then
-    OPT="${OPTARG%%=*}"       # extract long option name
-    OPTARG="${OPTARG#"$OPT"}" # extract long option argument (may be empty)
-    OPTARG="${OPTARG#=}"      # if long option argument, remove assigning `=`
-  fi
-
-  case "$OPT" in
-    h | help)
-      help
-      exit 0
-      ;;
-    l | list-versions)
-      get_versions
-      exit 0
-      ;;
-    v | version)
-      needs_arg
-      VERSION=${OPTARG#v}  # drop leading 'v's
-      ;;
-    a | arch) needs_arg; ARCH=$OPTARG
-      ;;
-    m | install-nm)
-      INSTALL_NETWORK_MANAGER="$(echo "${OPTARG:-'yes'}" | tr '[:upper:]' '[:lower:]')"
-      case "$INSTALL_NETWORK_MANAGER" in
-        yes)
-          ;;
-        no)
-          ;;
-        ask)
-          ;;
-        * )
-          die "Valid options for -m, --install-nm are: 'yes', 'no', and 'ask'"
-          ;;
-      esac
-      ;;
-    n | no-networking) DISABLE_NETWORKING="true"
-      ;;
-    q | quiet) QUIET="true"
-      ;;
-    \?)  # Handle invalid short options
-      die "Error: Invalid option -$OPTARG" \
-          "See './install.sh -h' for more information."
-      ;;
-    * )  # Handle invalid long options
-      die "Error: Invalid option --$OPT" \
-          "See './install.sh -h' for more information."
-      ;;
-  esac
-done
-
-if [ "$(id -u)" != "0" ]; then
-   die "This script must be run as root"
-fi
-
-if [[ -z "$ARCH" ]]; then
-  debug "Arch was not specified. Inferring..."
-  ARCH=$(uname -m)
-  debug "Arch was inferred to be $ARCH"
-fi
-
-ARCH_NAME=""
-if [ "$ARCH" = "aarch64" ]; then
-  ARCH_NAME="linuxarm64"
-elif [ "$ARCH" = "armv7l" ]; then
-  die "ARM32 is not supported by PhotonVision. Exiting."
-elif [ "$ARCH" = "x86_64" ]; then
-  ARCH_NAME="linuxx64"
-else
-  die "Unsupported or unknown architecture: '$ARCH'." \
-  "Please specify your architecture using: ./install.sh -a <arch> " \
-  "Run './install.sh -h' for more information."
-fi
-
-debug "This is the installation script for PhotonVision."
-debug "Installing for platform $ARCH"
-
-DISTRO=$(lsb_release -is)
-
-# Only ask if it makes sense to do so.
-# i.e. the distro is Ubuntu, you haven't requested disabling networking,
-# and you have requested a quiet install.
-if [[ "$INSTALL_NETWORK_MANAGER" == "ask" ]]; then
-  if [[ "$DISTRO" != "Ubuntu" || -n "$DISABLE_NETWORKING" || -n "$QUIET" ]] ; then
-    INSTALL_NETWORK_MANAGER="no"
-  fi
-fi
-
-if [[ "$INSTALL_NETWORK_MANAGER" == "ask" ]]; then
-  debug ""
-  debug "Photonvision uses NetworkManager to control networking on your device."
-  debug "This could possibly mess up the network configuration in Ubuntu."
-  read -p "Do you want this script to install and configure NetworkManager? [y/N]: " response
-  if [[ $response == [yY] || $response == [yY][eE][sS] ]]; then
-    INSTALL_NETWORK_MANAGER="yes"
-  fi
-fi
-
-debug "Updating package list..."
-apt-get update
-debug "Updated package list."
-
-install_if_missing curl
-install_if_missing avahi-daemon
-install_if_missing cpufrequtils
-install_if_missing libatomic1
-install_if_missing v4l-utils
-install_if_missing sqlite3
-install_if_missing openjdk-17-jre-headless
-
-debug "Setting cpufrequtils to performance mode"
-if [ -f /etc/default/cpufrequtils ]; then
-    sed -i -e 's/^#\?GOVERNOR=.*$/GOVERNOR=performance/' /etc/default/cpufrequtils
-else
-    echo 'GOVERNOR=performance' > /etc/default/cpufrequtils
-fi
-
-if [[ "$INSTALL_NETWORK_MANAGER" == "yes" ]]; then
-  debug "NetworkManager installation specified. Installing components..."
-  install_if_missing network-manager
-  install_if_missing net-tools
-
-  debug "Configuring..."
-  systemctl disable systemd-networkd-wait-online.service
-  cat > /etc/netplan/00-default-nm-renderer.yaml <<EOF
-network:
-  renderer: NetworkManager
-EOF
-  debug "network-manager installation complete."
-fi
-
-debug ""
-debug "Installing additional math packages"
-if [[ "$DISTRO" = "Ubuntu" && -z $(apt-cache search libcholmod3) ]]; then
-  debug "Adding jammy to list of apt sources"
-  add-apt-repository -y -S 'deb http://ports.ubuntu.com/ubuntu-ports jammy main universe'
-fi
-
-install_if_missing libcholmod3
-install_if_missing liblapack3
-install_if_missing libsuitesparseconfig5
-
-debug ""
-
-if ! is_version_available "$VERSION" ; then
-  die "PhotonVision v$VERSION is not available" \
-      "See ./install --list-versions for a complete list of available versions."
-fi
-
-if [ "$VERSION" = "latest" ] ; then
-  RELEASE_URL="https://api.github.com/repos/photonvision/photonvision/releases/latest"
-  debug "Downloading PhotonVision (latest)..."
-else
-  RELEASE_URL="https://api.github.com/repos/photonvision/photonvision/releases/tags/v$VERSION"
-  debug "Downloading PhotonVision (v$VERSION)..."
-fi
-
-mkdir -p /opt/photonvision
-cd /opt/photonvision || die "Tried to enter /opt/photonvision, but it was not created."
-curl -sk "$RELEASE_URL" |
-    grep "browser_download_url.*$ARCH_NAME.jar" |
-    cut -d : -f 2,3 |
-    tr -d '"' |
-    wget -qi - -O photonvision.jar
-debug "Downloaded PhotonVision."
-
-debug "Creating the PhotonVision systemd service..."
-
-# service --status-all doesn't list photonvision on OrangePi use systemctl instead:
-if systemctl --quiet is-active photonvision; then
-  debug "PhotonVision is already running. Stopping service."
-  systemctl stop photonvision
-  systemctl disable photonvision
-  rm /lib/systemd/system/photonvision.service
-  rm /etc/systemd/system/photonvision.service
-  systemctl daemon-reload
-  systemctl reset-failed
-fi
-
-cat > /lib/systemd/system/photonvision.service <<EOF
-[Unit]
-Description=Service that runs PhotonVision
-
-[Service]
-WorkingDirectory=/opt/photonvision
-# Run photonvision at "nice" -10, which is higher priority than standard
-Nice=-10
-# for non-uniform CPUs, like big.LITTLE, you want to select the big cores
-# look up the right values for your CPU
-# AllowedCPUs=4-7
-
-ExecStart=/usr/bin/java -Xmx512m -jar /opt/photonvision/photonvision.jar
-ExecStop=/bin/systemctl kill photonvision
-Type=simple
-Restart=on-failure
-RestartSec=1
-
-[Install]
-WantedBy=multi-user.target
-EOF
-
-if [ "$DISABLE_NETWORKING" = "true" ]; then
-  sed -i "s/photonvision.jar/photonvision.jar -n/" /lib/systemd/system/photonvision.service
-fi
-
-if grep -q "RK3588" /proc/cpuinfo; then
-  debug "This has a Rockchip RK3588, enabling all cores"
-  sed -i 's/# AllowedCPUs=4-7/AllowedCPUs=4-7/g' /lib/systemd/system/photonvision.service
-fi
-
-cp /lib/systemd/system/photonvision.service /etc/systemd/system/photonvision.service
-chmod 644 /etc/systemd/system/photonvision.service
-systemctl daemon-reload
-systemctl enable photonvision.service
-
-debug "Created PhotonVision systemd service."
-
-debug "PhotonVision installation successful."
+# The install script is now in photon-image-modifier
+# this downloads and runs that install script for people using the old short URL
+wget -q https://raw.githubusercontent.com/PhotonVision/photon-image-modifier/master/install.sh -O ./real_install.sh
+chmod +x ./real_install.sh
+./real_install.sh "$@"
+rm ./real_install.sh

shared/config.gradle

@@ -85,8 +85,6 @@ ext.appendDebugPathToBinaries = { binaries ->
     }
 }
 
-def licenseFile = file("$rootDir/LICENCE")
-
 // Create ZIP tasks for each component.
 ext.createComponentZipTasks = { components, names, base, type, project, func ->
     def stringNames = names.collect { it.toString() }

shared/javacommon.gradle

@@ -13,6 +13,7 @@ def artifactGroupId = 'org.photonvision'
 def javaBaseName = "_GROUP_org_photonvision_${baseArtifactId}_ID_${baseArtifactId}-java_CLS"
 
 def outputsFolder = file("$buildDir/outputs")
+def licenseFile = ext.licenseFile
 
 javadoc {
     options {
@@ -21,20 +22,27 @@ javadoc {
     }
 }
 
+jar {
+    from licenseFile
+}
+
 task sourcesJar(type: Jar, dependsOn: classes) {
     archiveClassifier = 'sources'
     from sourceSets.main.allSource
+    from licenseFile
 }
 
 task javadocJar(type: Jar, dependsOn: javadoc) {
     archiveClassifier = 'javadoc'
     from javadoc.destinationDir
+    from licenseFile
 }
 
 task outputJar(type: Jar, dependsOn: classes) {
     archiveBaseName = javaBaseName
     destinationDirectory = outputsFolder
     from sourceSets.main.output
+    from licenseFile
 }
 
 task outputSourcesJar(type: Jar, dependsOn: classes) {
@@ -42,6 +50,7 @@ task outputSourcesJar(type: Jar, dependsOn: classes) {
     destinationDirectory = outputsFolder
     archiveClassifier = 'sources'
     from sourceSets.main.allSource
+    from licenseFile
 }
 
 task outputJavadocJar(type: Jar, dependsOn: javadoc) {
@@ -49,6 +58,7 @@ task outputJavadocJar(type: Jar, dependsOn: javadoc) {
     destinationDirectory = outputsFolder
     archiveClassifier = 'javadoc'
     from javadoc.destinationDir
+    from licenseFile
 }
 
 artifacts {

shared/javacpp/publish.gradle

@@ -10,8 +10,7 @@ def zipBaseName = "_GROUP_org_photonvision_${baseArtifactId}_ID_${baseArtifactId
 def jniBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-jni_CLS"
 def jniCvStaticBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-jnicvstatic_CLS"
 
-def licenseFile = file("$rootDir/LICENCE")
-
+def licenseFile = ext.licenseFile
 // Quick hack to make this name visible to photon-lib for combined
 ext.zipBaseName = zipBaseName
 ext.artifactGroupId = artifactGroupId