Skip to content

Commit

Permalink
wpiformat, better test infrastructure, derive tag name
Browse files Browse the repository at this point in the history
  • Loading branch information
gerth2 committed Dec 13, 2023
1 parent eba7c76 commit 3fb8b8e
Show file tree
Hide file tree
Showing 11 changed files with 434 additions and 152 deletions.
9 changes: 5 additions & 4 deletions .github/workflows/python.yml
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ jobs:
- name: Set up Python
uses: actions/setup-python@v5
with:
python-version: 3.11 # CHose earliest that robotpy supports
python-version: 3.10

- name: Install dependencies
run: |
Expand All @@ -38,9 +38,9 @@ jobs:
- name: Run Unit Tests
working-directory: ./photon-lib/py
run: |
pip install dist/photonlibpy-0.0.2-py3-none-any.whl
pip install --no-cache-dir dist/*.whl
pytest
- name: Upload artifacts
uses: actions/upload-artifact@master
Expand All @@ -65,7 +65,8 @@ jobs:
name: dist
path: 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:
repository-url: https://test.pypi.org/legacy/

2 changes: 1 addition & 1 deletion photon-lib/py/.gitignore
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
photonlibpy.egg-info/
dist/
build/
.eggs/
.eggs/
14 changes: 14 additions & 0 deletions photon-lib/py/buildAndTest.bat
Original file line number Diff line number Diff line change
@@ -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
28 changes: 15 additions & 13 deletions photon-lib/py/photonlibpy/multiTargetPNPResult.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,20 +2,22 @@
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)
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
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:
def createFromPacket(self, packet: Packet) -> Packet:
self.isPresent = packet.decodeBoolean()
self.best = packet.decodeTransform()
self.alt = packet.decodeTransform()
Expand All @@ -24,23 +26,23 @@ def createFromPacket(self, packet:Packet) -> Packet:
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)
estimatedPose: PNPResult = field(default_factory=PNPResult)
fiducialIDsUsed: list[int] = field(default_factory=list)

def createFromPacket(self, packet:Packet) -> Packet:
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):
if (fidId >= 0):
self.fiducialIDsUsed.append(fidId)
return packet

56 changes: 26 additions & 30 deletions photon-lib/py/photonlibpy/packet.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,10 @@
from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Quaternion
import wpilib

class Packet:

class Packet:

def __init__(self, data:list[int]):
def __init__(self, data: list[int]):
"""
* Constructs an empty packet.
*
Expand All @@ -15,28 +15,26 @@ def __init__(self, data:list[int]):
self.size = len(data)
self.readPos = 0
self.outOfBytes = False


def clear(self):
def clear(self):
""" Clears the packet and resets the read and write positions."""
self.packetData = [0]*self.size
self.packetData = [0] * self.size
self.readPos = 0
self.outOfBytes = False


def getSize(self):
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
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):

if (not self.outOfBytes):
try:
retVal = 0x00ff & self.packetData[self.readPos]
self.readPos += 1
Expand All @@ -46,15 +44,15 @@ def _getNextByte(self) -> int:

return retVal

def getData(self) -> list[int]:
def getData(self) -> list[int]:
"""
* Returns the packet data.
* Returns the packet data.
*
* @return The packet data.
"""
return self.packetData
def setData(self, data:list[int]):

def setData(self, data: list[int]):
"""
* Sets the packet data.
*
Expand All @@ -63,51 +61,50 @@ def setData(self, data:list[int]):
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:
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:

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:
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:
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:
"""
Expand All @@ -117,7 +114,7 @@ def decodeBoolean(self) -> bool:
"""
return (self.decode8() == 1)

def decodeDoubleArray(self, length:int) -> list[float]:
def decodeDoubleArray(self, length: int) -> list[float]:
"""
* Returns a decoded array of floats from the packet.
*
Expand All @@ -127,7 +124,7 @@ def decodeDoubleArray(self, length:int) -> list[float]:
for _ in range(length):
ret.append(self.decodeDouble())
return ret

def decodeTransform(self) -> Transform3d:
"""
* Returns a decoded Transform3d
Expand All @@ -137,13 +134,12 @@ def decodeTransform(self) -> Transform3d:
x = self.decodeDouble()
y = self.decodeDouble()
z = self.decodeDouble()
translation = Translation3d(x,y,z)
translation = Translation3d(x, y, z)

w = self.decodeDouble()
x = self.decodeDouble()
y = self.decodeDouble()
z = self.decodeDouble()
rotation = Rotation3d(Quaternion(w,x,y,z))
rotation = Rotation3d(Quaternion(w, x, y, z))

return Transform3d(translation, rotation)

Loading

0 comments on commit 3fb8b8e

Please sign in to comment.