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

@@ -48,13 +48,13 @@ jobs:
           fetch-depth: 0
       - name: Fetch tags
         run: git fetch --tags --force
-      - name: Install RoboRIO Toolchain
-        run: ./gradlew installRoboRioToolchain
       - name: Install Java 17
         uses: actions/setup-java@v4
         with:
           java-version: 17
           distribution: temurin
+      - name: Install RoboRIO Toolchain
+        run: ./gradlew installRoboRioToolchain
       # Need to publish to maven local first, so that C++ sim can pick it up
       # Still haven't figured out how to make the vendordep file be copied before trying to build examples
       - name: Publish photonlib to maven local
@@ -85,7 +85,7 @@ jobs:
       - name: Fetch tags
         run: git fetch --tags --force
       - name: Install Java 17
-        uses: actions/setup-java@v3
+        uses: actions/setup-java@v4
         with:
           java-version: 17
           distribution: temurin
@@ -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:
@@ -115,6 +115,10 @@ jobs:
       - uses: actions/setup-python@v5
         with:
           python-version: '3.11'
+      - name: Install graphviz
+        run: |
+          sudo apt-get update
+          sudo apt-get -y install graphviz
       - name: Install dependencies
         working-directory: docs
         run: |
@@ -129,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
@@ -161,6 +196,7 @@ jobs:
       - run: |
           chmod +x gradlew
           ./gradlew photon-targeting:build photon-lib:build -i
+        name: Build with Gradle
       - run: ./gradlew photon-lib:publish photon-targeting:publish
         name: Publish
         env:
@@ -283,6 +319,9 @@ jobs:
           java-version: 17
           distribution: temurin
           architecture: ${{ matrix.architecture }}
+      - name: Install Arm64 Toolchain
+        run: ./gradlew installArm64Toolchain
+        if: ${{ (matrix.artifact-name) == 'LinuxArm64' }}
       - run: |
           rm -rf photon-server/src/main/resources/web/*
           mkdir -p photon-server/src/main/resources/web/docs
@@ -301,7 +340,7 @@ jobs:
           path: photon-server/src/main/resources/web/docs
       - run: |
           chmod +x gradlew
-          ./gradlew photon-server:shadowJar -PArchOverride=${{ matrix.arch-override }}
+          ./gradlew photon-targeting:jar photon-server:shadowJar -PArchOverride=${{ matrix.arch-override }}
         if: ${{ (matrix.arch-override != 'none') }}
       - run: |
           chmod +x gradlew
@@ -311,6 +350,10 @@ jobs:
         with:
           name: jar-${{ matrix.artifact-name }}
           path: photon-server/build/libs
+      - uses: actions/upload-artifact@v4
+        with:
+          name: photon-targeting_jar-${{ matrix.artifact-name }}
+          path: photon-targeting/build/libs
 
   run-smoketest-native:
     needs: [build-package]
@@ -344,7 +387,7 @@ jobs:
       - run: |
           sudo apt-get update
           sudo apt-get install --yes libcholmod3 liblapack3 libsuitesparseconfig5
-        if: ${{ (matrix.os) == 'ubuntu-latest' }}
+        if: ${{ (matrix.os) == 'ubuntu-22.04' }}
       # and actually run the jar
       - run: java -jar ${{ matrix.extraOpts }} *.jar --smoketest
         if: ${{ (matrix.os) != 'windows-latest' }}
@@ -439,7 +482,7 @@ jobs:
             image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/v2025.0.0-beta-6/photonvision_opi5pro.img.xz
             cpu: cortex-a8
             image_additional_mb: 1024
-          - os: ubuntu-latest
+          - os: ubuntu-22.04
             artifact-name: LinuxArm64
             image_suffix: orangepi5max
             image_url: https://github.com/PhotonVision/photon-image-modifier/releases/download/v2025.0.0-beta-6/photonvision_opi5max.img.xz
@@ -495,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:
@@ -517,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
@@ -534,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,16 +54,17 @@ 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@v3
+      - uses: actions/setup-java@v4
         with:
           java-version: 17
           distribution: temurin
       - run: |
           chmod +x gradlew
           ./gradlew spotlessCheck
+        name: Run spotless
 
   client-lint-format:
     name: "PhotonClient Lint and Formatting"
@@ -72,9 +73,9 @@ jobs:
         working-directory: photon-client
     runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: Setup Node.js
-        uses: actions/setup-node@v3
+        uses: actions/setup-node@v4
         with:
           node-version: 18
       - name: Install Dependencies
@@ -87,7 +88,7 @@ jobs:
     name: "Check server index.html not changed"
     runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: Fetch all history and metadata
         run: |
           git fetch --prune --unshallow

.github/workflows/photon-code-docs.yml

@@ -59,7 +59,7 @@ jobs:
     - name: Fetch tags
       run: git fetch --tags --force
     - name: Install Java 17
-      uses: actions/setup-java@v3
+      uses: actions/setup-java@v4
       with:
         java-version: 17
         distribution: temurin

.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:
@@ -26,6 +26,11 @@ jobs:
     - name: Install and upgrade pip
       run: python -m pip install --upgrade pip
 
+    - name: Install graphviz
+      run: |
+        sudo apt-get update
+        sudo apt-get -y install graphviz
+
     - name: Install Python dependencies
       working-directory: docs
       run: |

.github/workflows/python.yml

@@ -21,7 +21,7 @@ on:
 
 jobs:
   buildAndDeploy:
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
 
     steps:
       - name: Checkout code

.readthedocs.yaml

@@ -9,6 +9,8 @@ build:
   os: ubuntu-22.04
   tools:
     python: "3.11"
+  apt_packages:
+    - graphviz
   jobs:
     post_checkout:
       # Cancel building pull requests when there aren't changed in the docs directory or YAML file.

README.md

@@ -25,7 +25,7 @@ If you are interested in contributing code or documentation to the project, plea
 
 Gradle is used for all C++ and Java code, and NPM is used for the web UI. Instructions to compile PhotonVision yourself can be found [in our docs](https://docs.photonvision.org/en/latest/docs/contributing/building-photon.html#compiling-instructions).
 
-You can run one of the many built in examples straight from the command line, too! They contain a fully featured robot project, and some include simulation support. The projects can be found inside the [`photonlib-java-examples`](photonlib-java-examples) and [`photonlib-cpp-examples`](photonlib-cpp-examples) subdirectories, respectively. Instructions for running these examples directly from the repo are found [in the docs](https://docs.photonvision.org/en/latest/docs/contributing/photonvision/build-instructions.html#running-examples).
+You can run one of the many built in examples straight from the command line, too! They contain a fully featured robot project, and some include simulation support. The projects can be found inside the [`photonlib-java-examples`](photonlib-java-examples) and [`photonlib-cpp-examples`](photonlib-cpp-examples) subdirectories, respectively. Instructions for running these examples directly from the repo are found [in the docs](https://docs.photonvision.org/en/latest/docs/contributing/building-photon.html#running-examples).
 
 ## Gradle Arguments
 
@@ -47,7 +47,7 @@ If you're cross-compiling, you'll need the wpilib toolchain installed. This can
 
 ## Out-of-Source Dependencies
 
-PhotonVision uses the following additonal out-of-source repositories for building code.
+PhotonVision uses the following additional out-of-source repositories for building code.
 
 - Base system images for Raspberry Pi & Orange Pi: https://github.com/PhotonVision/photon-image-modifier
 - C++ driver for Raspberry Pi CSI cameras: https://github.com/PhotonVision/photon-libcamera-gl-driver

build.gradle

@@ -4,11 +4,10 @@ plugins {
     id "java"
     id "cpp"
     id "com.diffplug.spotless" version "6.24.0"
-    id "edu.wpi.first.NativeUtils" version "2024.6.1" apply false
     id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2"
-    id "edu.wpi.first.GradleRIO" version "2024.3.2"
+    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1"
     id 'edu.wpi.first.WpilibTools' version '1.3.0'
-    id 'com.google.protobuf' version '0.9.4' apply false
+    id 'com.google.protobuf' version '0.9.3' apply false
     id 'edu.wpi.first.GradleJni' version '1.1.0'
 }
 
@@ -31,14 +30,15 @@ ext.allOutputsFolder = file("$project.buildDir/outputs")
 apply from: "versioningHelper.gradle"
 
 ext {
-    wpilibVersion = "2024.3.2"
+    wpilibVersion = "2025.1.1-beta-1"
     wpimathVersion = wpilibVersion
-    openCVversion = "4.8.0-2"
+    openCVYear = "2024"
+    openCVversion = "4.8.0-4"
     joglVersion = "2.4.0"
     javalinVersion = "5.6.2"
     libcameraDriverVersion = "dev-v2023.1.0-14-g787ab59"
     rknnVersion = "dev-v2024.0.1-4-g0db16ac"
-    frcYear = "2024"
+    frcYear = "2025"
     mrcalVersion = "dev-v2024.0.0-24-gc1efcf0";
 
 

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/conf.py

@@ -37,6 +37,7 @@ extensions = [
     "sphinx_design",
     "myst_parser",
     "sphinx.ext.mathjax",
+    "sphinx.ext.graphviz",
 ]
 
 # Configure OpenGraph support

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

@@ -26,7 +26,7 @@ The built documentation is located at `docs/build/html/index.html` relative to t
 
 ## Docs Builds on Pull Requests
 
-Pre-merge builds of docs can be found at: `https://photonvision-docs--PRNUMBER.org.readthedocs.build/en/PRNUMBER/index.html`. These docs are republished on every commit to a pull request made to PhotonVision/photonvision-docs. For example, PR 325 would have pre-merge documentation published to `https://photonvision-docs--325.org.readthedocs.build/en/325/index.html`. Additionally, the pull requrest will have a link directly to the pre-release build of the docs. This build only runs when there is a change to files in the docs sub-folder.
+Pre-merge builds of docs can be found at: `https://photonvision-docs--PRNUMBER.org.readthedocs.build/en/PRNUMBER/index.html`. These docs are republished on every commit to a pull request made to PhotonVision/photonvision-docs. For example, PR 325 would have pre-merge documentation published to `https://photonvision-docs--325.org.readthedocs.build/en/325/index.html`. Additionally, the pull request will have a link directly to the pre-release build of the docs. This build only runs when there is a change to files in the docs sub-folder.
 
 ## Style Guide
 

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

@@ -8,7 +8,7 @@ This section contains the build instructions from the source code available at [
 
 **Java Development Kit:**
 
- This project requires Java Development Kit (JDK) 17 to be compiled. This is the same Java version that comes with WPILib for 2025+. If you don't have this JDK with WPILib, you can follow the instructions to install JDK 17 for your platform [here](https://bell-sw.com/pages/downloads/#jdk-17-lts).
+ This project requires Java Development Kit (JDK) 17 to be compiled. This is the same Java version that comes with WPILib for 2025+. **Windows Users must use the JDK that ships with WPILib.** For other platforms, you can follow the instructions to install JDK 17 for your platform [here](https://bell-sw.com/pages/downloads/#jdk-17-lts).
 
 **Node JS:**
 
@@ -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/index.md

@@ -3,4 +3,5 @@
 ```{toctree}
 :maxdepth: 1
 image-rotation
+time-sync
 ```

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

@@ -0,0 +1,111 @@
+# Time Synchronization Protocol Specification, Version 1.0
+
+Protocol Revision 1.0, 08/25/2024
+
+## Background
+
+In a distributed compute environment like robots, time synchronization between computers is increasingly important. Currently, [NetworkTables Version 4.1](https://github.com/wpilibsuite/allwpilib/blob/main/ntcore/doc/networktables4.adoc) provides support for time synchronization of clients with the NetworkTables server using binary PING/PONG messages sent over WebSockets. This approach, while fundamentally the same as is described in this memo, has demonstrated some opportunities for improvement:
+
+- PING/PONG messages are processed in the same queue as other NetworkTables messages. Depending on the underlying implementation and processor speed, this can incur message processing delays and increase client-calculated Round-Trip Time (RTT), and cause messages to arrive at the server timestamped in the future.
+- Messages use WebSockets over TCP for their transport layer. We don't need the robustness guarantees of TCP as our connection is stateless.
+
+For these reasons, a time synchronization solution separate from NetworkTables communication was desired. Architecture decisions made to address these issues are:
+
+- Use the User Datagram Protocol (UDP) transport layer, as we don't need the robustness guarantees afforded by TCP. As a Client, if a PING isn't replied to, we'll just try again at the start of the next PING window. As a bonus, we are free to use UDP port 5810 as NetworkTables only uses TCP Port 5810/5811 as of Version 4.1.
+- Use a separate thread from the current NetworkTables libUV runner.
+
+
+## Prior Art
+
+The [NetworkTables 4.1 timestamp synchronization](https://github.com/wpilibsuite/allwpilib/blob/main/ntcore/doc/networktables4.adoc#timestamps) approach, an implementation of [Cristian's Algorithm](https://en.wikipedia.org/wiki/Cristian%27s_algorithm). We also implement Cristian’s Algorithm.
+
+The [Precision Time Protocol](https://en.wikipedia.org/wiki/Precision_Time_Protocol#Synchronization) at it's core does something similar with Sync/Delay_Req/Delay_Resp. We do not have (guaranteed) access to hardware timestamping, but we utilize this PING/PONG pattern to estimate total round-trip time.
+
+
+## Roles
+
+```{graphviz}
+digraph CristianAlgorithm {
+    ratio=0.5;
+    bgcolor="transparent";
+
+    node [
+        fontcolor = "#e6e6e6",
+        style = filled,
+        color = "#e6e6e6",
+        fillcolor = "#333333"
+        fontsize=10;
+    ]
+
+    edge [
+        color = "#e6e6e6",
+        fontcolor = "#e6e6e6"
+        fontsize=10;
+    ]
+
+    rankdir=LR;
+    node [shape=box, style=filled, color=lightblue];
+
+    user_send [label="User Sends T1"];
+    server_receive [label="Server Receives T1"];
+    server_send [label="Server Sends T2"];
+    user_receive [label="User Receives T2"];
+    user_compute [label="User Computes Time"];
+
+    user_send -> server_receive [label="T1 (Request)"];
+    server_receive -> server_send [label="T1 received by server"];
+    server_send -> user_receive [label="T2 sent by server"];
+    user_receive -> user_compute [label="T2 received by user"];
+    user_compute -> user_send [label="Computed Time: T3 = T2 + (deltaT2 - deltaT1)/2"];
+}
+```
+
+Time Synchronization Protocol (TSP) participants can assume either a server role or a client role. The server role is responsible for listening for incoming time synchronization requests from clients and replying appropriately. The client role is responsible for sending "Ping" messages to the server and listening for "Pong" replies to estimate the offset between the server and client time bases.
+
+All time values shall use units of microseconds. The epoch of the time base this is measured against is unspecified.
+
+Clients shall periodically (e.g. every few seconds) send, in a manner that minimizes transmission delays, a **TSP Ping Message** that contains the client's current local time.
+
+When the server receives a **TSP Ping Message** from any client, it shall respond to the client, in a manner that minimizes transmission delays, with a **TSP Pong message** encoding a timestamp of its (the server's) current local time (in microseconds), and the client-provided data value.
+
+When the client receives a **TSP Pong Message** from the server, it shall verify that the `Client Local Time` corresponds to the currently in-flight TSP Ping message; if not, it shall drop this packet. The round trip time (RTT) shall be computed from the delta between the message's data value and the current local time.  If the RTT is less than that from previous measurements, the client shall use the timestamp in the message plus ½ the RTT as the server time equivalent to the current local time, and use this equivalence to compute server time base timestamps from local time for future messages.
+
+## Transport
+
+Communication between server and clients shall occur over the User Datagram Protocol (UDP) Port 5810.
+
+## Message Format
+
+The message format forgoes CRCs (as these are provided by the Ethernet physical layer) or packet delimination (as our packetsa are assumed be under the network MTU). **TSP Ping** and **TSP Pong** messages shall be encoded in a manor compatible with a WPILib packed struct with respect to byte alignment and endienness.
+
+### TSP Ping
+
+| Offset | Format | Data | Notes |
+| ------ | ------ | ---- | ----- |
+| 0 | uint8 | Protocol version | This field shall always set to 1 (0b1) for TSP Version 1. |
+| 1 | uint8 | Message ID | This field shall always be set to 1 (0b1). |
+| 2 | uint64 | Client Local Time | The client's local time value, at the time this Ping message was sent. |
+
+### TSP Pong
+
+| Offset | Format | Data | Notes |
+| ------ | ------ | ---- | ----- |
+| 0 | uint8 | Protocol version | This field shall always set to 1 (0b1) for TSP Version 1.
+| 1 | uint8 | Message ID | This field shall always be set to 2 (0b2).
+| 2 | uint64 | Client Local Time | The client's local time value from the Ping message that this Pong is generated in response to.
+| 10 | uint64 | Server Local Time | The current time at the server, at the time this Pong message was sent.
+
+
+## Optional Protocol Extensions
+
+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 |
+| pong_rx_time_us | Integer | The time, in client local time, that the last pong was received |
+| rtt2_us | Integer | The time in us from last complete (ping transmission to pong reception) |
+
+PhotonVision has chosen to publish to the sub-table `/photonvision/.timesync/{DEVICE_HOSTNAME}`. Future implementations of this protocol may decide to implement this as a structured data type.

docs/source/docs/installation/sw_install/raspberry-pi.md

@@ -7,7 +7,7 @@ A Pre-Built Raspberry Pi image is available for ease of installation.
 Download the latest release of the PhotonVision Raspberry image (.xz file) from the [releases page](https://github.com/PhotonVision/photonvision/releases). You do not need to extract the downloaded ZIP file.
 
 :::{note}
-Make sure you download the image that ends in '-RasberryPi.xz'.
+Make sure you download the image that ends in '-RaspberryPi.xz'.
 :::
 
 ## Flashing the Pi Image

docs/source/docs/programming/photonlib/controlling-led.md

@@ -14,7 +14,7 @@ You can control the vision LEDs of supported hardware via PhotonLib using the `s
       // Blink the LEDs.
       camera.SetLED(photonlib::VisionLEDMode::kBlink);
 
-    .. code-block:: Python
+   .. code-block:: Python
 
         # Coming Soon!
 ```

docs/source/docs/programming/photonlib/driver-mode-pipeline-index.md

@@ -62,7 +62,7 @@ You can also get the pipeline latency from a pipeline result using the `getLaten
       // Get the pipeline latency.
       units::second_t latency = result.GetLatency();
 
-    .. code-block:: Python
+   .. code-block:: Python
 
         # Coming Soon!
 ```

docs/source/docs/programming/photonlib/robot-pose-estimator.md

@@ -24,7 +24,7 @@ The API documentation can be found in here: [Java](https://github.wpilib.org/all
       // The parameter for LoadAPrilTagLayoutField will be different depending on the game.
       frc::AprilTagFieldLayout aprilTagFieldLayout = frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo);
 
-    .. code-block:: Python
+   .. code-block:: Python
 
         # Coming Soon!
 
@@ -81,7 +81,7 @@ The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (s
       photonlib::RobotPoseEstimator estimator(
           aprilTags, photonlib::CLOSEST_TO_REFERENCE_POSE, cameras);
 
-    .. code-block:: Python
+   .. code-block:: Python
 
         kRobotToCam = wpimath.geometry.Transform3d(
             wpimath.geometry.Translation3d(0.5, 0.0, 0.5),
@@ -123,7 +123,9 @@ Calling `update()` on your `PhotonPoseEstimator` will return an `EstimatedRobotP
         }
       }
 
-    .. code-block:: Python
+   .. code-block:: Python
+
+      # Coming Soon!
 
 
 

photon-client/src/components/settings/NetworkingCard.vue

@@ -17,10 +17,10 @@ const resetTempSettingsStruct = () => {
 const settingsValid = ref(true);
 
 const isValidNetworkTablesIP = (v: string | undefined): boolean => {
-  // Check if it is a valid team number between 1-9999
-  const teamNumberRegex = /^[1-9][0-9]{0,3}$/;
+  // Check if it is a valid team number between 1-99999 (5 digits)
+  const teamNumberRegex = /^[1-9][0-9]{0,4}$/;
   // Check if it is a team number longer than 5 digits
-  const badTeamNumberRegex = /^[0-9]{5,}$/;
+  const badTeamNumberRegex = /^[0-9]{6,}$/;
 
   if (v === undefined) return false;
   if (teamNumberRegex.test(v)) return true;

photon-client/src/types/SettingTypes.ts

@@ -142,7 +142,7 @@ export interface CameraCalibrationResult {
   distCoeffs: JsonMatOfDouble;
   observations: BoardObservation[];
   calobjectWarp?: number[];
-  // We might have to omit observations for bandwith, so backend will send us this
+  // We might have to omit observations for bandwidth, so backend will send us this
   numSnapshots: number;
   meanErrors: number[];
 }

photon-core/build.gradle

@@ -4,6 +4,7 @@ plugins {
 
 import java.nio.file.Path
 
+ext.licenseFile = file("$rootDir/LICENSE")
 apply from: "${rootDir}/shared/common.gradle"
 
 wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get()
@@ -17,6 +18,7 @@ def nativeTasks = wpilibTools.createExtractionTasks {
 
 nativeTasks.addToSourceSetResources(sourceSets.main)
 
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpilibc")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
@@ -24,7 +26,7 @@ nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
-nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
+nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, wpi.versions.opencvVersion.get())
 
 dependencies {
     // JOGL stuff (currently we only distribute for aarch64, which is Pi 4)

photon-core/settings.gradle

@@ -1 +0,0 @@
-rootProject.name = 'photon-core'

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

@@ -84,15 +84,7 @@ public class CameraConfiguration {
         this.calibrations = new ArrayList<>();
         this.otherPaths = alternates;
 
-        logger.debug(
-                "Creating USB camera configuration for "
-                        + cameraType
-                        + " "
-                        + baseName
-                        + " (AKA "
-                        + nickname
-                        + ") at "
-                        + path);
+        logger.debug("Creating USB camera configuration for " + this.toShortString());
     }
 
     @JsonCreator
@@ -120,15 +112,7 @@ public class CameraConfiguration {
         this.usbPID = usbPID;
         this.usbVID = usbVID;
 
-        logger.debug(
-                "Creating camera configuration for "
-                        + cameraType
-                        + " "
-                        + baseName
-                        + " (AKA "
-                        + nickname
-                        + ") at "
-                        + path);
+        logger.debug("Loaded camera configuration for " + toShortString());
     }
 
     public void addPipelineSettings(List<CVPipelineSettings> settings) {
@@ -189,6 +173,30 @@ public class CameraConfiguration {
         return Arrays.stream(otherPaths).filter(path -> path.contains("/by-path/")).findFirst();
     }
 
+    public String toShortString() {
+        return "CameraConfiguration [baseName="
+                + baseName
+                + ", uniqueName="
+                + uniqueName
+                + ", nickname="
+                + nickname
+                + ", path="
+                + path
+                + ", otherPaths="
+                + Arrays.toString(otherPaths)
+                + ", cameraType="
+                + cameraType
+                + ", cameraQuirks="
+                + cameraQuirks
+                + ", FOV="
+                + FOV
+                + "]"
+                + ", PID="
+                + usbPID
+                + ", VID="
+                + usbVID;
+    }
+
     @Override
     public String toString() {
         return "CameraConfiguration [baseName="

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

@@ -340,7 +340,8 @@ public class ConfigManager {
 
     /**
      * Disable flushing settings to disk as part of our JVM exit hook. Used to prevent uploading all
-     * settings from getting its new configs overwritten at program exit and before theyre all loaded.
+     * settings from getting its new configs overwritten at program exit and before they're all
+     * loaded.
      */
     public void disableFlushOnShutdown() {
         this.flushOnShutdown = false;

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/configuration/SqlConfigProvider.java

@@ -265,7 +265,7 @@ public class SqlConfigProvider extends ConfigProvider {
                         JacksonUtils.deserialize(
                                 getOneConfigFile(conn, GlobalKeys.HARDWARE_CONFIG), HardwareConfig.class);
             } catch (IOException e) {
-                logger.error("Could not deserialize hardware config! Loading defaults");
+                logger.error("Could not deserialize hardware config! Loading defaults", e);
                 hardwareConfig = new HardwareConfig();
             }
 
@@ -274,7 +274,7 @@ public class SqlConfigProvider extends ConfigProvider {
                         JacksonUtils.deserialize(
                                 getOneConfigFile(conn, GlobalKeys.HARDWARE_SETTINGS), HardwareSettings.class);
             } catch (IOException e) {
-                logger.error("Could not deserialize hardware settings! Loading defaults");
+                logger.error("Could not deserialize hardware settings! Loading defaults", e);
                 hardwareSettings = new HardwareSettings();
             }
 
@@ -283,7 +283,7 @@ public class SqlConfigProvider extends ConfigProvider {
                         JacksonUtils.deserialize(
                                 getOneConfigFile(conn, GlobalKeys.NETWORK_CONFIG), NetworkConfig.class);
             } catch (IOException e) {
-                logger.error("Could not deserialize network config! Loading defaults");
+                logger.error("Could not deserialize network config! Loading defaults", e);
                 networkConfig = new NetworkConfig();
             }
 
@@ -292,7 +292,7 @@ public class SqlConfigProvider extends ConfigProvider {
                         JacksonUtils.deserialize(
                                 getOneConfigFile(conn, GlobalKeys.ATFL_CONFIG_FILE), AprilTagFieldLayout.class);
             } catch (IOException e) {
-                logger.error("Could not deserialize apriltag layout! Loading defaults");
+                logger.error("Could not deserialize apriltag layout! Loading defaults", e);
                 try {
                     atfl = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
                 } catch (UncheckedIOException e2) {

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

@@ -20,7 +20,7 @@ package org.photonvision.common.dataflow.networktables;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEvent;
-import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.networktables.NetworkTablesJNI;
 import java.util.List;
 import java.util.function.BooleanSupplier;
 import java.util.function.Consumer;
@@ -146,13 +146,19 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
                             List.of(),
                             result.inputAndOutputFrame);
         else acceptedResult = result;
-        var now = WPIUtilJNI.now();
-        var captureMicros = MathUtils.nanosToMicros(acceptedResult.getImageCaptureTimestampNanos());
+        var now = NetworkTablesJNI.now();
+        var captureMicros = MathUtils.nanosToMicros(result.getImageCaptureTimestampNanos());
+
+        var offset = NetworkTablesManager.getInstance().getOffset();
+
+        // Transform the metadata timestamps from the local nt::Now timebase to the Time Sync Server's
+        // timebase
         var simplified =
                 new PhotonPipelineResult(
                         acceptedResult.sequenceID,
-                        captureMicros,
-                        now,
+                        captureMicros + offset,
+                        now + offset,
+                        NetworkTablesManager.getInstance().getTimeSinceLastPong(),
                         TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets),
                         acceptedResult.multiTagResult);
 

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

@@ -18,6 +18,7 @@
 package org.photonvision.common.dataflow.networktables;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.networktables.LogMessage;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEvent;
 import edu.wpi.first.networktables.NetworkTableEvent.Kind;
@@ -26,7 +27,6 @@ import edu.wpi.first.networktables.StringSubscriber;
 import java.io.IOException;
 import java.util.EnumSet;
 import java.util.HashMap;
-import java.util.function.Consumer;
 import org.photonvision.PhotonVersion;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.configuration.NetworkConfig;
@@ -34,6 +34,7 @@ import org.photonvision.common.dataflow.DataChangeService;
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.hardware.HardwareManager;
 import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.LogLevel;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.scripting.ScriptEventType;
 import org.photonvision.common.scripting.ScriptManager;
@@ -41,32 +42,39 @@ import org.photonvision.common.util.TimedTaskManager;
 import org.photonvision.common.util.file.JacksonUtils;
 
 public class NetworkTablesManager {
+    private static final Logger logger =
+            new Logger(NetworkTablesManager.class, LogGroup.NetworkTables);
+
     private final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
     private final String kRootTableName = "/photonvision";
     private final String kFieldLayoutName = "apriltag_field_layout";
     public final NetworkTable kRootTable = ntInstance.getTable(kRootTableName);
 
-    private final NTLogger m_ntLogger = new NTLogger();
-
     private boolean m_isRetryingConnection = false;
 
     private StringSubscriber m_fieldLayoutSubscriber =
             kRootTable.getStringTopic(kFieldLayoutName).subscribe("");
 
+    private final TimeSyncManager m_timeSync = new TimeSyncManager(kRootTable);
+
     private NetworkTablesManager() {
-        ntInstance.addLogger(255, 255, (event) -> {}); // to hide error messages
-        ntInstance.addConnectionListener(true, m_ntLogger); // to hide error messages
+        ntInstance.addLogger(
+                LogMessage.kInfo, LogMessage.kCritical, this::logNtMessage); // to hide error messages
+        ntInstance.addConnectionListener(true, this::checkNtConnectState); // to hide error messages
 
         ntInstance.addListener(
                 m_fieldLayoutSubscriber, EnumSet.of(Kind.kValueAll), this::onFieldLayoutChanged);
 
-        TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000);
-
         // Get the UI state in sync with the backend. NT should fire a callback when it first connects
         // to the robot
         broadcastConnectedStatus();
     }
 
+    public void registerTimedTasks() {
+        m_timeSync.start();
+        TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000);
+    }
+
     private static NetworkTablesManager INSTANCE;
 
     public static NetworkTablesManager getInstance() {
@@ -74,43 +82,72 @@ public class NetworkTablesManager {
         return INSTANCE;
     }
 
-    private static final Logger logger = new Logger(NetworkTablesManager.class, LogGroup.General);
+    private void logNtMessage(NetworkTableEvent event) {
+        String levelmsg = "DEBUG";
+        LogLevel pvlevel = LogLevel.DEBUG;
+        if (event.logMessage.level >= LogMessage.kCritical) {
+            pvlevel = LogLevel.ERROR;
+            levelmsg = "CRITICAL";
+        } else if (event.logMessage.level >= LogMessage.kError) {
+            pvlevel = LogLevel.ERROR;
+            levelmsg = "ERROR";
+        } else if (event.logMessage.level >= LogMessage.kWarning) {
+            pvlevel = LogLevel.WARN;
+            levelmsg = "WARNING";
+        } else if (event.logMessage.level >= LogMessage.kInfo) {
+            pvlevel = LogLevel.INFO;
+            levelmsg = "INFO";
+        }
 
-    private static class NTLogger implements Consumer<NetworkTableEvent> {
-        private boolean hasReportedConnectionFailure = false;
+        logger.log(
+                "NT: "
+                        + levelmsg
+                        + " "
+                        + event.logMessage.level
+                        + ": "
+                        + event.logMessage.message
+                        + " ("
+                        + event.logMessage.filename
+                        + ":"
+                        + event.logMessage.line
+                        + ")",
+                pvlevel);
+    }
 
-        @Override
-        public void accept(NetworkTableEvent event) {
-            var isConnEvent = event.is(Kind.kConnected);
-            var isDisconnEvent = event.is(Kind.kDisconnected);
+    public void checkNtConnectState(NetworkTableEvent event) {
+        var isConnEvent = event.is(Kind.kConnected);
+        var isDisconnEvent = event.is(Kind.kDisconnected);
 
-            if (!hasReportedConnectionFailure && isDisconnEvent) {
-                var msg =
-                        String.format(
-                                "NT lost connection to %s:%d! (NT version %d). Will retry in background.",
-                                event.connInfo.remote_ip,
-                                event.connInfo.remote_port,
-                                event.connInfo.protocol_version);
-                logger.error(msg);
-                HardwareManager.getInstance().setNTConnected(false);
+        if (isDisconnEvent) {
+            var msg =
+                    String.format(
+                            "NT lost connection to %s:%d! (NT version %d). Will retry in background.",
+                            event.connInfo.remote_ip,
+                            event.connInfo.remote_port,
+                            event.connInfo.protocol_version);
+            logger.error(msg);
+            HardwareManager.getInstance().setNTConnected(false);
 
-                hasReportedConnectionFailure = true;
-                getInstance().broadcastConnectedStatus();
-            } else if (isConnEvent && event.connInfo != null) {
-                var msg =
-                        String.format(
-                                "NT connected to %s:%d! (NT version %d)",
-                                event.connInfo.remote_ip,
-                                event.connInfo.remote_port,
-                                event.connInfo.protocol_version);
-                logger.info(msg);
-                HardwareManager.getInstance().setNTConnected(true);
+            getInstance().broadcastConnectedStatus();
+        } else if (isConnEvent && event.connInfo != null) {
+            var msg =
+                    String.format(
+                            "NT connected to %s:%d! (NT version %d)",
+                            event.connInfo.remote_ip,
+                            event.connInfo.remote_port,
+                            event.connInfo.protocol_version);
+            logger.info(msg);
+            HardwareManager.getInstance().setNTConnected(true);
 
-                hasReportedConnectionFailure = false;
-                ScriptManager.queueEvent(ScriptEventType.kNTConnected);
-                getInstance().broadcastVersion();
-                getInstance().broadcastConnectedStatus();
-            }
+            ScriptManager.queueEvent(ScriptEventType.kNTConnected);
+            getInstance().broadcastVersion();
+            getInstance().broadcastConnectedStatus();
+
+            m_timeSync.reportNtConnected();
+        } else if (isConnEvent) {
+            logger.warn("Got connection event with no connection info??");
+        } else {
+            logger.warn("Got a non-sensical connection message that is neither connect nor disconnect?");
         }
     }
 
@@ -168,9 +205,16 @@ public class NetworkTablesManager {
         } else {
             setClientMode(config.ntServerAddress);
         }
+
+        m_timeSync.setConfig(config);
+
         broadcastVersion();
     }
 
+    public long getOffset() {
+        return m_timeSync.getOffset();
+    }
+
     private void setClientMode(String ntServerAddress) {
         ntInstance.stopServer();
         ntInstance.startClient4("photonvision");
@@ -211,4 +255,8 @@ public class NetworkTablesManager {
                     "[NetworkTablesManager] Could not connect to the robot! Will retry in the background...");
         }
     }
+
+    public long getTimeSinceLastPong() {
+        return m_timeSync.getTimeSinceLastPong();
+    }
 }

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

@@ -0,0 +1,169 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.dataflow.networktables;
+
+import edu.wpi.first.cscore.CameraServerJNI;
+import edu.wpi.first.networktables.IntegerPublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import org.photonvision.common.configuration.NetworkConfig;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.common.util.TimedTaskManager;
+import org.photonvision.jni.PhotonTargetingJniLoader;
+import org.photonvision.jni.TimeSyncClient;
+import org.photonvision.jni.TimeSyncServer;
+
+public class TimeSyncManager {
+    private static final Logger logger = new Logger(TimeSyncManager.class, LogGroup.NetworkTables);
+
+    private TimeSyncClient m_client = null;
+    private TimeSyncServer m_server = null;
+
+    private NetworkTableInstance ntInstance;
+    IntegerPublisher m_offsetPub;
+    IntegerPublisher m_rtt2Pub;
+    IntegerPublisher m_pingsPub;
+    IntegerPublisher m_pongsPub;
+    IntegerPublisher m_lastPongTimePub;
+
+    public TimeSyncManager(NetworkTable kRootTable) {
+        if (!PhotonTargetingJniLoader.isWorking) {
+            logger.error("PhotonTargetingJNI was not loaded! Cannot do time-sync");
+        }
+
+        this.ntInstance = kRootTable.getInstance();
+
+        // Need this subtable to be unique per coprocessor. TODO: consider using MAC address or
+        // something similar for metrics?
+        var timeTable = kRootTable.getSubTable(".timesync").getSubTable(CameraServerJNI.getHostname());
+        m_offsetPub = timeTable.getIntegerTopic("offset_us").publish();
+        m_rtt2Pub = timeTable.getIntegerTopic("rtt2_us").publish();
+        m_pingsPub = timeTable.getIntegerTopic("ping_tx_count").publish();
+        m_pongsPub = timeTable.getIntegerTopic("pong_rx_count").publish();
+        m_lastPongTimePub = timeTable.getIntegerTopic("pong_rx_time_us").publish();
+
+        // default to being a client
+        logger.debug("Starting TimeSyncClient on localhost (for now)");
+        m_client = new TimeSyncClient("127.0.0.1", 5810, 1.0);
+    }
+
+    // Since we're spinning off tasks in a new thread, be careful and start it seperately
+    public void start() {
+        if (!PhotonTargetingJniLoader.isWorking) {
+            logger.error("PhotonTargetingJNI was not loaded! Cannot start");
+        }
+
+        TimedTaskManager.getInstance().addTask("TimeSyncManager::tick", this::tick, 1000);
+    }
+
+    public synchronized long getOffset() {
+        if (!PhotonTargetingJniLoader.isWorking) {
+            return 0;
+        }
+
+        // if we're a client, return the offset to server time
+        if (m_client != null) return m_client.getOffset();
+        // if we're a server, our time (nt::Now) is the same as network time
+        if (m_server != null) return 0;
+
+        // ????? should never hit
+        logger.error("Client and server and null?");
+        return 0;
+    }
+
+    synchronized void setConfig(NetworkConfig config) {
+        if (!PhotonTargetingJniLoader.isWorking) {
+            return;
+        }
+
+        if (m_client == null && m_server == null) {
+            throw new RuntimeException("Neither client nor server are null?");
+        }
+
+        // if not already running a server, set it up
+        if (config.runNTServer && m_server == null) {
+            // tear down anything old
+            if (m_client != null) {
+                logger.debug("Tearing down old client");
+                m_client.stop();
+                m_client = null;
+            }
+
+            logger.debug("Starting TimeSyncServer");
+            m_server = new TimeSyncServer(5810);
+            m_server.start();
+        } else
+        // if not already running a client, set it up
+        if (m_client == null) {
+            // tear down anything old
+            if (m_server != null) {
+                logger.debug("Tearing down old server");
+                m_server.stop();
+                m_server = null;
+            }
+
+            // Guess at IP -- tick will take care of changing this (may take up to 1 second)
+            logger.debug("Starting TimeSyncClient on localhost (for now)");
+            m_client = new TimeSyncClient("127.0.0.1", 5810, 1.0);
+        }
+    }
+
+    synchronized void tick() {
+        if (m_client != null) {
+            var conns = ntInstance.getConnections();
+
+            if (conns.length > 0) {
+                logger.debug("Changing TimeSyncClient server to " + conns[0].remote_ip);
+                m_client.setServer(conns[0].remote_ip);
+            }
+
+            if (m_client != null) {
+                var m = m_client.getPingMetadata();
+
+                m_offsetPub.set(m.offset);
+                m_rtt2Pub.set(m.rtt2);
+                m_pingsPub.set(m.pingsSent);
+                m_pongsPub.set(m.pongsReceived);
+                m_lastPongTimePub.set(m.lastPongTime);
+            }
+        }
+    }
+
+    public synchronized long getTimeSinceLastPong() {
+        if (m_client != null) {
+            return m_client.getPingMetadata().timeSinceLastPong();
+        } else if (m_server != null) {
+            return 0;
+        } else {
+            // ????
+            return 0;
+        }
+    }
+
+    /** Restart our timesync client if NT just connected */
+    public synchronized void reportNtConnected() {
+        if (m_client != null) {
+            // restart (in java code; we could just add a reset metrics function...)
+            logger.debug(
+                    "NT (re)connected -- restarting Time Sync Client at " + m_client.getServer() + ":5810");
+            m_client.stop();
+            m_client = new TimeSyncClient(m_client.getServer(), 5810, 1.0);
+        }
+    }
+}

photon-core/src/main/java/org/photonvision/common/hardware/GPIO/pi/PigpioCommand.java

@@ -28,7 +28,7 @@ public enum PigpioCommand {
     PCMD_WVDEL(50), // int wave_delete(unsigned wave_id)
     PCMD_WVTX(51), // int wave_tx_send(unsigned wave_id) (once)
     PCMD_WVTXR(52), // int wave_tx_send(unsigned wave_id) (repeat)
-    PCMD_GDC(83), // int get_duty_cyle(unsigned user_gpio)
+    PCMD_GDC(83), // int get_duty_cycle(unsigned user_gpio)
     PCMD_HP(86), // int hardware_pwm(unsigned gpio, unsigned PWMfreq, unsigned PWMduty)
     PCMD_WVTXM(100); // int wave_tx_send(unsigned wave_id, unsigned wave_mode)
 

photon-core/src/main/java/org/photonvision/common/hardware/metrics/cmds/RK3588Cmds.java

@@ -27,7 +27,7 @@ public class RK3588Cmds extends LinuxCmds {
         // CPU Temperature
         /* The RK3588 chip has 7 thermal zones that can be accessed via:
          *       /sys/class/thermal/thermal_zoneX/temp
-         * where X is an interger from 0 to 6.
+         * where X is an integer from 0 to 6.
          *
          * || Zone || Location    || Comments                                                ||
          * |    0  |  soc         |  soc thermal (near the center of the chip)                |

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

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

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

@@ -25,4 +25,6 @@ public enum LogGroup {
     General,
     Config,
     CSCore,
+    NetworkTables,
+    System,
 }

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

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

photon-core/src/main/java/org/photonvision/common/networking/NetworkManager.java

@@ -19,6 +19,7 @@ package org.photonvision.common.networking;
 
 import java.nio.file.Files;
 import java.nio.file.Path;
+import java.nio.file.Paths;
 import java.util.NoSuchElementException;
 
 import org.photonvision.common.configuration.ConfigManager;
@@ -71,10 +72,7 @@ public class NetworkManager {
         // Start tasks to monitor the network interface(s)
         var ethernetDevices = NetworkUtils.getAllWiredInterfaces();
         for (NMDeviceInfo deviceInfo : ethernetDevices) {
-            var task = "deviceStatus-" + deviceInfo.devName;
-            if (!TimedTaskManager.getInstance().taskActive(task)) {
-                TimedTaskManager.getInstance().addTask(task, deviceStatus(deviceInfo.devName), 5000);
-            }
+            monitorDevice(deviceInfo.devName, 5000);
         }
 
         var physicalDevices = NetworkUtils.getAllActiveWiredInterfaces();
@@ -258,14 +256,22 @@ public class NetworkManager {
     }
 
     // Detects changes in the carrier and reinitializes after re-connect
-    private Runnable deviceStatus(String devName) {
-        Path file = Path.of("/sys/class/net/{device}/carrier".replace("{device}", devName));
-        logger.debug("Watching network interface at path: " + file.toString());
-        var last = new Object() {boolean carrier = true;};
-        return () ->
-        {
+    private void monitorDevice(String devName, int millisInterval) {
+        String taskName = "deviceStatus-" + devName;
+        if (TimedTaskManager.getInstance().taskActive(taskName)) {
+            // task is already running
+            return;
+        }
+        Path path = Paths.get("/sys/class/net/{device}/carrier".replace("{device}", devName));
+        if (Files.notExists(path)) {
+            logger.error("Can't find " + path + ", so can't monitor " + devName);
+            return;
+        }
+        logger.debug("Watching network interface at path: " + path);
+        var last = new Object() {boolean carrier = true; boolean exceptionLogged = false;};
+        Runnable task = () -> {
             try {
-                boolean carrier = Files.readString(file).trim().equals("1");
+                boolean carrier = Files.readString(path).trim().equals("1");
                 if (carrier != last.carrier) {
                     if (carrier) {
                         // carrier came back
@@ -276,9 +282,16 @@ public class NetworkManager {
                     }
                 }
                 last.carrier = carrier;
-            } catch (Exception e) {
-                logger.error("Could not check network status", e);
-            }
-        };
+                last.exceptionLogged = false;
+                } catch (Exception e) {
+                    if (!last.exceptionLogged) {
+                        // Log the exception only once, but keep trying
+                        logger.error("Could not check network status for " + devName, e);
+                        last.exceptionLogged = true;
+                    }
+                }
+            };
+
+        TimedTaskManager.getInstance().addTask(taskName, task, millisInterval);
     }
 }

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

@@ -18,71 +18,20 @@
 package org.photonvision.common.util;
 
 import com.fasterxml.jackson.databind.ObjectMapper;
-import edu.wpi.first.apriltag.jni.AprilTagJNI;
-import edu.wpi.first.cscore.CameraServerCvJNI;
-import edu.wpi.first.cscore.CameraServerJNI;
-import edu.wpi.first.hal.JNIWrapper;
-import edu.wpi.first.math.WPIMathJNI;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.net.WPINetJNI;
-import edu.wpi.first.networktables.NetworkTablesJNI;
-import edu.wpi.first.util.CombinedRuntimeLoader;
-import edu.wpi.first.util.WPIUtilJNI;
 import java.awt.HeadlessException;
 import java.io.File;
 import java.io.IOException;
 import java.nio.file.Path;
-import org.opencv.core.Core;
 import org.opencv.core.Mat;
 import org.opencv.highgui.HighGui;
+import org.photonvision.jni.WpilibLoader;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 
 public class TestUtils {
-    private static boolean has_loaded = false;
-
     public static boolean loadLibraries() {
-        if (has_loaded) return true;
-
-        NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
-        WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
-        WPIMathJNI.Helper.setExtractOnStaticLoad(false);
-        CameraServerJNI.Helper.setExtractOnStaticLoad(false);
-        CameraServerCvJNI.Helper.setExtractOnStaticLoad(false);
-        // OpenCvLoader.Helper.setExtractOnStaticLoad(false);
-        JNIWrapper.Helper.setExtractOnStaticLoad(false);
-        WPINetJNI.Helper.setExtractOnStaticLoad(false);
-        AprilTagJNI.Helper.setExtractOnStaticLoad(false);
-
-        // wpimathjni is a bit odd, it's all in the wpimathjni shared lib, but the java side stuff has
-        // been split.
-        // ArmFeedforwardJNI.Helper.setExtractOnStaticLoad(false);
-        // DAREJNI.Helper.setExtractOnStaticLoad(false);
-        // EigenJNI.Helper.setExtractOnStaticLoad(false);
-        // Ellipse2dJNI.Helper.setExtractOnStaticLoad(false);
-        // Pose3dJNI.Helper.setExtractOnStaticLoad(false);
-        // StateSpaceUtilJNI.Helper.setExtractOnStaticLoad(false);
-        // TrajectoryUtilJNI.Helper.setExtractOnStaticLoad(false);
-
-        try {
-            CombinedRuntimeLoader.loadLibraries(
-                    TestUtils.class,
-                    "wpiutiljni",
-                    "wpimathjni",
-                    "ntcorejni",
-                    "wpinetjni",
-                    "wpiHaljni",
-                    Core.NATIVE_LIBRARY_NAME,
-                    "cscorejni",
-                    "apriltagjni");
-
-            has_loaded = true;
-        } catch (IOException e) {
-            e.printStackTrace();
-            has_loaded = false;
-        }
-
-        return has_loaded;
+        return WpilibLoader.loadLibraries();
     }
 
     @SuppressWarnings("unused")

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

@@ -33,6 +33,7 @@ import java.io.IOException;
 import java.nio.file.Path;
 import java.util.HashMap;
 import java.util.Map;
+import org.eclipse.jetty.io.EofException;
 
 public class JacksonUtils {
     public static class UIMap extends HashMap<String, Object> {}
@@ -76,6 +77,10 @@ public class JacksonUtils {
     }
 
     public static <T> T deserialize(String s, Class<T> ref) throws IOException {
+        if (s.length() == 0) {
+            throw new EofException("Provided empty string for class " + ref.getName());
+        }
+
         PolymorphicTypeValidator ptv =
                 BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build();
         ObjectMapper objectMapper =

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

@@ -25,7 +25,7 @@ import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.networktables.NetworkTablesJNI;
 import java.util.Arrays;
 import java.util.List;
 import org.opencv.core.Core;
@@ -98,7 +98,7 @@ public class MathUtils {
     }
 
     public static long wpiNanoTime() {
-        return microsToNanos(WPIUtilJNI.now());
+        return microsToNanos(NetworkTablesJNI.now());
     }
 
     /**

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

@@ -143,7 +143,7 @@ public class QuirkyCamera {
      * @param usbVid USB VID of camera
      * @param usbPid USB PID of camera
      * @param baseName CSCore name of camera
-     * @param displayName Human-friendly quicky camera name
+     * @param displayName Human-friendly quirky camera name
      * @param quirks Camera quirks
      */
     private QuirkyCamera(
@@ -160,7 +160,7 @@ public class QuirkyCamera {
             this.quirks.put(q, true);
         }
 
-        // (2) for all other quirks in CameraQuirks (in this version of Photon), defalut to false
+        // (2) for all other quirks in CameraQuirks (in this version of Photon), default to false
         for (var q : CameraQuirk.values()) {
             this.quirks.putIfAbsent(q, false);
         }

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

@@ -96,9 +96,12 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
         var autoExpProp = findProperty("exposure_auto", "auto_exposure");
 
         exposureAbsProp = expProp.get();
-        autoExposureProp = autoExpProp.get();
         this.minExposure = exposureAbsProp.getMin();
         this.maxExposure = exposureAbsProp.getMax();
+
+        if (autoExpProp.isPresent()) {
+            autoExposureProp = autoExpProp.get();
+        }
     }
 
     public void setAllCamDefaults() {
@@ -169,7 +172,7 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
             softSet("auto_exposure_bias", 0);
             softSet("iso_sensitivity_auto", 0); // Disable auto ISO adjustment
             softSet("iso_sensitivity", 0); // Manual ISO adjustment
-            autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
+            if (autoExposureProp != null) autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
 
             // Most cameras leave exposure time absolute at the last value from their AE
             // algorithm.
@@ -199,7 +202,7 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
     public void setExposureRaw(double exposureRaw) {
         if (exposureRaw >= 0.0) {
             try {
-                autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
+                if (autoExposureProp != null) autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
 
                 int propVal = (int) MathUtil.clamp(exposureRaw, minExposure, maxExposure);
 
@@ -240,7 +243,8 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
 
     @Override
     public VideoMode getCurrentVideoMode() {
-        return camera.isConnected() ? camera.getVideoMode() : null;
+        return camera
+                .getVideoMode(); // This returns the current video mode even if the camera is disconnected
     }
 
     @Override
@@ -250,7 +254,7 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
                 logger.error("Got a null video mode! Doing nothing...");
                 return;
             }
-            camera.setVideoMode(videoMode);
+            if (camera.setVideoMode(videoMode)) logger.debug("Failed to set video mode!");
         } catch (Exception e) {
             logger.error("Failed to set video mode!", e);
         }

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

@@ -152,17 +152,17 @@ public class FileSaveFrameConsumer implements Consumer<CVMat> {
         var matchType = ntMatchType.getAtomic();
         if (matchType.timestamp == 0) {
             // no NT info yet
-            logger.warn("Did not recieve match type, defaulting to 0");
+            logger.warn("Did not receive match type, defaulting to 0");
         }
 
         var matchNum = ntMatchNum.getAtomic();
         if (matchNum.timestamp == 0) {
-            logger.warn("Did not recieve match num, defaulting to -1");
+            logger.warn("Did not receive match num, defaulting to -1");
         }
 
         var eventName = ntEventName.getAtomic();
         if (eventName.timestamp == 0) {
-            logger.warn("Did not recieve event name, defaulting to 'UNKNOWN'");
+            logger.warn("Did not receive event name, defaulting to 'UNKNOWN'");
         }
 
         String matchTypeStr =

photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java

@@ -42,18 +42,18 @@ public class USBFrameProvider extends CpuImageProcessor {
 
     @Override
     public CapturedFrame getInputMat() {
-        var mat = new CVMat(); // We do this so that we don't fill a Mat in use by another thread
-        // This is from wpi::Now, or WPIUtilJNI.now()
-        long time =
-                cvSink.grabFrame(mat.getMat())
-                        * 1000; // Units are microseconds, epoch is the same as the Unix epoch
+        // We allocate memory so we don't fill a Mat in use by another thread (memory model is easier)
+        var mat = new CVMat();
+        // This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since
+        // Hal::initialize was called
+        long captureTimeNs = cvSink.grabFrame(mat.getMat()) * 1000;
 
-        if (time == 0) {
+        if (captureTimeNs == 0) {
             var error = cvSink.getError();
             logger.error("Error grabbing image: " + error);
         }
 
-        return new CapturedFrame(mat, settables.getFrameStaticProperties(), time);
+        return new CapturedFrame(mat, settables.getFrameStaticProperties(), captureTimeNs);
     }
 
     @Override

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

@@ -59,6 +59,7 @@ public class AprilTagDetectionPipe
     public void setParams(AprilTagDetectionPipeParams newParams) {
         if (this.params == null || !this.params.equals(newParams)) {
             m_detector.setConfig(newParams.detectorParams);
+            m_detector.setQuadThresholdParameters(newParams.quadParams);
 
             m_detector.clearFamilies();
             m_detector.addFamily(newParams.family.getNativeName());

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

@@ -23,10 +23,15 @@ import org.photonvision.vision.apriltag.AprilTagFamily;
 public class AprilTagDetectionPipeParams {
     public final AprilTagFamily family;
     public final AprilTagDetector.Config detectorParams;
+    public final AprilTagDetector.QuadThresholdParameters quadParams;
 
-    public AprilTagDetectionPipeParams(AprilTagFamily tagFamily, AprilTagDetector.Config config) {
+    public AprilTagDetectionPipeParams(
+            AprilTagFamily tagFamily,
+            AprilTagDetector.Config config,
+            AprilTagDetector.QuadThresholdParameters quadParams) {
         this.family = tagFamily;
         this.detectorParams = config;
+        this.quadParams = quadParams;
     }
 
     @Override
@@ -35,6 +40,7 @@ public class AprilTagDetectionPipeParams {
         int result = 1;
         result = prime * result + ((family == null) ? 0 : family.hashCode());
         result = prime * result + ((detectorParams == null) ? 0 : detectorParams.hashCode());
+        result = prime * result + ((quadParams == null) ? 0 : quadParams.hashCode());
         return result;
     }
 
@@ -46,7 +52,11 @@ public class AprilTagDetectionPipeParams {
         AprilTagDetectionPipeParams other = (AprilTagDetectionPipeParams) obj;
         if (family != other.family) return false;
         if (detectorParams == null) {
-            return other.detectorParams == null;
-        } else return detectorParams.equals(other.detectorParams);
+            if (other.detectorParams != null) return false;
+        } else if (!detectorParams.equals(other.detectorParams)) return false;
+        if (quadParams == null) {
+            if (other.quadParams != null) return false;
+        } else if (!quadParams.equals(other.quadParams)) return false;
+        return true;
     }
 }

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

@@ -140,7 +140,7 @@ public class CornerDetectionPipe
         var compareDistToTr =
                 Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(2)));
 
-        // top left and top right are the poly corners closest to the bouding box tl and tr
+        // top left and top right are the poly corners closest to the bounding box tl and tr
         pointList.sort(compareDistToTl);
         var tl = pointList.get(0);
         pointList.remove(tl);

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

@@ -87,7 +87,21 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
         config.refineEdges = settings.refineEdges;
         config.quadSigma = (float) settings.blur;
         config.quadDecimate = settings.decimate;
-        aprilTagDetectionPipe.setParams(new AprilTagDetectionPipeParams(settings.tagFamily, config));
+
+        var quadParams = new AprilTagDetector.QuadThresholdParameters();
+        // 5 was the default minClusterPixels in WPILib prior to 2025
+        // increasing it causes detection problems when decimate > 1
+        quadParams.minClusterPixels = 5;
+        // these are the same as the values in WPILib 2025
+        // setting them here to prevent upstream changes from changing behavior of the detector
+        quadParams.maxNumMaxima = 10;
+        quadParams.criticalAngle = 45 * Math.PI / 180.0;
+        quadParams.maxLineFitMSE = 10.0f;
+        quadParams.minWhiteBlackDiff = 5;
+        quadParams.deglitch = false;
+
+        aprilTagDetectionPipe.setParams(
+                new AprilTagDetectionPipeParams(settings.tagFamily, config, quadParams));
 
         if (frameStaticProperties.cameraCalibration != null) {
             var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat();

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

@@ -512,7 +512,7 @@ public class PipelineManager {
         var oldSettings = userPipelineSettings.get(idx);
 
         var name = getCurrentPipelineSettings().pipelineNickname;
-        // Dummy settings to copy common fileds over
+        // Dummy settings to copy common fields over
         var newSettings = createSettingsForType(type, name);
 
         // Copy all fields from AdvancedPipelineSettings/its superclasses from old to new

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

@@ -30,6 +30,7 @@ import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.dataflow.DataChangeService;
 import org.photonvision.common.dataflow.events.OutgoingUIEvent;
 import org.photonvision.common.hardware.Platform;
+import org.photonvision.common.hardware.Platform.OSType;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.TimedTaskManager;
@@ -128,20 +129,26 @@ public class VisionSourceManager {
         return tryMatchCamImpl(null);
     }
 
+    protected List<VisionSource> tryMatchCamImpl(ArrayList<CameraInfo> cameraInfos) {
+        return tryMatchCamImpl(cameraInfos, Platform.getCurrentPlatform());
+    }
+
     /**
      * @param cameraInfos Used to feed camera info for unit tests.
      * @return New VisionSources.
      */
-    protected List<VisionSource> tryMatchCamImpl(ArrayList<CameraInfo> cameraInfos) {
+    protected List<VisionSource> tryMatchCamImpl(
+            ArrayList<CameraInfo> cameraInfos, Platform platform) {
         boolean createSources = true;
         List<CameraInfo> connectedCameras;
         if (cameraInfos == null) {
             // Detect USB cameras using CSCore
-            connectedCameras = new ArrayList<>(filterAllowedDevices(getConnectedUSBCameras()));
+            connectedCameras = new ArrayList<>(filterAllowedDevices(getConnectedUSBCameras(), platform));
             // Detect CSI cameras using libcamera
-            connectedCameras.addAll(new ArrayList<>(filterAllowedDevices(getConnectedCSICameras())));
+            connectedCameras.addAll(
+                    new ArrayList<>(filterAllowedDevices(getConnectedCSICameras(), platform)));
         } else {
-            connectedCameras = new ArrayList<>(filterAllowedDevices(cameraInfos));
+            connectedCameras = new ArrayList<>(filterAllowedDevices(cameraInfos, platform));
             createSources =
                     false; // Dont create sources if we are using supplied camerainfo for unit tests.
         }
@@ -162,7 +169,7 @@ public class VisionSourceManager {
         // All cameras are already loaded return no new sources.
         if (connectedCameras.isEmpty()) return null;
 
-        logger.debug("Matching " + connectedCameras.size() + " new cameras!");
+        logger.debug("Matching " + connectedCameras.size() + " new camera(s)!");
 
         // Debug prints
         for (var info : connectedCameras) {
@@ -170,7 +177,7 @@ public class VisionSourceManager {
         }
 
         if (!unmatchedLoadedConfigs.isEmpty())
-            logger.debug("Trying to match " + unmatchedLoadedConfigs.size() + " unmatched configs...");
+            logger.debug("Trying to match " + unmatchedLoadedConfigs.size() + " unmatched config(s)...");
 
         // Match camera configs to physical cameras
         List<CameraConfiguration> matchedCameras =
@@ -182,7 +189,7 @@ public class VisionSourceManager {
                     () ->
                             "After matching, "
                                     + unmatchedLoadedConfigs.size()
-                                    + " configs remained unmatched. Is your camera disconnected?");
+                                    + " config(s) remained unmatched. Is your camera disconnected?");
             logger.warn(
                     "Unloaded configs: "
                             + unmatchedLoadedConfigs.stream()
@@ -233,7 +240,7 @@ public class VisionSourceManager {
         if (checkUSBPath && savedConfig.getUSBPath().isEmpty()) {
             logger.debug(
                     "WARN: Camera has empty USB path, but asked to match by name: "
-                            + camCfgToString(savedConfig));
+                            + savedConfig.toShortString());
         }
 
         return (CameraInfo physicalCamera) -> {
@@ -277,22 +284,6 @@ public class VisionSourceManager {
                 ConfigManager.getInstance().getConfig().getNetworkConfig().matchCamerasOnlyByPath);
     }
 
-    private static final String camCfgToString(CameraConfiguration c) {
-        return new StringBuilder()
-                .append("[baseName=")
-                .append(c.baseName)
-                .append(", uniqueName=")
-                .append(c.uniqueName)
-                .append(", otherPaths=")
-                .append(Arrays.toString(c.otherPaths))
-                .append(", vid=")
-                .append(c.usbVID)
-                .append(", pid=")
-                .append(c.usbPID)
-                .append("]")
-                .toString();
-    }
-
     /**
      * Create {@link CameraConfiguration}s based on a list of detected USB cameras and the configs on
      * disk.
@@ -423,7 +414,7 @@ public class VisionSourceManager {
                 logger.debug(
                         String.format(
                                 "Trying to find a match for loaded camera %s (%s) with camera config: %s",
-                                config.baseName, config.uniqueName, camCfgToString(config)));
+                                config.baseName, config.uniqueName, config.toShortString()));
 
                 // Get matcher and filter against it, picking out the first match
                 Predicate<CameraInfo> matches =
@@ -463,7 +454,7 @@ public class VisionSourceManager {
             List<CameraConfiguration> loadedConfigs) {
         List<CameraConfiguration> ret = new ArrayList<CameraConfiguration>();
         logger.debug(
-                "After matching loaded configs, these configs remained unmatched: "
+                "After matching loaded configs, these cameras remained unmatched: "
                         + detectedCameraList.stream()
                                 .map(n -> String.valueOf(n))
                                 .collect(Collectors.joining("-", "{", "}")));
@@ -535,9 +526,9 @@ public class VisionSourceManager {
      * Filter out any blacklisted or ignored devices.
      *
      * @param allDevices
-     * @return list of devices with blacklisted or ingore devices removed.
+     * @return list of devices with blacklisted or ignore devices removed.
      */
-    private List<CameraInfo> filterAllowedDevices(List<CameraInfo> allDevices) {
+    private List<CameraInfo> filterAllowedDevices(List<CameraInfo> allDevices, Platform platform) {
         List<CameraInfo> filteredDevices = new ArrayList<>();
         for (var device : allDevices) {
             if (deviceBlacklist.contains(device.name)) {
@@ -546,6 +537,13 @@ public class VisionSourceManager {
             } else if (device.name.matches(ignoredCamerasRegex)) {
                 logger.trace("Skipping ignored device: \"" + device.name + "\" at \"" + device.path);
             } else if (device.getIsV4lCsiCamera()) {
+            } else if (device.otherPaths.length == 0
+                    && platform.osType == OSType.LINUX
+                    && device.cameraType == CameraType.UsbCamera) {
+                logger.trace(
+                        "Skipping device with no other paths: \"" + device.name + "\" at \"" + device.path);
+                // If cscore hasnt passed this other paths aka a path by id or a path as in usb port then we
+                // cant guarantee it is a valid camera.
             } else {
                 filteredDevices.add(device);
                 logger.trace(
@@ -559,8 +557,6 @@ public class VisionSourceManager {
             List<CameraConfiguration> camConfigs, boolean createSources) {
         var cameraSources = new ArrayList<VisionSource>();
         for (var configuration : camConfigs) {
-            logger.debug("Creating VisionSource for " + camCfgToString(configuration));
-
             // In unit tests, create dummy
             if (!createSources) {
                 cameraSources.add(new TestSource(configuration));
@@ -580,6 +576,7 @@ public class VisionSourceManager {
                     cameraSources.add(newCam);
                 }
             }
+            logger.debug("Creating VisionSource for " + configuration.toShortString());
         }
         return cameraSources;
     }

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

@@ -59,7 +59,7 @@ public abstract class VisionSourceSettables {
     public abstract void setGain(int gain);
 
     // Pretty uncommon so instead of abstract this is just a no-op by default
-    // Overriden by cameras with AWB gain support
+    // Overriddenn by cameras with AWB gain support
     public void setRedGain(int red) {}
 
     public void setBlueGain(int blue) {}

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

@@ -287,7 +287,7 @@ public class Calibrate3dPipeTest {
     }
 
     /**
-     * Uses a given camera coefficents matrix set to "undistort" every image file found in a given
+     * Uses a given camera coefficientss matrix set to "undistort" every image file found in a given
      * directory and display them. Provides an easy way to visually debug the results of the
      * calibration routine. Seems to play havoc with CI and takes a chunk of time, so shouldn't
      * usually be left active in tests.

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

@@ -139,7 +139,7 @@ public class CalibrationRotationPipeTest {
 
         Point[] originalPoints = {new Point(100, 100), new Point(200, 200), new Point(300, 100)};
 
-        // Distort the origional points
+        // Distort the origonal points
         var distortedOriginalPoints =
                 OpenCVHelp.distortPoints(
                         List.of(originalPoints),
@@ -153,14 +153,14 @@ public class CalibrationRotationPipeTest {
                         .collect(Collectors.toList());
 
         // Now let's instead rotate then distort
-        var rotatedOrigionalPoints =
+        var rotatedOriginalPoints =
                 Arrays.stream(originalPoints)
                         .map(it -> rot.rotatePoint(it, frameProps.imageWidth, frameProps.imageHeight))
                         .collect(Collectors.toList());
 
         var distortedRotatedPoints =
                 OpenCVHelp.distortPoints(
-                        rotatedOrigionalPoints,
+                        rotatedOriginalPoints,
                         rotatedFrameProps.cameraCalibration.getCameraIntrinsicsMat(),
                         rotatedFrameProps.cameraCalibration.getDistCoeffsMat());
 

photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java

@@ -18,8 +18,10 @@
 package org.photonvision.vision.processes;
 
 import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.fail;
 
 import edu.wpi.first.cscore.VideoMode;
+import java.io.IOException;
 import java.util.Arrays;
 import java.util.HashMap;
 import java.util.List;
@@ -31,6 +33,7 @@ import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.dataflow.CVPipelineResultConsumer;
 import org.photonvision.common.util.TestUtils;
+import org.photonvision.jni.PhotonTargetingJniLoader;
 import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.camera.USBCameras.USBCameraSource;
 import org.photonvision.vision.frame.FrameProvider;
@@ -41,7 +44,16 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult;
 public class VisionModuleManagerTest {
     @BeforeAll
     public static void init() {
+        String classpathStr = System.getProperty("java.class.path");
+        System.out.print(classpathStr);
+
         TestUtils.loadLibraries();
+        try {
+            if (!PhotonTargetingJniLoader.load()) fail();
+        } catch (UnsatisfiedLinkError | IOException e) {
+            e.printStackTrace();
+            fail(e);
+        }
     }
 
     private static class TestSource extends VisionSource {

photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java

@@ -17,14 +17,14 @@
 
 package org.photonvision.vision.processes;
 
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.*;
 
 import java.util.ArrayList;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 import org.photonvision.common.configuration.CameraConfiguration;
 import org.photonvision.common.configuration.ConfigManager;
+import org.photonvision.common.hardware.Platform;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.LogLevel;
 import org.photonvision.common.logging.Logger;
@@ -62,7 +62,8 @@ public class VisionSourceManagerTest {
         config4.usbVID = 5;
         config4.usbPID = 6;
 
-        CameraInfo info1 = new CameraInfo(0, "dev/video0", "testVideo", new String[0], 1, 2);
+        CameraInfo info1 =
+                new CameraInfo(0, "dev/video0", "testVideo", new String[] {"/usb/path/0"}, 1, 2);
 
         cameraInfos.add(info1);
 
@@ -73,7 +74,8 @@ public class VisionSourceManagerTest {
         assertTrue(inst.knownCameras.contains(info1));
         assertEquals(2, inst.unmatchedLoadedConfigs.size());
 
-        CameraInfo info2 = new CameraInfo(0, "dev/video1", "secondTestVideo", new String[0], 2, 3);
+        CameraInfo info2 =
+                new CameraInfo(0, "dev/video1", "secondTestVideo", new String[] {"/usb/path/1"}, 2, 3);
 
         cameraInfos.add(info2);
 
@@ -297,7 +299,7 @@ public class VisionSourceManagerTest {
                 new CameraConfiguration(
                         "Arducam OV2311 USB Camera",
                         "Arducam OV2311 USB Camera",
-                        "fromt-left",
+                        "front-left",
                         "/dev/video0",
                         CAM1_OLD_PATHS);
         camera1_saved_config.usbVID = 3141;
@@ -306,7 +308,7 @@ public class VisionSourceManagerTest {
                 new CameraConfiguration(
                         "Arducam OV2311 USB Camera",
                         "Arducam OV2311 USB Camera (1)",
-                        "fromt-left",
+                        "front-left",
                         "/dev/video2",
                         CAM2_OLD_PATH);
         camera2_saved_config.usbVID = 3141;
@@ -362,7 +364,7 @@ public class VisionSourceManagerTest {
                 new CameraConfiguration(
                         "Arducam OV2311 USB Camera",
                         "Arducam OV2311 USB Camera (1)",
-                        "fromt-left",
+                        "front-left",
                         "/dev/video0",
                         CAM1_OLD_PATHS);
         camera1_saved_config.usbVID = 3141;
@@ -371,7 +373,7 @@ public class VisionSourceManagerTest {
                 new CameraConfiguration(
                         "Arducam OV2311 USB Camera",
                         "Arducam OV2311 USB Camera (1)",
-                        "fromt-left",
+                        "front-left",
                         "/dev/video2",
                         CAM2_OLD_PATH);
         camera2_saved_config.usbVID = 3141;
@@ -500,6 +502,43 @@ public class VisionSourceManagerTest {
         }
     }
 
+    @Test
+    public void testNoOtherPaths() {
+        Logger.setLevel(LogGroup.Camera, LogLevel.DEBUG);
+
+        // List of known cameras
+        var cameraInfos = new ArrayList<CameraInfo>();
+
+        var inst = new VisionSourceManager();
+        ConfigManager.getInstance().clearConfig();
+        ConfigManager.getInstance().load();
+        ConfigManager.getInstance().getConfig().getNetworkConfig().matchCamerasOnlyByPath = false;
+
+        // Match empty camera infos
+        inst.tryMatchCamImpl(cameraInfos);
+
+        CameraInfo info1 =
+                new CameraInfo(0, "/dev/video0", "Arducam OV2311 USB Camera", new String[] {}, 3141, 25446);
+
+        cameraInfos.add(info1);
+
+        // Match two "new" cameras
+        var ret1 = inst.tryMatchCamImpl(cameraInfos, Platform.LINUX_64);
+
+        // Our cameras should be "known"
+        assertFalse(inst.knownCameras.contains(info1));
+        assertEquals(0, inst.knownCameras.size());
+        assertEquals(null, ret1);
+
+        // Match two "new" cameras
+        var ret2 = inst.tryMatchCamImpl(cameraInfos, Platform.WINDOWS_64);
+
+        // Our cameras should be "known"
+        assertTrue(inst.knownCameras.contains(info1));
+        assertEquals(1, inst.knownCameras.size());
+        assertEquals(1, ret2.size());
+    }
+
     @Test
     public void testIdenticalCameras() {
         Logger.setLevel(LogGroup.Camera, LogLevel.DEBUG);
@@ -571,7 +610,7 @@ public class VisionSourceManagerTest {
                     ret1.stream().filter(it -> thisName.equals(it.cameraConfiguration.uniqueName)).count());
         }
 
-        // duplciate cameras, same info, new ref
+        // duplicate cameras, same info, new ref
         var duplicateCameraInfos = new ArrayList<CameraInfo>();
         CameraInfo info1_dup =
                 new CameraInfo(
@@ -609,7 +648,7 @@ public class VisionSourceManagerTest {
         assertTrue(inst.knownCameras.contains(info2_dup));
         assertEquals(2, inst.knownCameras.size());
 
-        // duplciate cameras this simulates unplugging one and plugging the other in where v4l assigns
+        // duplicate cameras this simulates unplugging one and plugging the other in where v4l assigns
         // the same by-id path to the other camera
         var duplicateCameraInfos1 = new ArrayList<CameraInfo>();
         CameraInfo info3_dup =

photon-docs/build.gradle

@@ -217,7 +217,7 @@ task generateJavaDocs(type: Javadoc) {
     classpath = files(exportedProjects.collect { project(it).sourceSets.main.compileClasspath })
     dependsOn project(':photon-core').writeCurrentVersion
 
-    options.links("https://docs.oracle.com/en/java/javase/17/docs/api/")
+    options.links "https://docs.oracle.com/en/java/javase/17/docs/api/", "https://github.wpilib.org/allwpilib/docs/release/java/"
     options.addStringOption("tag", "pre:a:Pre-Condition")
     options.addBooleanOption("Xdoclint:html,missing,reference,syntax", true)
     options.addBooleanOption('html5', true)

photon-lib/build.gradle

@@ -9,6 +9,7 @@ ext {
     includePhotonTargeting = true
     // Include the generated Version file
     generatedHeaders = "src/generate/native/include"
+    licenseFile = file("LICENSE")
 }
 
 apply plugin: 'cpp'
@@ -22,25 +23,7 @@ apply from: "${rootDir}/versioningHelper.gradle"
 
 nativeUtils {
     exportsConfigs {
-        "${nativeName}" {
-            // From https://github.com/wpilibsuite/allwpilib/blob/a32589831184969939fd3d63f449a2788a0a8542/wpimath/build.gradle#L72
-            // Copyright (c) FIRST and other WPILib contributors.
-            // Open Source Software; you can modify and/or share it under the terms of
-            // the WPILib BSD license file in the root directory of this project.
-            x64ExcludeSymbols = [
-                '_CT??_R0?AV_System_error',
-                '_CT??_R0?AVexception',
-                '_CT??_R0?AVfailure',
-                '_CT??_R0?AVruntime_error',
-                '_CT??_R0?AVsystem_error',
-                '_CTA5?AVfailure',
-                '_TI5?AVfailure',
-                '_CT??_R0?AVout_of_range',
-                '_CTA3?AVout_of_range',
-                '_TI3?AVout_of_range',
-                '_CT??_R0?AVbad_cast'
-            ]
-        }
+        "${nativeName}" {}
     }
 }
 
@@ -177,6 +160,7 @@ task generateVendorJson() {
     def read = photonlibFileInput.text
             .replace('${photon_version}', pubVersion)
             .replace('${frc_year}', frcYear)
+            .replace('${wpilib_version}', wpilibVersion)
     photonlibFileOutput.text = read
 
     outputs.upToDateWhen { false }
@@ -331,6 +315,7 @@ def nativeTasks = wpilibTools.createExtractionTasks {
 
 nativeTasks.addToSourceSetResources(sourceSets.test)
 
+nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpilibc")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
@@ -338,4 +323,4 @@ nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
 nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
-nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get())
+nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, wpi.versions.opencvVersion.get())

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,6 +20,7 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
+from ..packet import Packet
 from ..targeting import *
 
 
@@ -28,6 +29,17 @@ class MultiTargetPNPResultSerde:
     MESSAGE_VERSION = "541096947e9f3ca2d3f425ff7b04aa7b"
     MESSAGE_FORMAT = "PnpResult:ae4d655c0a3104d88df4f5db144c1e86 estimatedPose;int16 fiducialIDsUsed[?];"
 
+    @staticmethod
+    def pack(value: "MultiTargetPNPResult") -> "Packet":
+        ret = Packet()
+
+        # estimatedPose is of non-intrinsic type PnpResult
+        ret.encodeBytes(PnpResult.photonStruct.pack(value.estimatedPose).getData())
+
+        # fiducialIDsUsed is a custom VLA!
+        ret.encodeShortList(value.fiducialIDsUsed)
+        return ret
+
     @staticmethod
     def unpack(packet: "Packet") -> "MultiTargetPNPResult":
         ret = MultiTargetPNPResult()

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

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

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

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

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

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

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

@@ -20,6 +20,7 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
+from ..packet import Packet
 from ..targeting import *
 
 
@@ -28,6 +29,24 @@ class PnpResultSerde:
     MESSAGE_VERSION = "ae4d655c0a3104d88df4f5db144c1e86"
     MESSAGE_FORMAT = "Transform3d best;Transform3d alt;float64 bestReprojErr;float64 altReprojErr;float64 ambiguity;"
 
+    @staticmethod
+    def pack(value: "PnpResult") -> "Packet":
+        ret = Packet()
+
+        ret.encodeTransform(value.best)
+
+        ret.encodeTransform(value.alt)
+
+        # bestReprojErr is of intrinsic type float64
+        ret.encodeDouble(value.bestReprojErr)
+
+        # altReprojErr is of intrinsic type float64
+        ret.encodeDouble(value.altReprojErr)
+
+        # ambiguity is of intrinsic type float64
+        ret.encodeDouble(value.ambiguity)
+        return ret
+
     @staticmethod
     def unpack(packet: "Packet") -> "PnpResult":
         ret = PnpResult()

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

@@ -20,6 +20,7 @@
 ##                        --> DO NOT MODIFY <--
 ###############################################################################
 
+from ..packet import Packet
 from ..targeting import *
 
 
@@ -28,6 +29,17 @@ class TargetCornerSerde:
     MESSAGE_VERSION = "16f6ac0dedc8eaccb951f4895d9e18b6"
     MESSAGE_FORMAT = "float64 x;float64 y;"
 
+    @staticmethod
+    def pack(value: "TargetCorner") -> "Packet":
+        ret = Packet()
+
+        # x is of intrinsic type float64
+        ret.encodeDouble(value.x)
+
+        # y is of intrinsic type float64
+        ret.encodeDouble(value.y)
+        return ret
+
     @staticmethod
     def unpack(packet: "Packet") -> "TargetCorner":
         ret = TargetCorner()

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

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

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

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

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

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

photon-lib/py/photonlibpy/packet.py

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

photon-lib/py/photonlibpy/photonCamera.py

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

photon-lib/py/photonlibpy/photonPoseEstimator.py

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -1,9 +1,12 @@
 from dataclasses import dataclass, field
-from typing import Optional
+from typing import TYPE_CHECKING, ClassVar, Optional
 
 from .multiTargetPNPResult import MultiTargetPNPResult
 from .photonTrackedTarget import PhotonTrackedTarget
 
+if TYPE_CHECKING:
+    from .. import generated
+
 
 @dataclass
 class PhotonPipelineMetadata:
@@ -15,7 +18,9 @@ class PhotonPipelineMetadata:
     # Mirror of the heartbeat entry -- monotonically increasing
     sequenceID: int = -1
 
-    photonStruct: "PhotonPipelineMetadataSerde" = None
+    timeSinceLastPong: int = -1
+
+    photonStruct: ClassVar["generated.PhotonPipelineMetadataSerde"]
 
 
 @dataclass
@@ -24,8 +29,10 @@ class PhotonPipelineResult:
     ntReceiveTimestampMicros: int = -1
 
     targets: list[PhotonTrackedTarget] = field(default_factory=list)
+    # Python users beware! We don't currently run a Time Sync Server, so these timestamps are in
+    # an arbitrary timebase. This is not true in C++ or Java.
     metadata: PhotonPipelineMetadata = field(default_factory=PhotonPipelineMetadata)
-    multiTagResult: Optional[MultiTargetPNPResult] = None
+    multitagResult: Optional[MultiTargetPNPResult] = None
 
     def getLatencyMillis(self) -> float:
         return (
@@ -53,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.
@@ -62,4 +69,4 @@ class PhotonPipelineResult:
             return None
         return self.getTargets()[0]
 
-    photonStruct: "PhotonPipelineResultSerde" = None
+    photonStruct: ClassVar["generated.PhotonPipelineResultSerde"]

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

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

photon-lib/py/setup.py

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

photon-lib/py/test/photonPoseEstimator_test.py

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

photon-lib/py/test/photonlibpy_test.py

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

photon-lib/py/test/visionSystemSim_test.py

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

photon-lib/src/generate/photonlib.json.in

@@ -9,7 +9,47 @@
     "https://maven.photonvision.org/repository/snapshots"
   ],
   "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
-  "jniDependencies": [],
+  "jniDependencies": [
+    {
+      "groupId": "edu.wpi.first.wpilibc",
+      "artifactId": "wpilibc-cpp",
+      "version": "${wpilib_version}",
+      "skipInvalidPlatforms": true,
+      "isJar": false,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-cpp",
+      "version": "${photon_version}",
+      "skipInvalidPlatforms": true,
+      "isJar": false,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-jni",
+      "version": "${photon_version}",
+      "skipInvalidPlatforms": true,
+      "isJar": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ],
   "cppDependencies": [
     {
       "groupId": "org.photonvision",

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

@@ -32,9 +32,7 @@ import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.numbers.*;
 import edu.wpi.first.networktables.BooleanPublisher;
 import edu.wpi.first.networktables.BooleanSubscriber;
-import edu.wpi.first.networktables.DoubleArrayPublisher;
 import edu.wpi.first.networktables.DoubleArraySubscriber;
-import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.IntegerEntry;
 import edu.wpi.first.networktables.IntegerPublisher;
 import edu.wpi.first.networktables.IntegerSubscriber;
@@ -53,6 +51,7 @@ import java.util.stream.Collectors;
 import org.photonvision.common.hardware.VisionLEDMode;
 import org.photonvision.common.networktables.PacketSubscriber;
 import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.timesync.TimeSyncSingleton;
 
 /** Represents a camera that is connected to PhotonVision. */
 public class PhotonCamera implements AutoCloseable {
@@ -63,13 +62,6 @@ public class PhotonCamera implements AutoCloseable {
     PacketSubscriber<PhotonPipelineResult> resultSubscriber;
     BooleanPublisher driverModePublisher;
     BooleanSubscriber driverModeSubscriber;
-    DoublePublisher latencyMillisEntry;
-    BooleanPublisher hasTargetEntry;
-    DoublePublisher targetPitchEntry;
-    DoublePublisher targetYawEntry;
-    DoublePublisher targetAreaEntry;
-    DoubleArrayPublisher targetPoseEntry;
-    DoublePublisher targetSkewEntry;
     StringSubscriber versionEntry;
     IntegerEntry inputSaveImgEntry, outputSaveImgEntry;
     IntegerPublisher pipelineIndexRequest, ledModeRequest;
@@ -85,13 +77,6 @@ public class PhotonCamera implements AutoCloseable {
         resultSubscriber.close();
         driverModePublisher.close();
         driverModeSubscriber.close();
-        latencyMillisEntry.close();
-        hasTargetEntry.close();
-        targetPitchEntry.close();
-        targetYawEntry.close();
-        targetAreaEntry.close();
-        targetPoseEntry.close();
-        targetSkewEntry.close();
         versionEntry.close();
         inputSaveImgEntry.close();
         outputSaveImgEntry.close();
@@ -110,12 +95,15 @@ public class PhotonCamera implements AutoCloseable {
 
     private static boolean VERSION_CHECK_ENABLED = true;
     private static long VERSION_CHECK_INTERVAL = 5;
-    private double lastVersionCheckTime = 0;
+    double lastVersionCheckTime = 0;
 
     private long prevHeartbeatValue = -1;
     private double prevHeartbeatChangeTime = 0;
     private static final double HEARTBEAT_DEBOUNCE_SEC = 0.5;
 
+    double prevTimeSyncWarnTime = 0;
+    private static final double WARN_DEBOUNCE_SEC = 5;
+
     public static void setVersionCheckEnabled(boolean enabled) {
         VERSION_CHECK_ENABLED = enabled;
     }
@@ -166,6 +154,9 @@ public class PhotonCamera implements AutoCloseable {
 
         HAL.report(tResourceType.kResourceType_PhotonCamera, InstanceCount);
         InstanceCount++;
+
+        // HACK - start a TimeSyncServer, if we haven't yet.
+        TimeSyncSingleton.load();
     }
 
     /**
@@ -189,13 +180,12 @@ public class PhotonCamera implements AutoCloseable {
 
         List<PhotonPipelineResult> ret = new ArrayList<>();
 
+        // Grab the latest results. We don't care about the timestamps from NT - the metadata header has
+        // this, latency compensated by the Time Sync Client
         var changes = resultSubscriber.getAllChanges();
-
-        // TODO: NT4 timestamps are still not to be trusted. But it's the best we can do until we can
-        // make time sync more reliable.
         for (var c : changes) {
             var result = c.value;
-            result.setReceiveTimestampMicros(c.timestamp);
+            checkTimeSyncOrWarn(result);
             ret.add(result);
         }
 
@@ -213,21 +203,38 @@ public class PhotonCamera implements AutoCloseable {
     public PhotonPipelineResult getLatestResult() {
         verifyVersion();
 
+        // Grab the latest result. We don't care about the timestamp from NT - the metadata header has
+        // this, latency compensated by the Time Sync Client
         var ret = resultSubscriber.get();
 
         if (ret.timestamp == 0) return new PhotonPipelineResult();
 
         var result = ret.value;
 
-        // Set the timestamp of the result. Since PacketSubscriber doesn't realize that the result
-        // contains a thing with time knowledge, set it here.
-        // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
-        // TODO: NT4 time sync is Not To Be Trusted, we should do something else?
-        result.setReceiveTimestampMicros(ret.timestamp);
+        checkTimeSyncOrWarn(result);
 
         return result;
     }
 
+    private void checkTimeSyncOrWarn(PhotonPipelineResult result) {
+        if (result.metadata.timeSinceLastPong > 5L * 1000000L) {
+            if (Timer.getFPGATimestamp() > (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
+                prevTimeSyncWarnTime = Timer.getFPGATimestamp();
+
+                DriverStation.reportWarning(
+                        "PhotonVision coprocessor at path "
+                                + path
+                                + " is not connected to the TimeSyncServer? It's been "
+                                + String.format("%.2f", result.metadata.timeSinceLastPong / 1e6)
+                                + "s since the coprocessor last heard a pong.\n\nCheck /photonvision/.timesync/{COPROCESSOR_HOSTNAME} for more information.",
+                        false);
+            }
+        } else {
+            // Got a valid packet, reset the last time
+            prevTimeSyncWarnTime = 0;
+        }
+    }
+
     /**
      * Returns whether the camera is in driver mode.
      *
@@ -373,7 +380,7 @@ public class PhotonCamera implements AutoCloseable {
         return cameraTable;
     }
 
-    private void verifyVersion() {
+    void verifyVersion() {
         if (!VERSION_CHECK_ENABLED) return;
 
         if ((Timer.getFPGATimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return;
@@ -410,7 +417,7 @@ public class PhotonCamera implements AutoCloseable {
         // Check for connection status. Warn if disconnected.
         else if (!isConnected()) {
             DriverStation.reportWarning(
-                    "PhotonVision coprocessor at path " + path + " is not sending new data.", true);
+                    "PhotonVision coprocessor at path " + path + " is not sending new data.", false);
         }
 
         String versionString = versionEntry.get("");
@@ -425,7 +432,7 @@ public class PhotonCamera implements AutoCloseable {
                     "PhotonVision coprocessor at path "
                             + path
                             + " has not reported a message interface UUID - is your coprocessor's camera started?",
-                    true);
+                    false);
         } else if (!local_uuid.equals(remote_uuid)) {
             // Error on a verified version mismatch
             // But stay silent otherwise

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

@@ -408,7 +408,7 @@ public class PhotonPoseEstimator {
                             result.getTargets(),
                             PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
         } else {
-            // We can nver fall back on another multitag strategy
+            // We can never fall back on another multitag strategy
             return update(result, Optional.empty(), Optional.empty(), this.multiTagFallbackStrategy);
         }
     }

photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java

@@ -68,8 +68,8 @@ import org.photonvision.targeting.PnpResult;
 public class PhotonCameraSim implements AutoCloseable {
     private final PhotonCamera cam;
 
-    NTTopicSet ts = new NTTopicSet();
-    private long heartbeatCounter = 0;
+    protected NTTopicSet ts = new NTTopicSet();
+    private long heartbeatCounter = 1;
 
     /** This simulated camera's {@link SimCameraProperties} */
     public final SimCameraProperties prop;
@@ -553,9 +553,10 @@ public class PhotonCameraSim implements AutoCloseable {
                         heartbeatCounter,
                         now - (long) (latencyMillis * 1000),
                         now,
+                        // Pretend like we heard a pong recently
+                        1000L + (long) ((Math.random() - 0.5) * 50),
                         detectableTgts,
                         multitagResult);
-        ret.setReceiveTimestampMicros(now);
         return ret;
     }
 
@@ -605,6 +606,8 @@ public class PhotonCameraSim implements AutoCloseable {
 
         ts.cameraIntrinsicsPublisher.set(prop.getIntrinsics().getData(), receiveTimestamp);
         ts.cameraDistortionPublisher.set(prop.getDistCoeffs().getData(), receiveTimestamp);
-        ts.heartbeatPublisher.set(heartbeatCounter++, receiveTimestamp);
+
+        ts.heartbeatPublisher.set(heartbeatCounter, receiveTimestamp);
+        heartbeatCounter += 1;
     }
 }

photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java

@@ -38,6 +38,7 @@ import java.util.Map;
 import org.opencv.core.Core;
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
+import org.opencv.core.MatOfByte;
 import org.opencv.core.MatOfPoint;
 import org.opencv.core.MatOfPoint2f;
 import org.opencv.core.Point;
@@ -99,11 +100,18 @@ public class VideoSimUtil {
      *
      * @param id The fiducial id of the desired tag
      */
-    public static Mat get36h11TagImage(int id) {
+    private static Mat get36h11TagImage(int id) {
         RawFrame frame = AprilTag.generate36h11AprilTagImage(id);
-        Mat result = new Mat(10, 10, CvType.CV_8UC1, frame.getData(), frame.getStride()).clone();
-        frame.close();
-        return result;
+
+        var buf = frame.getData();
+        byte[] arr = new byte[buf.remaining()];
+        buf.get(arr);
+        // frame.close();
+
+        var mat = new MatOfByte(arr).reshape(1, 10).submat(new Rect(0, 0, 10, 10));
+        mat.dump();
+
+        return mat;
     }
 
     /** Gets the points representing the marker(black square) corners. */

photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java

@@ -0,0 +1,52 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision.timesync;
+
+import java.io.IOException;
+import org.photonvision.jni.PhotonTargetingJniLoader;
+import org.photonvision.jni.TimeSyncServer;
+
+/** Helper to hold a single TimeSyncServer instance with some default config */
+public class TimeSyncSingleton {
+    private static TimeSyncServer INSTANCE = null;
+
+    public static boolean load() {
+        if (INSTANCE == null) {
+            try {
+                if (!PhotonTargetingJniLoader.load()) {
+                    return false;
+                }
+            } catch (UnsatisfiedLinkError | IOException e) {
+                e.printStackTrace();
+                return false;
+            }
+
+            INSTANCE = new TimeSyncServer(5810);
+            INSTANCE.start();
+        }
+
+        return INSTANCE != null;
+    }
+}

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

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

photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp

@@ -330,7 +330,8 @@ PhotonPipelineResult PhotonCameraSim::Process(
   heartbeatCounter++;
   return PhotonPipelineResult{
       PhotonPipelineMetadata{heartbeatCounter, 0,
-                             units::microsecond_t{latency}.to<int64_t>()},
+                             units::microsecond_t{latency}.to<int64_t>(),
+                             1000000},
       detectableTgts, multiTagResults};
 }
 void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) {

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

@@ -26,7 +26,6 @@ package org.photonvision;
 
 import static org.junit.jupiter.api.Assertions.*;
 
-import edu.wpi.first.cscore.CameraServerCvJNI;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -34,10 +33,12 @@ import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.util.CombinedRuntimeLoader;
 import java.io.IOException;
 import java.util.List;
 import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
+import org.opencv.core.Core;
 import org.photonvision.estimation.CameraTargetRelation;
 import org.photonvision.estimation.OpenCVHelp;
 import org.photonvision.estimation.RotTrlTransform3d;
@@ -77,7 +78,7 @@ public class OpenCVTest {
 
     @BeforeAll
     public static void setUp() throws IOException {
-        CameraServerCvJNI.forceLoad();
+        CombinedRuntimeLoader.loadLibraries(OpenCVTest.class, Core.NATIVE_LIBRARY_NAME);
 
         // NT live for debug purposes
         NetworkTableInstance.getDefault().startServer();
@@ -150,7 +151,7 @@ public class OpenCVTest {
         assertEquals(
                 actualRelation.camToTargPitch.getDegrees(),
                 pitchDiff.getDegrees()
-                        * Math.cos(yaw2d.getRadians()), // adjust for unaccounted perpsective distortion
+                        * Math.cos(yaw2d.getRadians()), // adjust for unaccounted perspective distortion
                 kRotDeltaDeg,
                 "2d pitch doesn't match 3d");
         assertEquals(

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

@@ -24,12 +24,48 @@
 
 package org.photonvision;
 
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.photonvision.UnitTestUtils.waitForCondition;
+import static org.photonvision.UnitTestUtils.waitForSequenceNumber;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.wpilibj.Timer;
+import java.io.IOException;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.Assertions;
+import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
 import org.photonvision.common.dataflow.structures.Packet;
+import org.photonvision.jni.PhotonTargetingJniLoader;
+import org.photonvision.jni.TimeSyncClient;
+import org.photonvision.jni.WpilibLoader;
+import org.photonvision.simulation.PhotonCameraSim;
 import org.photonvision.targeting.PhotonPipelineResult;
 
 class PhotonCameraTest {
+    @BeforeAll
+    public static void load_wpilib() {
+        WpilibLoader.loadLibraries();
+    }
+
+    @BeforeEach
+    public void setup() {
+        HAL.initialize(500, 0);
+    }
+
+    @AfterEach
+    public void teardown() {
+        HAL.shutdown();
+    }
+
     @Test
     public void testEmpty() {
         Assertions.assertDoesNotThrow(
@@ -40,4 +76,158 @@ class PhotonCameraTest {
                     PhotonPipelineResult.photonStruct.pack(packet, ret);
                 });
     }
+
+    // Just a smoketest for dev use -- don't run by default
+    @Test
+    public void testTimeSyncServerWithPhotonCamera() throws InterruptedException, IOException {
+        load_wpilib();
+        PhotonTargetingJniLoader.load();
+
+        HAL.initialize(500, 0);
+
+        NetworkTableInstance.getDefault().stopClient();
+        NetworkTableInstance.getDefault().startServer();
+
+        var camera = new PhotonCamera("Arducam_OV2311_USB_Camera");
+        PhotonCamera.setVersionCheckEnabled(false);
+
+        for (int i = 0; i < 5; i++) {
+            Thread.sleep(500);
+
+            var res = camera.getLatestResult();
+            var captureTime = res.getTimestampSeconds();
+            var now = Timer.getFPGATimestamp();
+
+            // expectTrue(captureTime < now);
+
+            System.out.println(
+                    "sequence "
+                            + res.metadata.sequenceID
+                            + " image capture "
+                            + captureTime
+                            + " received at "
+                            + res.getTimestampSeconds()
+                            + " now: "
+                            + NetworkTablesJNI.now() / 1e6
+                            + " time since last pong: "
+                            + res.metadata.timeSinceLastPong / 1e6);
+        }
+
+        HAL.shutdown();
+    }
+
+    private static Stream<Arguments> testNtOffsets() {
+        return Stream.of(
+                // various initializaiton orders
+                Arguments.of(1, 10, 30, 30),
+                Arguments.of(10, 2, 30, 30),
+                Arguments.of(10, 10, 30, 30),
+                // Reboot just the robot
+                Arguments.of(1, 1, 10, 30),
+                // Reboot just the coproc
+                Arguments.of(1, 1, 30, 10));
+    }
+
+    /**
+     * Try starting client before server and vice-versa, making sure that we never fail the version
+     * check
+     */
+    @ParameterizedTest
+    @MethodSource("testNtOffsets")
+    public void testRestartingRobotAndCoproc(
+            int robotStart, int coprocStart, int robotRestart, int coprocRestart) throws Throwable {
+        var robotNt = NetworkTableInstance.create();
+        var coprocNt = NetworkTableInstance.create();
+
+        robotNt.addLogger(10, 255, (it) -> System.out.println("ROBOT: " + it.logMessage.message));
+        coprocNt.addLogger(10, 255, (it) -> System.out.println("CLIENT: " + it.logMessage.message));
+
+        TimeSyncClient tspClient = null;
+
+        var robotCamera = new PhotonCamera(robotNt, "MY_CAMERA");
+
+        // apparently need a PhotonCamera to hand down
+        var fakePhotonCoprocCam = new PhotonCamera(coprocNt, "MY_CAMERA");
+        var coprocSim = new PhotonCameraSim(fakePhotonCoprocCam);
+        coprocSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(90));
+        coprocSim.prop.setFPS(30);
+        coprocSim.setMinTargetAreaPixels(20.0);
+
+        for (int i = 0; i < 20; i++) {
+            int seq = i + 1;
+
+            if (i == coprocRestart) {
+                System.out.println("Restarting coprocessor NT client");
+
+                fakePhotonCoprocCam.close();
+                coprocNt.close();
+                coprocNt = NetworkTableInstance.create();
+
+                coprocNt.addLogger(10, 255, (it) -> System.out.println("CLIENT: " + it.logMessage.message));
+
+                fakePhotonCoprocCam = new PhotonCamera(coprocNt, "MY_CAMERA");
+                coprocSim = new PhotonCameraSim(fakePhotonCoprocCam);
+                coprocSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(90));
+                coprocSim.prop.setFPS(30);
+                coprocSim.setMinTargetAreaPixels(20.0);
+            }
+            if (i == robotRestart) {
+                System.out.println("Restarting robot NT server");
+
+                robotNt.close();
+                robotNt = NetworkTableInstance.create();
+                robotNt.addLogger(10, 255, (it) -> System.out.println("ROBOT: " + it.logMessage.message));
+                robotCamera = new PhotonCamera(robotNt, "MY_CAMERA");
+            }
+
+            if (i == coprocStart || i == coprocRestart) {
+                coprocNt.setServer("127.0.0.1", 5940);
+                coprocNt.startClient4("testClient");
+
+                // PhotonCamera makes a server by default - connect to it
+                tspClient = new TimeSyncClient("127.0.0.1", 5810, 0.5);
+            }
+
+            if (i == robotStart || i == robotRestart) {
+                robotNt.startServer("networktables_random.json", "", 5941, 5940);
+            }
+
+            Thread.sleep(100);
+
+            if (i == Math.max(coprocStart, robotStart)) {
+                final var c = coprocNt;
+                final var r = robotNt;
+                waitForCondition("Coproc connection", () -> c.getConnections().length == 1);
+                waitForCondition("Rio connection", () -> r.getConnections().length == 1);
+            }
+
+            var result1 = new PhotonPipelineResult();
+            result1.metadata.captureTimestampMicros = seq * 100;
+            result1.metadata.publishTimestampMicros = seq * 150;
+            result1.metadata.sequenceID = seq;
+            if (tspClient != null) {
+                result1.metadata.timeSinceLastPong = tspClient.getPingMetadata().timeSinceLastPong();
+            } else {
+                result1.metadata.timeSinceLastPong = Long.MAX_VALUE;
+            }
+
+            coprocSim.submitProcessedFrame(result1, NetworkTablesJNI.now());
+            coprocNt.flush();
+
+            if (i > robotStart && i > coprocStart) {
+                var ret = waitForSequenceNumber(robotCamera, seq);
+                System.out.println(ret);
+            }
+
+            // force verifyVersion to do checks
+            robotCamera.lastVersionCheckTime = -100;
+            robotCamera.prevTimeSyncWarnTime = -100;
+            assertDoesNotThrow(robotCamera::verifyVersion);
+        }
+
+        coprocSim.close();
+        coprocNt.close();
+        robotNt.close();
+        tspClient.stop();
+    }
 }

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

@@ -64,8 +64,9 @@ class PhotonPoseEstimatorTest {
         cameraOne.result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        11 * 1000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -130,7 +131,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
-        cameraOne.result.setReceiveTimestampMicros((long) (11 * 1e6));
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
@@ -150,8 +150,9 @@ class PhotonPoseEstimatorTest {
         cameraOne.result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        4000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -217,8 +218,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
 
-        cameraOne.result.setReceiveTimestampMicros((long) (4 * 1e6));
-
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
                         aprilTags,
@@ -240,8 +239,9 @@ class PhotonPoseEstimatorTest {
         cameraOne.result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        17000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -306,7 +306,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
-        cameraOne.result.setReceiveTimestampMicros((long) (17 * 1e6));
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
@@ -330,8 +329,9 @@ class PhotonPoseEstimatorTest {
         cameraOne.result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        1000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -396,7 +396,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
-        cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6));
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
@@ -412,8 +411,9 @@ class PhotonPoseEstimatorTest {
         cameraOne.result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        7000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -478,7 +478,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
-        cameraOne.result.setReceiveTimestampMicros((long) (7 * 1e6));
 
         estimatedPose = estimator.update(cameraOne.result);
         pose = estimatedPose.get().estimatedPose;
@@ -495,8 +494,9 @@ class PhotonPoseEstimatorTest {
         var result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        20000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -519,7 +519,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
-        result.setReceiveTimestampMicros((long) (20 * 1e6));
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(
@@ -529,7 +528,7 @@ class PhotonPoseEstimatorTest {
 
         // Empty result, expect empty result
         cameraOne.result = new PhotonPipelineResult();
-        cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6));
+        cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6);
         Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
         assertFalse(estimatedPose.isPresent());
 
@@ -563,8 +562,9 @@ class PhotonPoseEstimatorTest {
         cameraOne.result =
                 new PhotonPipelineResult(
                         0,
-                        0,
-                        0,
+                        20 * 1000000,
+                        1100000,
+                        1024,
                         List.of(
                                 new PhotonTrackedTarget(
                                         3.0,
@@ -629,7 +629,6 @@ class PhotonPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8))))); // 3 3 3 ambig .4
-        cameraOne.result.setReceiveTimestampMicros(20 * 1000000);
 
         PhotonPoseEstimator estimator =
                 new PhotonPoseEstimator(