Correct yet more python type hinting (#1555)

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[
-            typing.Tuple[VisionTargetSim, list[typing.Tuple[float, float]]]
-        ] = []
+        visibleTgts: list[typing.Tuple[VisionTargetSim, np.ndarray]] = []
         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:
-        return (
-            cv.contourArea(cv.convexHull(np.array(points))) / self.getResArea() * 100.0
-        )
+    def getContourAreaPercent(self, points: np.ndarray) -> float:
+        return cv.contourArea(cv.convexHull(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(
-                camSim, timestampCapture
-            )
+            robotToCamera = self.getRobotToCamera(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)