diff --git a/photon-lib/py/photonlibpy/estimation/openCVHelp.py b/photon-lib/py/photonlibpy/estimation/openCVHelp.py index 79eb175af..8a6f4911f 100644 --- a/photon-lib/py/photonlibpy/estimation/openCVHelp.py +++ b/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] - y += p[1] - return (x / len(points), y / len(points)) + x += p[0, 0] + y += p[0, 1] + 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 diff --git a/photon-lib/py/photonlibpy/networktables/NTTopicSet.py b/photon-lib/py/photonlibpy/networktables/NTTopicSet.py index ae095b00a..462c48072 100644 --- a/photon-lib/py/photonlibpy/networktables/NTTopicSet.py +++ b/photon-lib/py/photonlibpy/networktables/NTTopicSet.py @@ -10,8 +10,10 @@ PhotonPipelineResult_TYPE_STRING = ( class NTTopicSet: - def __init__(self) -> None: - self.subTable = nt.NetworkTableInstance.getDefault() + def __init__(self, tableName: str, cameraName: str) -> None: + instance = nt.NetworkTableInstance.getDefault() + photonvision_root_table = instance.getTable(tableName) + self.subTable = photonvision_root_table.getSubTable(cameraName) def updateEntries(self) -> None: options = nt.PubSubOptions() diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index 18937cad1..c7635dca5 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/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( diff --git a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py index 97bb3d870..fb2fd7d74 100644 --- a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py +++ b/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) diff --git a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py index 1ab414607..3ae234b52 100644 --- a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py +++ b/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] diff --git a/photon-lib/py/photonlibpy/simulation/visionSystemSim.py b/photon-lib/py/photonlibpy/simulation/visionSystemSim.py index 839e01d5f..0c5903d62 100644 --- a/photon-lib/py/photonlibpy/simulation/visionSystemSim.py +++ b/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) diff --git a/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py b/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py index 55eca81c7..1eb03ea60 100644 --- a/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py +++ b/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"]