photonlibpy 2025.0.0a0__py3-none-any.whl → 2025.0.0b2__py3-none-any.whl

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()

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()

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

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()

photonlibpy/networktables/__init__.py

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

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)

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('"', "")
 

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

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