Add photonlibpy (#1040)

* Added a pure-python implementation of photonlib, named photonlibpy and hosted on pypi

---------

Co-authored-by: Matt <[email protected]>

.github/workflows/python.yml

@@ -0,0 +1,62 @@
+name: Build and Distribute PhotonLibPy
+
+permissions:
+  id-token: write  # IMPORTANT: this permission is mandatory for trusted publishing
+
+on:
+  push:
+    branches: [ master ]
+    tags:
+      - 'v*'
+  pull_request:
+    branches: [ master ]
+
+jobs:
+  buildAndDeploy:
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout code
+        uses: actions/checkout@v3
+        with:
+          sparse-checkout-cone-mode: false
+          fetch-tags: true
+          fetch-depth: 99999
+
+      - name: Set up Python
+        uses: actions/setup-python@v5
+        with:
+          python-version: 3.11
+
+      - name: Install dependencies
+        run: |
+          python -m pip install --upgrade pip
+          pip install setuptools wheel pytest
+
+      - name: Build wheel
+        working-directory: ./photon-lib/py
+        run: |
+          python setup.py sdist bdist_wheel
+
+      - name: Run Unit Tests
+        working-directory: ./photon-lib/py
+        run: |
+            pip install --no-cache-dir dist/*.whl
+            pytest
+
+
+      - name: Upload artifacts
+        uses: actions/upload-artifact@master
+        with:
+          name: dist
+          path: ./photon-lib/py/dist/
+
+      - name: Publish package distributions to TestPyPI
+        # Only upload on tags
+        if: startsWith(github.ref, 'refs/tags/v')
+        uses: pypa/gh-action-pypi-publish@release/v1
+        with:
+          packages_dir: ./photon-lib/py/dist/
+
+    permissions:
+      id-token: write  # IMPORTANT: this permission is mandatory for trusted publishing

photon-lib/py/.gitignore

@@ -0,0 +1,5 @@
+photonlibpy.egg-info/
+dist/
+build/
+.eggs/
+photonlibpy/version.py

photon-lib/py/buildAndTest.bat

@@ -0,0 +1,14 @@
+:: Uninstall if it already was installed
+pip uninstall -y photonlibpy
+
+:: Build wheel
+python setup.py bdist_wheel
+
+:: Install whatever wheel was made
+for %%f in (dist/*.whl) do (
+    echo installing dist/%%f
+    pip install --no-cache-dir dist/%%f
+)
+
+:: Run the test suite
+pytest

photon-lib/py/photonlibpy/__init__.py

@@ -0,0 +1 @@
+# No one here but us chickens

photon-lib/py/photonlibpy/multiTargetPNPResult.py

@@ -0,0 +1,45 @@
+from dataclasses import dataclass, field
+from wpimath.geometry import Transform3d
+from photonlibpy.packet import Packet
+
+
+@dataclass
+class PNPResult:
+    _NUM_BYTES_IN_FLOAT = 8
+    PACK_SIZE_BYTES = 1 + (_NUM_BYTES_IN_FLOAT * 7 * 2) + (_NUM_BYTES_IN_FLOAT * 3)
+
+    isPresent: bool = False
+    best: Transform3d = field(default_factory=Transform3d)
+    alt: Transform3d = field(default_factory=Transform3d)
+    ambiguity: float = 0.0
+    bestReprojError: float = 0.0
+    altReprojError: float = 0.0
+
+    def createFromPacket(self, packet: Packet) -> Packet:
+        self.isPresent = packet.decodeBoolean()
+        self.best = packet.decodeTransform()
+        self.alt = packet.decodeTransform()
+        self.bestReprojError = packet.decodeDouble()
+        self.altReprojError = packet.decodeDouble()
+        self.ambiguity = packet.decodeDouble()
+        return packet
+
+
+@dataclass
+class MultiTargetPNPResult:
+    _MAX_IDS = 32
+    # pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally)
+    _PACK_SIZE_BYTES = PNPResult.PACK_SIZE_BYTES + (1 * _MAX_IDS)
+
+    estimatedPose: PNPResult = field(default_factory=PNPResult)
+    fiducialIDsUsed: list[int] = field(default_factory=list)
+
+    def createFromPacket(self, packet: Packet) -> Packet:
+        self.estimatedPose = PNPResult()
+        self.estimatedPose.createFromPacket(packet)
+        self.fiducialIDsUsed = []
+        for _ in range(MultiTargetPNPResult._MAX_IDS):
+            fidId = packet.decode16()
+            if fidId >= 0:
+                self.fiducialIDsUsed.append(fidId)
+        return packet

photon-lib/py/photonlibpy/packet.py

@@ -0,0 +1,143 @@
+import struct
+from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Quaternion
+import wpilib
+
+
+class Packet:
+    def __init__(self, data: list[int]):
+        """
+        * Constructs an empty packet.
+        *
+        * @param self.size The self.size of the packet buffer.
+        """
+        self.packetData = data
+        self.size = len(data)
+        self.readPos = 0
+        self.outOfBytes = False
+
+    def clear(self):
+        """Clears the packet and resets the read and write positions."""
+        self.packetData = [0] * self.size
+        self.readPos = 0
+        self.outOfBytes = False
+
+    def getSize(self):
+        return self.size
+
+    _NO_MORE_BYTES_MESSAGE = """
+    Photonlib - Ran out of bytes while decoding.
+    Make sure the version of photonvision on the coprocessor
+    matches the version of photonlib running in the robot code.
+    """
+
+    def _getNextByte(self) -> int:
+        retVal = 0x00
+
+        if not self.outOfBytes:
+            try:
+                retVal = 0x00FF & self.packetData[self.readPos]
+                self.readPos += 1
+            except IndexError:
+                wpilib.reportError(Packet._NO_MORE_BYTES_MESSAGE, True)
+                self.outOfBytes = True
+
+        return retVal
+
+    def getData(self) -> list[int]:
+        """
+        * Returns the packet data.
+        *
+        * @return The packet data.
+        """
+        return self.packetData
+
+    def setData(self, data: list[int]):
+        """
+        * Sets the packet data.
+        *
+        * @param data The packet data.
+        """
+        self.clear()
+        self.packetData = data
+        self.size = len(self.packetData)
+
+    def _decodeGeneric(self, unpackFormat, numBytes):
+        # Read ints in from the data buffer
+        intList = []
+        for _ in range(numBytes):
+            intList.append(self._getNextByte())
+
+        # Interpret the bytes as a floating point number
+        value = struct.unpack(unpackFormat, bytes(intList))[0]
+
+        return value
+
+    def decode8(self) -> int:
+        """
+        * Returns a single decoded byte from the packet.
+        *
+        * @return A decoded byte from the packet.
+        """
+        return self._decodeGeneric(">b", 1)
+
+    def decode16(self) -> int:
+        """
+        * Returns a single decoded byte from the packet.
+        *
+        * @return A decoded byte from the packet.
+        """
+        return self._decodeGeneric(">h", 2)
+
+    def decode32(self) -> int:
+        """
+        * Returns a decoded int (32 bytes) from the packet.
+        *
+        * @return A decoded int from the packet.
+        """
+        return self._decodeGeneric(">l", 4)
+
+    def decodeDouble(self) -> float:
+        """
+        * Returns a decoded double from the packet.
+        *
+        * @return A decoded double from the packet.
+        """
+        return self._decodeGeneric(">d", 8)
+
+    def decodeBoolean(self) -> bool:
+        """
+        * Returns a decoded boolean from the packet.
+        *
+        * @return A decoded boolean from the packet.
+        """
+        return self.decode8() == 1
+
+    def decodeDoubleArray(self, length: int) -> list[float]:
+        """
+        * Returns a decoded array of floats from the packet.
+        *
+        * @return A decoded array of floats from the packet.
+        """
+        ret = []
+        for _ in range(length):
+            ret.append(self.decodeDouble())
+        return ret
+
+    def decodeTransform(self) -> Transform3d:
+        """
+        * Returns a decoded Transform3d
+        *
+        * @return A decoded Tansform3d from the packet.
+        """
+        x = self.decodeDouble()
+        y = self.decodeDouble()
+        z = self.decodeDouble()
+        translation = Translation3d(x, y, z)
+
+        w = self.decodeDouble()
+        x = self.decodeDouble()
+        y = self.decodeDouble()
+        z = self.decodeDouble()
+        rotation = Rotation3d(Quaternion(w, x, y, z))
+
+        return Transform3d(translation, rotation)