mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
47aea29b6b
* Added a pure-python implementation of photonlib, named photonlibpy and hosted on pypi --------- Co-authored-by: Matt <matthew.morley.ca@gmail.com>
83 lines
2.4 KiB
Python
83 lines
2.4 KiB
Python
from dataclasses import dataclass, field
|
|
from wpimath.geometry import Transform3d
|
|
from photonlibpy.packet import Packet
|
|
|
|
|
|
@dataclass
|
|
class TargetCorner:
|
|
x: float
|
|
y: float
|
|
|
|
|
|
@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)
|
|
|
|
yaw: float = 0.0
|
|
pitch: float = 0.0
|
|
area: float = 0.0
|
|
skew: float = 0.0
|
|
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
|
|
poseAmbiguity: float = 0.0
|
|
|
|
def getYaw(self) -> float:
|
|
return self.yaw
|
|
|
|
def getPitch(self) -> float:
|
|
return self.pitch
|
|
|
|
def getArea(self) -> float:
|
|
return self.area
|
|
|
|
def getSkew(self) -> float:
|
|
return self.skew
|
|
|
|
def getFiducialId(self) -> int:
|
|
return self.fiducialId
|
|
|
|
def getPoseAmbiguity(self) -> float:
|
|
return self.poseAmbiguity
|
|
|
|
def getMinAreaRectCorners(self) -> list[TargetCorner] | None:
|
|
return self.minAreaRectCorners
|
|
|
|
def getDetectedCorners(self) -> list[TargetCorner] | None:
|
|
return self.detectedCorners
|
|
|
|
def getBestCameraToTarget(self) -> Transform3d:
|
|
return self.bestCameraToTarget
|
|
|
|
def getAlternateCameraToTarget(self) -> Transform3d:
|
|
return self.altCameraToTarget
|
|
|
|
def _decodeTargetList(self, packet: Packet, numTargets: int) -> list[TargetCorner]:
|
|
retList = []
|
|
for _ in range(numTargets):
|
|
cx = packet.decodeDouble()
|
|
cy = packet.decodeDouble()
|
|
retList.append(TargetCorner(cx, cy))
|
|
return retList
|
|
|
|
def createFromPacket(self, packet: Packet) -> Packet:
|
|
self.yaw = packet.decodeDouble()
|
|
self.pitch = packet.decodeDouble()
|
|
self.area = packet.decodeDouble()
|
|
self.skew = packet.decodeDouble()
|
|
self.fiducialId = packet.decode32()
|
|
|
|
self.bestCameraToTarget = packet.decodeTransform()
|
|
self.altCameraToTarget = packet.decodeTransform()
|
|
|
|
self.poseAmbiguity = packet.decodeDouble()
|
|
|
|
self.minAreaRectCorners = self._decodeTargetList(packet, 4) # always four
|
|
numCorners = packet.decode8()
|
|
self.detectedCorners = self._decodeTargetList(packet, numCorners)
|
|
return packet
|