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
@@ -507,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:
@@ -529,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
@@ -546,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
@@ -73,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
@@ -88,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/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

@@ -18,9 +18,6 @@
 package org.photonvision.common.logging;
 
 import edu.wpi.first.util.RuntimeDetector;
-import java.io.IOException;
-import java.nio.file.Files;
-import java.nio.file.Path;
 import org.photonvision.common.util.TimedTaskManager;
 import org.photonvision.jni.QueuedFileLogger;
 
@@ -43,17 +40,6 @@ public class KernelLogLogger {
 
     public KernelLogLogger() {
         if (RuntimeDetector.isLinux()) {
-            logger.info("Listening for klogs on /var/log/dmesg ! Boot logs:");
-
-            try {
-                var bootlog = Files.readAllLines(Path.of("/var/log/dmesg"));
-                for (var line : bootlog) {
-                    logger.log(line, LogLevel.DEBUG);
-                }
-            } catch (IOException e) {
-                logger.error("Couldn't read /var/log/dmesg - not printing boot logs");
-            }
-
             listener = new QueuedFileLogger("/var/log/kern.log");
         } else {
             System.out.println("NOT for klogs");

photon-lib/build.gradle

@@ -23,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,8 +20,8 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
-from ..targeting import *
 from ..packet import Packet
+from ..targeting import *
 
 
 class MultiTargetPNPResultSerde:

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

@@ -20,8 +20,8 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
-from ..targeting import *
 from ..packet import Packet
+from ..targeting import *
 
 
 class PhotonPipelineMetadataSerde:

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

@@ -20,8 +20,8 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
-from ..targeting import *
 from ..packet import Packet
+from ..targeting import *
 
 
 class PhotonPipelineResultSerde:

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

@@ -20,8 +20,8 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
-from ..targeting import *
 from ..packet import Packet
+from ..targeting import *
 
 
 class PhotonTrackedTargetSerde:

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

@@ -20,8 +20,8 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
-from ..targeting import *
 from ..packet import Packet
+from ..targeting import *
 
 
 class PnpResultSerde:

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

@@ -20,8 +20,8 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
-from ..targeting import *
 from ..packet import Packet
+from ..targeting import *
 
 
 class TargetCornerSerde:

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,9 +16,17 @@
 ###############################################################################
 
 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:
@@ -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,14 +194,14 @@ 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:
@@ -280,7 +288,7 @@ class Packet:
         self.encodeDouble(quaternion.Y())
         self.encodeDouble(quaternion.Z())
 
-    def encodeList(self, values: list[Any], serde: Type):
+    def encodeList(self, values: list[T], serde: Serde[T]):
         """
         Encodes a list of items using a specific serializer and appends it to the packet.
         """
@@ -290,7 +298,7 @@ class Packet:
             self.packetData = self.packetData + packed.getData()
             self.size = len(self.packetData)
 
-    def encodeOptional(self, value: Optional[Any], serde: Type):
+    def encodeOptional(self, value: Optional[T], serde: Serde[T]):
         """
         Encodes an optional value using a specific serializer.
         """

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

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,7 +1,13 @@
 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:
@@ -11,7 +17,7 @@ class PnpResult:
     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:
@@ -17,7 +20,7 @@ class PhotonPipelineMetadata:
 
     timeSinceLastPong: int = -1
 
-    photonStruct: "PhotonPipelineMetadataSerde" = None
+    photonStruct: ClassVar["generated.PhotonPipelineMetadataSerde"]
 
 
 @dataclass
@@ -57,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.
@@ -66,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:
@@ -57,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,14 +15,14 @@
 ## along with this program.  If not, see <https://www.gnu.org/licenses/>.
 ###############################################################################
 
-from photonlibpy.targeting.multiTargetPNPResult import MultiTargetPNPResult, PnpResult
-from photonlibpy.targeting.photonPipelineResult import PhotonPipelineResult
 from photonlibpy import PhotonPoseEstimator, PoseStrategy
 from photonlibpy.targeting import (
+    PhotonPipelineMetadata,
     PhotonTrackedTarget,
     TargetCorner,
-    PhotonPipelineMetadata,
 )
+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
 

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/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-server/src/main/java/org/photonvision/server/RequestHandler.java

@@ -424,10 +424,13 @@ public class RequestHandler {
             ShellExec shell = new ShellExec();
             var tempPath = Files.createTempFile("photonvision-journalctl", ".txt");
             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()
-                            + " && journalctl -k > "
+                            + " && dmesg > "
                             + tempPath2.toAbsolutePath());
 
             while (!shell.isOutputCompleted()) {

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/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()

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