photonUtils

  1import math
  2from typing import overload
  3
  4from wpimath.geometry import (
  5    Pose2d,
  6    Pose3d,
  7    Rotation2d,
  8    Transform2d,
  9    Transform3d,
 10    Translation2d,
 11)
 12
 13
 14class PhotonUtils:
 15    def __init__():
 16        pass
 17
 18    @staticmethod
 19    def calculateDistanceToTargetMeters(
 20        cameraHeightMeters: float,
 21        targetHeightMeters: float,
 22        cameraPitchRadians: float,
 23        targetPitchRadians: float,
 24    ) -> float:
 25        return (targetHeightMeters - cameraHeightMeters) / math.tan(
 26            cameraPitchRadians + targetPitchRadians
 27        )
 28
 29    @staticmethod
 30    def estimateCameraToTargetTranslation(targetDistanceMeters: float, yaw: Rotation2d):
 31        return Translation2d(
 32            yaw.cos() * targetDistanceMeters, yaw.sin() * targetDistanceMeters
 33        )
 34
 35    @staticmethod
 36    def estimateCameraToTarget(
 37        cameraToTargetTranslation: Translation2d,
 38        fieldToTarget: Pose2d,
 39        gyroAngle: Rotation2d,
 40    ):
 41        return Transform2d(
 42            cameraToTargetTranslation, gyroAngle * (-1) - fieldToTarget.rotation()
 43        )
 44
 45    @staticmethod
 46    def estimateFieldToCamera(cameraToTarget: Transform2d, fieldToTarget: Pose2d):
 47        targetToCamera = cameraToTarget.inverse()
 48        return fieldToTarget.transformBy(targetToCamera)
 49
 50    @staticmethod
 51    @overload
 52    def estimateFieldToRobot(
 53        cameraToTarget: Transform2d, fieldToTarget: Pose2d, cameraToRobot: Transform2d
 54    ):
 55        return PhotonUtils.estimateFieldToCamera(
 56            cameraToTarget, fieldToTarget
 57        ).transformBy(cameraToRobot)
 58
 59    @staticmethod
 60    def estimateFieldToRobotAprilTag(
 61        cameraToTarget: Transform3d,
 62        fieldRelativeTagPose: Pose3d,
 63        cameraToRobot: Transform3d,
 64    ):
 65        return fieldRelativeTagPose + cameraToTarget.inverse() + cameraToRobot
 66
 67    @staticmethod
 68    def getYawToPose(robotPose: Pose2d, targetPose: Pose2d):
 69        relativeTr1 = targetPose.relativeTo(robotPose).translation()
 70        return Rotation2d(relativeTr1.X(), relativeTr1.Y())
 71
 72    @staticmethod
 73    def getDistanceToPose(robotPose: Pose2d, targetPose: Pose2d):
 74        return robotPose.translation().distance(targetPose.translation())
 75
 76    @staticmethod
 77    @overload
 78    def estimateFieldToRobot(
 79        cameraHeightMeters: float,
 80        targetHeightMeters: float,
 81        cameraPitchRadians: float,
 82        targetPitchRadians: float,
 83        targetYaw: Rotation2d,
 84        gyroAngle: Rotation2d,
 85        fieldToTarget: Pose2d,
 86        cameraToRobot: Transform2d,
 87    ):
 88        return PhotonUtils.estimateFieldToRobot(
 89            PhotonUtils.estimateCameraToTarget(
 90                PhotonUtils.estimateCameraToTargetTranslation(
 91                    PhotonUtils.calculateDistanceToTargetMeters(
 92                        cameraHeightMeters,
 93                        targetHeightMeters,
 94                        cameraPitchRadians,
 95                        targetPitchRadians,
 96                    ),
 97                    targetYaw,
 98                ),
 99                fieldToTarget,
100                gyroAngle,
101            ),
102            fieldToTarget,
103            cameraToRobot,
104        )