From 30e92599a9969d904ac99652c5716b3ecc120735 Mon Sep 17 00:00:00 2001 From: ConnorLong Date: Sat, 18 Jan 2025 14:00:33 -0700 Subject: [PATCH 01/12] ported photonutils to python --- photon-lib/py/buildAndTest.sh | 6 ++-- photon-lib/py/photonlibpy/photonUtils.py | 45 ++++++++++++++++++++++++ 2 files changed, 48 insertions(+), 3 deletions(-) create mode 100644 photon-lib/py/photonlibpy/photonUtils.py diff --git a/photon-lib/py/buildAndTest.sh b/photon-lib/py/buildAndTest.sh index 0916b41327..702fef90a6 100755 --- a/photon-lib/py/buildAndTest.sh +++ b/photon-lib/py/buildAndTest.sh @@ -1,13 +1,13 @@ # Uninstall if it already was installed -python3 -m pip uninstall -y photonlibpy +python -m pip uninstall -y photonlibpy # Build wheel -python3 setup.py bdist_wheel +python setup.py bdist_wheel # Install whatever wheel was made for f in dist/*.whl; do echo "installing $f" - python3 -m pip install --no-cache-dir "$f" + python -m pip install --no-cache-dir "$f" done # Run the test suite diff --git a/photon-lib/py/photonlibpy/photonUtils.py b/photon-lib/py/photonlibpy/photonUtils.py new file mode 100644 index 0000000000..670345c1d3 --- /dev/null +++ b/photon-lib/py/photonlibpy/photonUtils.py @@ -0,0 +1,45 @@ +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.__mul__(-1).__sub__(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.__add__(cameraToTarget.inverse()).__add__(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) \ No newline at end of file From ecc77a687f952900e619faec861b8316b52d04f3 Mon Sep 17 00:00:00 2001 From: ConnorLong Date: Sat, 18 Jan 2025 14:26:01 -0700 Subject: [PATCH 02/12] enabled photonutils test in simulation test file --- photon-lib/py/test/visionSystemSim_test.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/photon-lib/py/test/visionSystemSim_test.py b/photon-lib/py/test/visionSystemSim_test.py index df5dba4c13..ba47f740fd 100644 --- a/photon-lib/py/test/visionSystemSim_test.py +++ b/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.calculateDistanceToTarget( + robotToCamera.Z(), targetPose.Z(), -pitchParam, math.degrees(target.getPitch()) + ) + assert dist == pytest.approx(distParam, abs=0.25) def test_MultipleTargets() -> None: From 258cb0441499b81884f7940de486ae2dde64b714 Mon Sep 17 00:00:00 2001 From: ConnorLong Date: Sat, 18 Jan 2025 14:55:00 -0700 Subject: [PATCH 03/12] fixed calculate distance unit tests --- photon-lib/py/test/visionSystemSim_test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photon-lib/py/test/visionSystemSim_test.py b/photon-lib/py/test/visionSystemSim_test.py index ba47f740fd..085fcdc769 100644 --- a/photon-lib/py/test/visionSystemSim_test.py +++ b/photon-lib/py/test/visionSystemSim_test.py @@ -334,7 +334,7 @@ def test_distanceCalc(distParam, pitchParam, heightParam) -> None: assert target is not None assert target.getYaw() == pytest.approx(0.0, abs=0.5) - dist = PhotonUtils.calculateDistanceToTarget( + dist = PhotonUtils.calculateDistanceToTargetMeters( robotToCamera.Z(), targetPose.Z(), -pitchParam, math.degrees(target.getPitch()) ) assert dist == pytest.approx(distParam, abs=0.25) From 5e5d13c63d121ea874b4e6e51fe395d5b5b02989 Mon Sep 17 00:00:00 2001 From: raylmorris <=> Date: Wed, 22 Jan 2025 19:57:54 -0700 Subject: [PATCH 04/12] replaced dunder methods with operators --- photon-lib/py/photonlibpy/photonUtils.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/photon-lib/py/photonlibpy/photonUtils.py b/photon-lib/py/photonlibpy/photonUtils.py index 670345c1d3..9d3780c5c7 100644 --- a/photon-lib/py/photonlibpy/photonUtils.py +++ b/photon-lib/py/photonlibpy/photonUtils.py @@ -16,7 +16,7 @@ def estimateCameraToTargetTranslation(targetDistanceMeters : float, yaw : Rotati @staticmethod def estimateCameraToTarget(cameraToTargetTranslation : Translation2d, fieldToTarget : Pose2d, gyroAngle : Rotation2d): - return Transform2d(cameraToTargetTranslation, gyroAngle.__mul__(-1).__sub__(fieldToTarget.rotation())) + return Transform2d(cameraToTargetTranslation, gyroAngle * (-1) - (fieldToTarget.rotation())) @staticmethod def estimateFieldToCamera(cameraToTarget : Transform2d, fieldToTarget : Pose2d): @@ -29,7 +29,7 @@ def estimateFieldToRobot(cameraToTarget : Transform2d, fieldToTarget : Pose2d, c @staticmethod def estimateFieldToRobotAprilTag(cameraToTarget : Transform3d, fieldRelativeTagPose : Pose3d, cameraToRobot : Transform3d): - return fieldRelativeTagPose.__add__(cameraToTarget.inverse()).__add__(cameraToRobot) + return fieldRelativeTagPose + cameraToTarget.inverse() + cameraToRobot @staticmethod def getYawToPose(robotPose : Pose2d, targetPose : Pose2d): From 822f1052655b81c3b1f0427462b45f35deab9da9 Mon Sep 17 00:00:00 2001 From: CollinSwierkosz <147673564+CollinSwierkosz@users.noreply.github.com> Date: Wed, 22 Jan 2025 19:58:56 -0700 Subject: [PATCH 05/12] replaced dunder methods --- photon-lib/py/photonlibpy/photonUtils.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/photon-lib/py/photonlibpy/photonUtils.py b/photon-lib/py/photonlibpy/photonUtils.py index 670345c1d3..9d3780c5c7 100644 --- a/photon-lib/py/photonlibpy/photonUtils.py +++ b/photon-lib/py/photonlibpy/photonUtils.py @@ -16,7 +16,7 @@ def estimateCameraToTargetTranslation(targetDistanceMeters : float, yaw : Rotati @staticmethod def estimateCameraToTarget(cameraToTargetTranslation : Translation2d, fieldToTarget : Pose2d, gyroAngle : Rotation2d): - return Transform2d(cameraToTargetTranslation, gyroAngle.__mul__(-1).__sub__(fieldToTarget.rotation())) + return Transform2d(cameraToTargetTranslation, gyroAngle * (-1) - (fieldToTarget.rotation())) @staticmethod def estimateFieldToCamera(cameraToTarget : Transform2d, fieldToTarget : Pose2d): @@ -29,7 +29,7 @@ def estimateFieldToRobot(cameraToTarget : Transform2d, fieldToTarget : Pose2d, c @staticmethod def estimateFieldToRobotAprilTag(cameraToTarget : Transform3d, fieldRelativeTagPose : Pose3d, cameraToRobot : Transform3d): - return fieldRelativeTagPose.__add__(cameraToTarget.inverse()).__add__(cameraToRobot) + return fieldRelativeTagPose + cameraToTarget.inverse() + cameraToRobot @staticmethod def getYawToPose(robotPose : Pose2d, targetPose : Pose2d): From d1cbd2d9ed2afa348329ab2c227517e897a5ff15 Mon Sep 17 00:00:00 2001 From: raylmorris <=> Date: Fri, 24 Jan 2025 19:02:43 -0700 Subject: [PATCH 06/12] formatting back to python3 --- photon-lib/py/buildAndTest.sh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/photon-lib/py/buildAndTest.sh b/photon-lib/py/buildAndTest.sh index 702fef90a6..0916b41327 100755 --- a/photon-lib/py/buildAndTest.sh +++ b/photon-lib/py/buildAndTest.sh @@ -1,13 +1,13 @@ # Uninstall if it already was installed -python -m pip uninstall -y photonlibpy +python3 -m pip uninstall -y photonlibpy # Build wheel -python setup.py bdist_wheel +python3 setup.py bdist_wheel # Install whatever wheel was made for f in dist/*.whl; do echo "installing $f" - python -m pip install --no-cache-dir "$f" + python3 -m pip install --no-cache-dir "$f" done # Run the test suite From c8818fa903beb52d9719c0ec5dda1318aee9601c Mon Sep 17 00:00:00 2001 From: CollinSwierkosz <147673564+CollinSwierkosz@users.noreply.github.com> Date: Fri, 24 Jan 2025 19:02:52 -0700 Subject: [PATCH 07/12] fromatting back to python3 --- photon-lib/py/buildAndTest.sh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/photon-lib/py/buildAndTest.sh b/photon-lib/py/buildAndTest.sh index 702fef90a6..0916b41327 100755 --- a/photon-lib/py/buildAndTest.sh +++ b/photon-lib/py/buildAndTest.sh @@ -1,13 +1,13 @@ # Uninstall if it already was installed -python -m pip uninstall -y photonlibpy +python3 -m pip uninstall -y photonlibpy # Build wheel -python setup.py bdist_wheel +python3 setup.py bdist_wheel # Install whatever wheel was made for f in dist/*.whl; do echo "installing $f" - python -m pip install --no-cache-dir "$f" + python3 -m pip install --no-cache-dir "$f" done # Run the test suite From 08df4c185b6a2d3b10018e18793fb7fee6d44424 Mon Sep 17 00:00:00 2001 From: ConnorLong Date: Fri, 24 Jan 2025 19:02:53 -0700 Subject: [PATCH 08/12] formatting changed back to python3 --- photon-lib/py/buildAndTest.sh | 6 +++--- photon-lib/py/photonlibpy/photonUtils.py | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/photon-lib/py/buildAndTest.sh b/photon-lib/py/buildAndTest.sh index 702fef90a6..0916b41327 100755 --- a/photon-lib/py/buildAndTest.sh +++ b/photon-lib/py/buildAndTest.sh @@ -1,13 +1,13 @@ # Uninstall if it already was installed -python -m pip uninstall -y photonlibpy +python3 -m pip uninstall -y photonlibpy # Build wheel -python setup.py bdist_wheel +python3 setup.py bdist_wheel # Install whatever wheel was made for f in dist/*.whl; do echo "installing $f" - python -m pip install --no-cache-dir "$f" + python3 -m pip install --no-cache-dir "$f" done # Run the test suite diff --git a/photon-lib/py/photonlibpy/photonUtils.py b/photon-lib/py/photonlibpy/photonUtils.py index 9d3780c5c7..903f4b1cbc 100644 --- a/photon-lib/py/photonlibpy/photonUtils.py +++ b/photon-lib/py/photonlibpy/photonUtils.py @@ -16,7 +16,7 @@ def estimateCameraToTargetTranslation(targetDistanceMeters : float, yaw : Rotati @staticmethod def estimateCameraToTarget(cameraToTargetTranslation : Translation2d, fieldToTarget : Pose2d, gyroAngle : Rotation2d): - return Transform2d(cameraToTargetTranslation, gyroAngle * (-1) - (fieldToTarget.rotation())) + return Transform2d(cameraToTargetTranslation, gyroAngle * (-1) - fieldToTarget.rotation()) @staticmethod def estimateFieldToCamera(cameraToTarget : Transform2d, fieldToTarget : Pose2d): @@ -33,7 +33,7 @@ def estimateFieldToRobotAprilTag(cameraToTarget : Transform3d, fieldRelativeTagP @staticmethod def getYawToPose(robotPose : Pose2d, targetPose : Pose2d): - relativeTr1 = targetPose.relativeTo(robotPose).translation() + relativeTr1 = targetPose.rgitelativeTo(robotPose).translation() return Rotation2d(relativeTr1.X(), relativeTr1.Y()) @staticmethod From 902a89d29f79951667a0b38f9c72962469e804f6 Mon Sep 17 00:00:00 2001 From: GannonCodes Date: Fri, 24 Jan 2025 19:37:22 -0700 Subject: [PATCH 09/12] change back to python3 --- photon-lib/py/photonlibpy/photonUtils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photon-lib/py/photonlibpy/photonUtils.py b/photon-lib/py/photonlibpy/photonUtils.py index 9d3780c5c7..bcf756aad5 100644 --- a/photon-lib/py/photonlibpy/photonUtils.py +++ b/photon-lib/py/photonlibpy/photonUtils.py @@ -16,7 +16,7 @@ def estimateCameraToTargetTranslation(targetDistanceMeters : float, yaw : Rotati @staticmethod def estimateCameraToTarget(cameraToTargetTranslation : Translation2d, fieldToTarget : Pose2d, gyroAngle : Rotation2d): - return Transform2d(cameraToTargetTranslation, gyroAngle * (-1) - (fieldToTarget.rotation())) + return Transform2d(cameraToTargetTranslation, gyroAngle * (-1) - fieldToTarget.rotation()) @staticmethod def estimateFieldToCamera(cameraToTarget : Transform2d, fieldToTarget : Pose2d): From 1e6245cb103cc1ce62d14ade1a6b2b83f72d9a91 Mon Sep 17 00:00:00 2001 From: ConnorLong Date: Fri, 24 Jan 2025 19:57:41 -0700 Subject: [PATCH 10/12] fixed formatting in photonUtils.py --- photon-lib/py/photonlibpy/photonUtils.py | 40 ++++++++++++++++++------ 1 file changed, 30 insertions(+), 10 deletions(-) diff --git a/photon-lib/py/photonlibpy/photonUtils.py b/photon-lib/py/photonlibpy/photonUtils.py index 903f4b1cbc..1d69b89c4f 100644 --- a/photon-lib/py/photonlibpy/photonUtils.py +++ b/photon-lib/py/photonlibpy/photonUtils.py @@ -7,39 +7,59 @@ def __init__(): pass @staticmethod - def calculateDistanceToTargetMeters(cameraHeightMeters : float, targetHeightMeters : float, cameraPitchRadians : float, targetPitchRadians : float) -> float: + 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): + def estimateCameraToTargetTranslation(targetDistanceMeters: float, + yaw: Rotation2d): return Translation2d(yaw.cos() * targetDistanceMeters, yaw.sin() * targetDistanceMeters) @staticmethod - def estimateCameraToTarget(cameraToTargetTranslation : Translation2d, fieldToTarget : Pose2d, gyroAngle : Rotation2d): + def estimateCameraToTarget(cameraToTargetTranslation: Translation2d, + fieldToTarget : Pose2d, + gyroAngle: Rotation2d): return Transform2d(cameraToTargetTranslation, gyroAngle * (-1) - fieldToTarget.rotation()) @staticmethod - def estimateFieldToCamera(cameraToTarget : Transform2d, fieldToTarget : Pose2d): + def estimateFieldToCamera(cameraToTarget: Transform2d, + fieldToTarget: Pose2d): targetToCamera = cameraToTarget.inverse() return fieldToTarget.transformBy(targetToCamera) @staticmethod - def estimateFieldToRobot(cameraToTarget : Transform2d, fieldToTarget : Pose2d, cameraToRobot : Transform2d): + def estimateFieldToRobot(cameraToTarget: Transform2d, + fieldToTarget: Pose2d, + cameraToRobot: Transform2d): return PhotonUtils.estimateFieldToCamera(cameraToTarget, fieldToTarget).transformBy(cameraToRobot) @staticmethod - def estimateFieldToRobotAprilTag(cameraToTarget : Transform3d, fieldRelativeTagPose : Pose3d, cameraToRobot : Transform3d): + def estimateFieldToRobotAprilTag(cameraToTarget: Transform3d, + fieldRelativeTagPose: Pose3d, + cameraToRobot: Transform3d): return fieldRelativeTagPose + cameraToTarget.inverse() + cameraToRobot @staticmethod - def getYawToPose(robotPose : Pose2d, targetPose : Pose2d): - relativeTr1 = targetPose.rgitelativeTo(robotPose).translation() + def getYawToPose(robotPose: Pose2d, + targetPose: Pose2d): + relativeTr1 = targetPose.relativeTo(robotPose).translation() return Rotation2d(relativeTr1.X(), relativeTr1.Y()) @staticmethod - def getDistanceToPose(robotPose : Pose2d, targetPose : Pose2d): + 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): + 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) \ No newline at end of file From 73a2189d7730dcb0e8ec34380fb2eec6735852b4 Mon Sep 17 00:00:00 2001 From: ConnorLong Date: Fri, 31 Jan 2025 18:26:02 -0700 Subject: [PATCH 11/12] formatted utils to work with linter --- photon-lib/py/photonlibpy/photonUtils.py | 27 ++++++++++++++++-------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/photon-lib/py/photonlibpy/photonUtils.py b/photon-lib/py/photonlibpy/photonUtils.py index 1d69b89c4f..405232a5a6 100644 --- a/photon-lib/py/photonlibpy/photonUtils.py +++ b/photon-lib/py/photonlibpy/photonUtils.py @@ -10,47 +10,55 @@ def __init__(): def calculateDistanceToTargetMeters(cameraHeightMeters: float, targetHeightMeters: float, cameraPitchRadians: float, - targetPitchRadians: float) -> float: + targetPitchRadians: float + ) -> float: return (targetHeightMeters - cameraHeightMeters) / math.tan(cameraPitchRadians + targetPitchRadians) @staticmethod def estimateCameraToTargetTranslation(targetDistanceMeters: float, - yaw: Rotation2d): + yaw: Rotation2d + ): return Translation2d(yaw.cos() * targetDistanceMeters, yaw.sin() * targetDistanceMeters) @staticmethod def estimateCameraToTarget(cameraToTargetTranslation: Translation2d, fieldToTarget : Pose2d, - gyroAngle: Rotation2d): + gyroAngle: Rotation2d + ): return Transform2d(cameraToTargetTranslation, gyroAngle * (-1) - fieldToTarget.rotation()) @staticmethod def estimateFieldToCamera(cameraToTarget: Transform2d, - fieldToTarget: Pose2d): + fieldToTarget: Pose2d + ): targetToCamera = cameraToTarget.inverse() return fieldToTarget.transformBy(targetToCamera) @staticmethod def estimateFieldToRobot(cameraToTarget: Transform2d, fieldToTarget: Pose2d, - cameraToRobot: Transform2d): + cameraToRobot: Transform2d + ): return PhotonUtils.estimateFieldToCamera(cameraToTarget, fieldToTarget).transformBy(cameraToRobot) @staticmethod def estimateFieldToRobotAprilTag(cameraToTarget: Transform3d, fieldRelativeTagPose: Pose3d, - cameraToRobot: Transform3d): + cameraToRobot: Transform3d + ): return fieldRelativeTagPose + cameraToTarget.inverse() + cameraToRobot @staticmethod def getYawToPose(robotPose: Pose2d, - targetPose: Pose2d): + targetPose: Pose2d + ): relativeTr1 = targetPose.relativeTo(robotPose).translation() return Rotation2d(relativeTr1.X(), relativeTr1.Y()) @staticmethod def getDistanceToPose(robotPose: Pose2d, - targetPose: Pose2d): + targetPose: Pose2d + ): return robotPose.translation().distance(targetPose.translation()) @staticmethod @@ -61,5 +69,6 @@ def estimateFieldToRobot(cameraHeightMeters: float, targetYaw: Rotation2d, gyroAngle: Rotation2d, fieldToTarget: Pose2d, - cameraToRobot: Transform2d): + cameraToRobot: Transform2d + ): return PhotonUtils.estimateFieldToRobot(PhotonUtils.estimateCameraToTarget(PhotonUtils.estimateCameraToTargetTranslation(PhotonUtils.calculateDistanceToTargetMeters(cameraHeightMeters, targetHeightMeters, cameraPitchRadians, targetPitchRadians), targetYaw), fieldToTarget, gyroAngle), fieldToTarget, cameraToRobot) \ No newline at end of file From d9f96c661f05f266f70aa58d1ae1cf3e7a761845 Mon Sep 17 00:00:00 2001 From: ConnorLong Date: Fri, 31 Jan 2025 18:38:33 -0700 Subject: [PATCH 12/12] linter issues fixed --- photon-lib/py/photonlibpy/photonUtils.py | 119 +++++++++++++-------- photon-lib/py/test/visionSystemSim_test.py | 2 +- 2 files changed, 74 insertions(+), 47 deletions(-) diff --git a/photon-lib/py/photonlibpy/photonUtils.py b/photon-lib/py/photonlibpy/photonUtils.py index 405232a5a6..e1ef687fdf 100644 --- a/photon-lib/py/photonlibpy/photonUtils.py +++ b/photon-lib/py/photonlibpy/photonUtils.py @@ -1,74 +1,101 @@ -import photonlibpy -from wpimath.geometry import Translation2d, Rotation2d, Pose2d, Transform2d, Pose3d, Transform3d import math +from wpimath.geometry import ( + Pose2d, + Pose3d, + Rotation2d, + Transform2d, + Transform3d, + Translation2d, +) + + class PhotonUtils: def __init__(): pass - + @staticmethod - def calculateDistanceToTargetMeters(cameraHeightMeters: float, - targetHeightMeters: float, - cameraPitchRadians: float, - targetPitchRadians: float + def calculateDistanceToTargetMeters( + cameraHeightMeters: float, + targetHeightMeters: float, + cameraPitchRadians: float, + targetPitchRadians: float, ) -> float: - return (targetHeightMeters - cameraHeightMeters) / math.tan(cameraPitchRadians + targetPitchRadians) - + return (targetHeightMeters - cameraHeightMeters) / math.tan( + cameraPitchRadians + targetPitchRadians + ) + @staticmethod - def estimateCameraToTargetTranslation(targetDistanceMeters: float, - yaw: Rotation2d - ): - return Translation2d(yaw.cos() * targetDistanceMeters, yaw.sin() * targetDistanceMeters) - + def estimateCameraToTargetTranslation(targetDistanceMeters: float, yaw: Rotation2d): + return Translation2d( + yaw.cos() * targetDistanceMeters, yaw.sin() * targetDistanceMeters + ) + @staticmethod - def estimateCameraToTarget(cameraToTargetTranslation: Translation2d, - fieldToTarget : Pose2d, - gyroAngle: Rotation2d + def estimateCameraToTarget( + cameraToTargetTranslation: Translation2d, + fieldToTarget: Pose2d, + gyroAngle: Rotation2d, ): - return Transform2d(cameraToTargetTranslation, gyroAngle * (-1) - fieldToTarget.rotation()) - + return Transform2d( + cameraToTargetTranslation, gyroAngle * (-1) - fieldToTarget.rotation() + ) + @staticmethod - def estimateFieldToCamera(cameraToTarget: Transform2d, - fieldToTarget: Pose2d - ): + def estimateFieldToCamera(cameraToTarget: Transform2d, fieldToTarget: Pose2d): targetToCamera = cameraToTarget.inverse() return fieldToTarget.transformBy(targetToCamera) @staticmethod - def estimateFieldToRobot(cameraToTarget: Transform2d, - fieldToTarget: Pose2d, - cameraToRobot: Transform2d + def estimateFieldToRobot( + cameraToTarget: Transform2d, fieldToTarget: Pose2d, cameraToRobot: Transform2d ): - return PhotonUtils.estimateFieldToCamera(cameraToTarget, fieldToTarget).transformBy(cameraToRobot) - + return PhotonUtils.estimateFieldToCamera( + cameraToTarget, fieldToTarget + ).transformBy(cameraToRobot) + @staticmethod - def estimateFieldToRobotAprilTag(cameraToTarget: Transform3d, - fieldRelativeTagPose: Pose3d, - cameraToRobot: Transform3d + def estimateFieldToRobotAprilTag( + cameraToTarget: Transform3d, + fieldRelativeTagPose: Pose3d, + cameraToRobot: Transform3d, ): return fieldRelativeTagPose + cameraToTarget.inverse() + cameraToRobot @staticmethod - def getYawToPose(robotPose: Pose2d, - targetPose: Pose2d - ): + def getYawToPose(robotPose: Pose2d, targetPose: Pose2d): relativeTr1 = targetPose.relativeTo(robotPose).translation() return Rotation2d(relativeTr1.X(), relativeTr1.Y()) - + @staticmethod - def getDistanceToPose(robotPose: Pose2d, - targetPose: Pose2d - ): + 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 + 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) \ No newline at end of file + return PhotonUtils.estimateFieldToRobot( + PhotonUtils.estimateCameraToTarget( + PhotonUtils.estimateCameraToTargetTranslation( + PhotonUtils.calculateDistanceToTargetMeters( + cameraHeightMeters, + targetHeightMeters, + cameraPitchRadians, + targetPitchRadians, + ), + targetYaw, + ), + fieldToTarget, + gyroAngle, + ), + fieldToTarget, + cameraToRobot, + ) diff --git a/photon-lib/py/test/visionSystemSim_test.py b/photon-lib/py/test/visionSystemSim_test.py index 085fcdc769..5f365765cc 100644 --- a/photon-lib/py/test/visionSystemSim_test.py +++ b/photon-lib/py/test/visionSystemSim_test.py @@ -335,7 +335,7 @@ def test_distanceCalc(distParam, pitchParam, heightParam) -> None: assert target.getYaw() == pytest.approx(0.0, abs=0.5) dist = PhotonUtils.calculateDistanceToTargetMeters( - robotToCamera.Z(), targetPose.Z(), -pitchParam, math.degrees(target.getPitch()) + robotToCamera.Z(), targetPose.Z(), -pitchParam, math.degrees(target.getPitch()) ) assert dist == pytest.approx(distParam, abs=0.25)