Correct yet more python type hinting (#1555)

photon-lib/py/photonlibpy/estimation/openCVHelp.py

@@ -1,5 +1,5 @@
 import math
-from typing import Any, Tuple
+from typing import Any
 
 import cv2 as cv
 import numpy as np
@@ -48,13 +48,13 @@ class OpenCVHelp:
         )
 
     @staticmethod
-    def avgPoint(points: list[Tuple[float, float]]) -> Tuple[float, float]:
+    def avgPoint(points: np.ndarray) -> np.ndarray:
         x = 0.0
         y = 0.0
         for p in points:
-            x += p[0]
+            x += p[0, 0]
-            y += p[1]
+            y += p[0, 1]
-        return (x / len(points), y / len(points))
+        return np.array([[x / len(points), y / len(points)]])
 
     @staticmethod
     def pointsToTargetCorners(points: np.ndarray) -> list[TargetCorner]:
@@ -126,6 +126,10 @@ class OpenCVHelp:
         objectMat = np.array(OpenCVHelp.translationToTVec(modelTrls))
 
         alt: Transform3d | None = None
+        reprojectionError: cv.typing.MatLike | None = None
+        best: Transform3d = Transform3d()
+        alt: Transform3d | None = None
+
         for tries in range(2):
             retval, rvecs, tvecs, reprojectionError = cv.solvePnPGeneric(
                 objectMat,
@@ -145,7 +149,9 @@ class OpenCVHelp:
                     OpenCVHelp.rVecToRotation(rvecs[1]),
                 )
 
-            if not math.isnan(reprojectionError[0, 0]):
+            if reprojectionError is not None and not math.isnan(
+                reprojectionError[0, 0]
+            ):
                 break
             else:
                 pt = imagePoints[0]
@@ -153,7 +159,7 @@ class OpenCVHelp:
                 pt[0, 1] -= 0.001
                 imagePoints[0] = pt
 
-        if math.isnan(reprojectionError[0, 0]):
+        if reprojectionError is None or math.isnan(reprojectionError[0, 0]):
             print("SolvePNP_Square failed!")
             return None
 

photon-lib/py/photonlibpy/networktables/NTTopicSet.py

@@ -10,8 +10,10 @@ PhotonPipelineResult_TYPE_STRING = (
 
 class NTTopicSet:
 
-    def __init__(self) -> None:
+    def __init__(self, tableName: str, cameraName: str) -> None:
-        self.subTable = nt.NetworkTableInstance.getDefault()
+        instance = nt.NetworkTableInstance.getDefault()
+        photonvision_root_table = instance.getTable(tableName)
+        self.subTable = photonvision_root_table.getSubTable(cameraName)
 
     def updateEntries(self) -> None:
         options = nt.PubSubOptions()

photon-lib/py/photonlibpy/photonCamera.py

@@ -231,7 +231,7 @@ class PhotonCamera:
         versionString = self.versionEntry.get(defaultValue="")
         localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
 
-        remoteUUID = self._rawBytesEntry.getTopic().getProperty("message_uuid")
+        remoteUUID = str(self._rawBytesEntry.getTopic().getProperty("message_uuid"))
 
         if not remoteUUID:
             wpilib.reportWarning(

photon-lib/py/photonlibpy/simulation/photonCameraSim.py

@@ -42,7 +42,6 @@ class PhotonCameraSim:
         self.videoSimWireframeEnabled: bool = False
         self.videoSimWireframeResolution: float = 0.1
         self.videoSimProcEnabled: bool = True
-        self.ts = NTTopicSet()
         self.heartbeatCounter: int = 0
         self.nextNtEntryTime = int(wpilib.Timer.getFPGATimestamp() * 1e6)
         self.tagLayout = robotpy_apriltag.loadAprilTagLayoutField(
@@ -101,7 +100,7 @@ class PhotonCameraSim:
             (self.prop.getResWidth(), self.prop.getResHeight())
         )
 
-        self.ts.subTable = self.cam._cameraTable
+        self.ts = NTTopicSet("photonvision", self.cam.getName())
         self.ts.updateEntries()
 
         # Handle this last explicitly for this function signature because the other constructor is called in the initialiser list
@@ -217,9 +216,7 @@ class PhotonCameraSim:
 
         targets.sort(key=distance, reverse=True)
 
-        visibleTgts: list[
+        visibleTgts: list[typing.Tuple[VisionTargetSim, np.ndarray]] = []
-            typing.Tuple[VisionTargetSim, list[typing.Tuple[float, float]]]
-        ] = []
         detectableTgts: list[PhotonTrackedTarget] = []
 
         camRt = RotTrlTransform3d.makeRelativeTo(cameraPose)
@@ -258,6 +255,7 @@ class PhotonCameraSim:
                 ] * 4
                 t = (l + 1) % 4
                 b = (l + 1) % 4
+                r = 0
                 for i in range(4):
                     if i == l:
                         continue
@@ -271,14 +269,14 @@ class PhotonCameraSim:
                     if i != t and i != l and i != b:
                         r = i
                 rect = cv.RotatedRect(
-                    center,
+                    (center[0, 0], center[0, 1]),
                     (
                         imagePoints[r, 0, 0] - lc[0, 0],
                         imagePoints[b, 0, 1] - imagePoints[t, 0, 1],
                     ),
                     -angles[r],
                 )
-                imagePoints = rect.points()
+                imagePoints = np.array([[p[0], p[1], p[2]] for p in rect.points()])
 
             visibleTgts.append((tgt, imagePoints))
             noisyTargetCorners = self.prop.estPixelNoise(imagePoints)
@@ -385,6 +383,7 @@ class PhotonCameraSim:
             self.ts.targetSkewEntry.set(0.0, receiveTimestamp)
         else:
             bestTarget = result.getBestTarget()
+            assert bestTarget
 
             self.ts.targetPitchEntry.set(bestTarget.getPitch(), receiveTimestamp)
             self.ts.targetYawEntry.set(bestTarget.getYaw(), receiveTimestamp)

photon-lib/py/photonlibpy/simulation/simCameraProperties.py

@@ -171,10 +171,8 @@ class SimCameraProperties:
     def getLatencyStdDev(self) -> seconds:
         return self.latencyStdDev
 
-    def getContourAreaPercent(self, points: list[typing.Tuple[float, float]]) -> float:
+    def getContourAreaPercent(self, points: np.ndarray) -> float:
-        return (
+        return cv.contourArea(cv.convexHull(points)) / self.getResArea() * 100.0
-            cv.contourArea(cv.convexHull(np.array(points))) / self.getResArea() * 100.0
-        )
 
     def getPixelYaw(self, pixelX: float) -> Rotation2d:
         fx = self.camIntrinsics[0, 0]
@@ -188,14 +186,14 @@ class SimCameraProperties:
         yOffset = cy - pixelY
         return Rotation2d(fy, -yOffset)
 
-    def getPixelRot(self, point: typing.Tuple[int, int]) -> Rotation3d:
+    def getPixelRot(self, point: cv.typing.Point2f) -> Rotation3d:
         return Rotation3d(
             0.0,
             self.getPixelPitch(point[1]).radians(),
             self.getPixelYaw(point[0]).radians(),
         )
 
-    def getCorrectedPixelRot(self, point: typing.Tuple[float, float]) -> Rotation3d:
+    def getCorrectedPixelRot(self, point: cv.typing.Point2f) -> Rotation3d:
         fx = self.camIntrinsics[0, 0]
         cx = self.camIntrinsics[0, 2]
         xOffset = cx - point[0]

photon-lib/py/photonlibpy/simulation/visionSystemSim.py

@@ -84,7 +84,11 @@ class VisionSystemSim:
         if robotToCamera is None:
             return None
         else:
-            return self.getRobotPose(time) + robotToCamera
+            pose = self.getRobotPose(time)
+            if pose:
+                return pose + robotToCamera
+            else:
+                return None
 
     def adjustCamera(
         self, cameraSim: PhotonCameraSim, robotToCamera: Transform3d
@@ -167,7 +171,7 @@ class VisionSystemSim:
 
     def getRobotPose(
         self, timestamp: seconds = wpilib.Timer.getFPGATimestamp()
-    ) -> Pose3d:
+    ) -> Pose3d | None:
         return self.robotPoseBuffer.sample(timestamp)
 
     def resetRobotPose(self, robotPose: Pose2d | Pose3d) -> None:
@@ -216,9 +220,10 @@ class VisionSystemSim:
             timestampCapture = timestampNt * 1.0e-6 - latency
 
             lateRobotPose = self.getRobotPose(timestampCapture)
-            lateCameraPose = lateRobotPose + self.getRobotToCamera(
+            robotToCamera = self.getRobotToCamera(camSim, timestampCapture)
-                camSim, timestampCapture
+            if lateRobotPose is None or robotToCamera is None:
-            )
+                return None
+            lateCameraPose = lateRobotPose + robotToCamera
             cameraPoses2d.append(lateCameraPose.toPose2d())
 
             camResult = camSim.process(latency, lateCameraPose, allTargets)

photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py

@@ -3,8 +3,6 @@ from typing import TYPE_CHECKING, ClassVar
 
 from wpimath.geometry import Transform3d
 
-from ..packet import Packet
-
 if TYPE_CHECKING:
     from .. import generated
 
@@ -27,14 +25,4 @@ class MultiTargetPNPResult:
     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
-
     photonStruct: ClassVar["generated.MultiTargetPNPResultSerde"]