Ported PhotonUtils into Python

photon-lib/py/photonlibpy/photonUtils.py

@@ -0,0 +1,65 @@
+import photonlibpy
+from wpimath.geometry import Translation2d, Rotation2d, Pose2d, Transform2d, Pose3d, Transform3d
+import math
+
+class PhotonUtils:
+    def __init__():
+        pass
+
+    @staticmethod
+    def calculateDistanceToTargetMeters(cameraHeightMeters: float,
+                                        targetHeightMeters: float,
+                                        cameraPitchRadians: float,
+                                        targetPitchRadians: float) -> float:
+        return (targetHeightMeters - cameraHeightMeters) / math.tan(cameraPitchRadians + targetPitchRadians)
+
+    @staticmethod
+    def estimateCameraToTargetTranslation(targetDistanceMeters: float,
+                                          yaw: Rotation2d):
+        return Translation2d(yaw.cos() * targetDistanceMeters, yaw.sin() * targetDistanceMeters)
+
+    @staticmethod
+    def estimateCameraToTarget(cameraToTargetTranslation: Translation2d,
+                               fieldToTarget : Pose2d,
+                               gyroAngle: Rotation2d):
+        return Transform2d(cameraToTargetTranslation, gyroAngle * (-1) - fieldToTarget.rotation())
+
+    @staticmethod
+    def estimateFieldToCamera(cameraToTarget: Transform2d,
+                              fieldToTarget: Pose2d):
+        targetToCamera = cameraToTarget.inverse()
+        return fieldToTarget.transformBy(targetToCamera)
+
+    @staticmethod
+    def estimateFieldToRobot(cameraToTarget: Transform2d,
+                             fieldToTarget: Pose2d,
+                             cameraToRobot: Transform2d):
+        return PhotonUtils.estimateFieldToCamera(cameraToTarget, fieldToTarget).transformBy(cameraToRobot)
+
+    @staticmethod
+    def estimateFieldToRobotAprilTag(cameraToTarget: Transform3d,
+                                     fieldRelativeTagPose: Pose3d,
+                                     cameraToRobot: Transform3d):
+        return fieldRelativeTagPose + cameraToTarget.inverse() + cameraToRobot
+
+    @staticmethod
+    def getYawToPose(robotPose: Pose2d,
+                     targetPose: Pose2d):
+        relativeTr1 = targetPose.relativeTo(robotPose).translation()
+        return Rotation2d(relativeTr1.X(), relativeTr1.Y())
+
+    @staticmethod
+    def getDistanceToPose(robotPose: Pose2d,
+                          targetPose: Pose2d):
+        return robotPose.translation().distance(targetPose.translation())
+
+    @staticmethod
+    def estimateFieldToRobot(cameraHeightMeters: float,
+                             targetHeightMeters: float,
+                             cameraPitchRadians: float,
+                             targetPitchRadians: float,
+                             targetYaw: Rotation2d,
+                             gyroAngle: Rotation2d,
+                             fieldToTarget: Pose2d,
+                             cameraToRobot: Transform2d):
+        return PhotonUtils.estimateFieldToRobot(PhotonUtils.estimateCameraToTarget(PhotonUtils.estimateCameraToTargetTranslation(PhotonUtils.calculateDistanceToTargetMeters(cameraHeightMeters, targetHeightMeters, cameraPitchRadians, targetPitchRadians), targetYaw), fieldToTarget, gyroAngle), fieldToTarget, cameraToRobot)

photon-lib/py/test/visionSystemSim_test.py

@@ -3,6 +3,7 @@
 import pytest
 from photonlibpy.estimation import TargetModel, VisionEstimation
 from photonlibpy.photonCamera import PhotonCamera
+from photonlibpy.photonUtils import PhotonUtils
 from photonlibpy.simulation import PhotonCameraSim, VisionSystemSim, VisionTargetSim
 from robotpy_apriltag import AprilTag, AprilTagFieldLayout
 from wpimath.geometry import (
@@ -333,11 +334,10 @@ def test_distanceCalc(distParam, pitchParam, heightParam) -> None:
     assert target is not None
     assert target.getYaw() == pytest.approx(0.0, abs=0.5)
 
-    # TODO Enable when PhotonUtils is ported
-    # dist = PhotonUtils.calculateDistanceToTarget(
-    #    robotToCamera.Z(), targetPose.Z(), -pitchParam, math.degrees(target.getPitch())
-    # )
-    # assert dist == pytest.approx(distParam, abs=0.25)
+    dist = PhotonUtils.calculateDistanceToTargetMeters(
+       robotToCamera.Z(), targetPose.Z(), -pitchParam, math.degrees(target.getPitch())
+    )
+    assert dist == pytest.approx(distParam, abs=0.25)
 
 
 def test_MultipleTargets() -> None: