Skip to content

Commit

Permalink
are ya happy wpiformat. Just be happy.
Browse files Browse the repository at this point in the history
Stop being 2017 dean.
  • Loading branch information
gerth2 committed Dec 13, 2023
1 parent efa95e4 commit d591c2c
Show file tree
Hide file tree
Showing 8 changed files with 60 additions and 58 deletions.
7 changes: 2 additions & 5 deletions photon-lib/py/photonlibpy/multiTargetPNPResult.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,10 +5,8 @@

@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)
Expand All @@ -29,7 +27,6 @@ def createFromPacket(self, packet: Packet) -> 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)
Expand All @@ -43,6 +40,6 @@ def createFromPacket(self, packet: Packet) -> Packet:
self.fiducialIDsUsed = []
for _ in range(MultiTargetPNPResult._MAX_IDS):
fidId = packet.decode16()
if (fidId >= 0):
if fidId >= 0:
self.fiducialIDsUsed.append(fidId)
return packet
16 changes: 7 additions & 9 deletions photon-lib/py/photonlibpy/packet.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,20 +4,19 @@


class Packet:

def __init__(self, data: list[int]):
"""
* Constructs an empty packet.
*
* @param self.size The self.size of the packet buffer.
* 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."""
"""Clears the packet and resets the read and write positions."""
self.packetData = [0] * self.size
self.readPos = 0
self.outOfBytes = False
Expand All @@ -34,9 +33,9 @@ def getSize(self):
def _getNextByte(self) -> int:
retVal = 0x00

if (not self.outOfBytes):
if not self.outOfBytes:
try:
retVal = 0x00ff & self.packetData[self.readPos]
retVal = 0x00FF & self.packetData[self.readPos]
self.readPos += 1
except IndexError:
wpilib.reportError(Packet._NO_MORE_BYTES_MESSAGE, True)
Expand All @@ -63,7 +62,6 @@ def setData(self, data: list[int]):
self.size = len(self.packetData)

def _decodeGeneric(self, unpackFormat, numBytes):

# Read ints in from the data buffer
intList = []
for _ in range(numBytes):
Expand Down Expand Up @@ -112,7 +110,7 @@ def decodeBoolean(self) -> bool:
*
* @return A decoded boolean from the packet.
"""
return (self.decode8() == 1)
return self.decode8() == 1

def decodeDoubleArray(self, length: int) -> list[float]:
"""
Expand Down
55 changes: 32 additions & 23 deletions photon-lib/py/photonlibpy/photonCamera.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,42 +13,50 @@ class VisionLEDMode(Enum):


class PhotonCamera:

def __init__(self, cameraName: str):
instance = ntcore.NetworkTableInstance.getDefault()
self.name = cameraName
photonvision_root_table = instance.getTable("photonvision")
cameraTable = photonvision_root_table.getSubTable(cameraName)
self.path = cameraTable.getPath()
self.rawBytesEntry = cameraTable.getRawTopic("rawBytes") \
.subscribe("rawBytes", bytes([]), ntcore.PubSubOptions(periodic=0.01, sendAll=True))
self.rawBytesEntry = cameraTable.getRawTopic("rawBytes").subscribe(
"rawBytes", bytes([]), ntcore.PubSubOptions(periodic=0.01, sendAll=True)
)

self.driverModePublisher = cameraTable.getBooleanTopic(
"driverModeRequest").publish()
self.driverModeSubscriber = cameraTable.getBooleanTopic(
"driverMode").subscribe(False)
"driverModeRequest"
).publish()
self.driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(
False
)
self.inputSaveImgEntry = cameraTable.getIntegerTopic(
"inputSaveImgCmd").getEntry(0)
"inputSaveImgCmd"
).getEntry(0)
self.outputSaveImgEntry = cameraTable.getIntegerTopic(
"outputSaveImgCmd").getEntry(0)
"outputSaveImgCmd"
).getEntry(0)
self.pipelineIndexRequest = cameraTable.getIntegerTopic(
"pipelineIndexRequest").publish()
"pipelineIndexRequest"
).publish()
self.pipelineIndexState = cameraTable.getIntegerTopic(
"pipelineIndexState").subscribe(0)
self.heartbeatEntry = cameraTable.getIntegerTopic(
"heartbeat").subscribe(-1)
"pipelineIndexState"
).subscribe(0)
self.heartbeatEntry = cameraTable.getIntegerTopic("heartbeat").subscribe(-1)

self.ledModeRequest = photonvision_root_table.getIntegerTopic(
"ledModeRequest").publish()
"ledModeRequest"
).publish()
self.ledModeState = photonvision_root_table.getIntegerTopic(
"ledModeState").subscribe(-1)
self.versionEntry = photonvision_root_table.getStringTopic(
"version").subscribe("")
"ledModeState"
).subscribe(-1)
self.versionEntry = photonvision_root_table.getStringTopic("version").subscribe(
""
)

# Existing is enough to make this multisubscriber do its thing
self.topicNameSubscriber = \
ntcore.MultiSubscriber( \
instance, ["/photonvision/"], ntcore.PubSubOptions(topicsOnly =True))
self.topicNameSubscriber = ntcore.MultiSubscriber(
instance, ["/photonvision/"], ntcore.PubSubOptions(topicsOnly=True)
)

self.prevHeartbeat = 0
self.prevHeartbeatChangeTime = Timer.getFPGATimestamp()
Expand All @@ -59,13 +67,14 @@ def getLatestResult(self) -> PhotonPipelineResult:
byteList = packetWithTimestamp.value
timestamp = packetWithTimestamp.time

if (len(byteList) < 1):
if len(byteList) < 1:
return retVal
else:
retVal.populateFromPacket(Packet(byteList))
# NT4 allows us to correct the timestamp based on when the message was sent
retVal.setTimestampSeconds(timestamp / 1e-6 -
retVal.getLatencyMillis() / 1e-3)
retVal.setTimestampSeconds(
timestamp / 1e-6 - retVal.getLatencyMillis() / 1e-3
)
return retVal

def getDriverMode(self) -> bool:
Expand Down Expand Up @@ -100,7 +109,7 @@ def isConnected(self) -> bool:
curHeartbeat = self.heartbeatEntry.get()
now = Timer.getFPGATimestamp()

if (curHeartbeat != self.prevHeartbeat):
if curHeartbeat != self.prevHeartbeat:
self.prevHeartbeat = curHeartbeat
self.prevHeartbeatChangeTime = now

Expand Down
3 changes: 1 addition & 2 deletions photon-lib/py/photonlibpy/photonPipelineResult.py
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,7 @@ class PhotonPipelineResult:
latencyMillis: float = -1.0
timestampSec: float = -1.0
targets: list[PhotonTrackedTarget] = field(default_factory=list)
multiTagResult: MultiTargetPNPResult = field(
default_factory=MultiTargetPNPResult)
multiTagResult: MultiTargetPNPResult = field(default_factory=MultiTargetPNPResult)

def populateFromPacket(self, packet: Packet) -> Packet:
self.targets = []
Expand Down
10 changes: 3 additions & 7 deletions photon-lib/py/photonlibpy/photonTrackedTarget.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,9 @@ class TargetCorner:

@dataclass
class PhotonTrackedTarget:

_MAX_CORNERS = 8
_NUM_BYTES_IN_FLOAT = 8
_PACK_SIZE_BYTES = _NUM_BYTES_IN_FLOAT * (5 + 7 + 2 * 4 + 1 + 7 +
2 * _MAX_CORNERS)
_PACK_SIZE_BYTES = _NUM_BYTES_IN_FLOAT * (5 + 7 + 2 * 4 + 1 + 7 + 2 * _MAX_CORNERS)

yaw: float = 0.0
pitch: float = 0.0
Expand Down Expand Up @@ -58,8 +56,7 @@ def getBestCameraToTarget(self) -> Transform3d:
def getAlternateCameraToTarget(self) -> Transform3d:
return self.altCameraToTarget

def _decodeTargetList(self, packet: Packet,
numTargets: int) -> list[TargetCorner]:
def _decodeTargetList(self, packet: Packet, numTargets: int) -> list[TargetCorner]:
retList = []
for _ in range(numTargets):
cx = packet.decodeDouble()
Expand All @@ -79,8 +76,7 @@ def createFromPacket(self, packet: Packet) -> Packet:

self.poseAmbiguity = packet.decodeDouble()

self.minAreaRectCorners = self._decodeTargetList(packet,
4) # always four
self.minAreaRectCorners = self._decodeTargetList(packet, 4) # always four
numCorners = packet.decode8()
self.detectedCorners = self._decodeTargetList(packet, numCorners)
return packet
12 changes: 7 additions & 5 deletions photon-lib/py/setup.py
Original file line number Diff line number Diff line change
@@ -1,11 +1,13 @@
from setuptools import setup, find_packages
import subprocess, re

gitDescribeResult = subprocess.check_output(
['git', 'describe', '--tags', "--match=v*",
"--always"]).decode('utf-8').strip()
gitDescribeResult = (
subprocess.check_output(["git", "describe", "--tags", "--match=v*", "--always"])
.decode("utf-8")
.strip()
)

m = re.search(r'v[0-9]{4}\.[0-9]{1}.[0-9]{1}', gitDescribeResult)
m = re.search(r"v[0-9]{4}\.[0-9]{1}.[0-9]{1}", gitDescribeResult)

# Extract the first portion of the git describe result
# which should be PEP440 compliant
Expand All @@ -24,7 +26,7 @@
"""

setup(
name='photonlibpy',
name="photonlibpy",
packages=find_packages(),
version=versionString,
install_requires=[
Expand Down
1 change: 1 addition & 0 deletions photon-lib/py/test/data.py
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
# fmt: off
rawBytes1 = [
64, 166, 117, 41, 225, 243, 165, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
Expand Down
14 changes: 7 additions & 7 deletions photon-lib/py/test/photonlibpy_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,35 +12,35 @@ def setupCommon(bytesIn):
res = PhotonPipelineResult()
packet = Packet(bytesIn)
res.populateFromPacket(packet)
assert (packet.outOfBytes is False)
assert packet.outOfBytes is False
return res


def test_byteParse1():
res = setupCommon(rawBytes1)
assert (len(res.getTargets()) == 0)
assert len(res.getTargets()) == 0


def test_byteParse2():
res = setupCommon(rawBytes2)
assert (len(res.getTargets()) == 0)
assert len(res.getTargets()) == 0


def test_byteParse3():
res = setupCommon(rawBytes3)
assert (len(res.getTargets()) == 0)
assert len(res.getTargets()) == 0


def test_byteParse4():
res = setupCommon(rawBytes4)
assert (len(res.getTargets()) == 1)
assert len(res.getTargets()) == 1


def test_byteParse5():
res = setupCommon(rawBytes5)
assert (len(res.getTargets()) == 1)
assert len(res.getTargets()) == 1


def test_byteParse6():
res = setupCommon(rawBytes6)
assert (len(res.getTargets()) > 6)
assert len(res.getTargets()) > 6

0 comments on commit d591c2c

Please sign in to comment.