From 1d8d934a8a597a8ba66fbd76552e9619a69d8a19 Mon Sep 17 00:00:00 2001 From: Lucien Morey Date: Sat, 9 Nov 2024 11:08:57 +1100 Subject: [PATCH] Enable Python tests, standardise variable spelling and fix arg checking (#1533) I found these with a quick find-and-replace and checked against the inbuilt Python type checking. I am away from my robot and can't really confirm there are no flow-on effects. There are no other active usages of the bad casing in the Python code, so we should be good. The generated serde messages already use this casing, so we don't need to update there. --- .../py/photonlibpy/photonPoseEstimator.py | 4 +- .../targeting/multiTargetPNPResult.py | 4 +- .../targeting/photonPipelineResult.py | 2 +- .../py/test/photonPoseEstimator_test.py | 445 +++++++++--------- 4 files changed, 234 insertions(+), 221 deletions(-) diff --git a/photon-lib/py/photonlibpy/photonPoseEstimator.py b/photon-lib/py/photonlibpy/photonPoseEstimator.py index 419c64102..a5a99f5cd 100644 --- a/photon-lib/py/photonlibpy/photonPoseEstimator.py +++ b/photon-lib/py/photonlibpy/photonPoseEstimator.py @@ -269,8 +269,8 @@ class PhotonPoseEstimator: def _multiTagOnCoprocStrategy( self, result: PhotonPipelineResult ) -> Optional[EstimatedRobotPose]: - if result.multiTagResult.estimatedPose.isPresent: - best_tf = result.multiTagResult.estimatedPose.best + if result.multitagResult is not None: + best_tf = result.multitagResult.estimatedPose.best best = ( Pose3d() .transformBy(best_tf) # field-to-camera diff --git a/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py b/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py index 6eb62d455..93e1fe2a8 100644 --- a/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py +++ b/photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py @@ -8,8 +8,8 @@ class PnpResult: best: Transform3d = field(default_factory=Transform3d) alt: Transform3d = field(default_factory=Transform3d) ambiguity: float = 0.0 - bestReprojError: float = 0.0 - altReprojError: float = 0.0 + bestReprojErr: float = 0.0 + altReprojErr: float = 0.0 photonStruct: "PNPResultSerde" = None diff --git a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py b/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py index 4160750f5..07a175abb 100644 --- a/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py +++ b/photon-lib/py/photonlibpy/targeting/photonPipelineResult.py @@ -29,7 +29,7 @@ class PhotonPipelineResult: # Python users beware! We don't currently run a Time Sync Server, so these timestamps are in # an arbitrary timebase. This is not true in C++ or Java. metadata: PhotonPipelineMetadata = field(default_factory=PhotonPipelineMetadata) - multiTagResult: Optional[MultiTargetPNPResult] = None + multitagResult: Optional[MultiTargetPNPResult] = None def getLatencyMillis(self) -> float: return ( diff --git a/photon-lib/py/test/photonPoseEstimator_test.py b/photon-lib/py/test/photonPoseEstimator_test.py index e761f5bb9..02be614ae 100644 --- a/photon-lib/py/test/photonPoseEstimator_test.py +++ b/photon-lib/py/test/photonPoseEstimator_test.py @@ -15,247 +15,260 @@ ## along with this program. If not, see . ############################################################################### -# from photonlibpy import MultiTargetPNPResult, PnpResult -# from photonlibpy import PhotonPipelineResult -# from photonlibpy import PhotonPoseEstimator, PoseStrategy -# from photonlibpy import PhotonTrackedTarget, TargetCorner, PhotonPipelineMetadata -# from robotpy_apriltag import AprilTag, AprilTagFieldLayout -# from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d +from photonlibpy.targeting.multiTargetPNPResult import MultiTargetPNPResult, PnpResult +from photonlibpy.targeting.photonPipelineResult import PhotonPipelineResult +from photonlibpy import PhotonPoseEstimator, PoseStrategy +from photonlibpy.targeting import ( + PhotonTrackedTarget, + TargetCorner, + PhotonPipelineMetadata, +) +from robotpy_apriltag import AprilTag, AprilTagFieldLayout +from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d -# class PhotonCameraInjector: -# result: PhotonPipelineResult +class PhotonCameraInjector: + result: PhotonPipelineResult -# def getLatestResult(self) -> PhotonPipelineResult: -# return self.result + def getLatestResult(self) -> PhotonPipelineResult: + return self.result -# def setupCommon() -> AprilTagFieldLayout: -# tagList = [] -# tagPoses = ( -# Pose3d(3, 3, 3, Rotation3d()), -# Pose3d(5, 5, 5, Rotation3d()), -# ) -# for id_, pose in enumerate(tagPoses): -# aprilTag = AprilTag() -# aprilTag.ID = id_ -# aprilTag.pose = pose -# tagList.append(aprilTag) +def setupCommon() -> AprilTagFieldLayout: + tagList = [] + tagPoses = ( + Pose3d(3, 3, 3, Rotation3d()), + Pose3d(5, 5, 5, Rotation3d()), + ) + for id_, pose in enumerate(tagPoses): + aprilTag = AprilTag() + aprilTag.ID = id_ + aprilTag.pose = pose + tagList.append(aprilTag) -# fieldLength = 54 / 3.281 # 54 ft -> meters -# fieldWidth = 27 / 3.281 # 24 ft -> meters + fieldLength = 54 / 3.281 # 54 ft -> meters + fieldWidth = 27 / 3.281 # 24 ft -> meters -# return AprilTagFieldLayout(tagList, fieldLength, fieldWidth) + return AprilTagFieldLayout(tagList, fieldLength, fieldWidth) -# def test_lowestAmbiguityStrategy(): -# aprilTags = setupCommon() +def test_lowestAmbiguityStrategy(): + aprilTags = setupCommon() -# cameraOne = PhotonCameraInjector() -# cameraOne.result = PhotonPipelineResult( -# 11 * 1e6, -# [ -# PhotonTrackedTarget( -# 3.0, -# -4.0, -# 9.0, -# 4.0, -# 0, -# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), -# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), -# [ -# TargetCorner(1, 2), -# TargetCorner(3, 4), -# TargetCorner(5, 6), -# TargetCorner(7, 8), -# ], -# [ -# TargetCorner(1, 2), -# TargetCorner(3, 4), -# TargetCorner(5, 6), -# TargetCorner(7, 8), -# ], -# 0.7, -# ), -# PhotonTrackedTarget( -# 3.0, -# -4.0, -# 9.1, -# 6.7, -# 1, -# Transform3d(Translation3d(4, 2, 3), Rotation3d(0, 0, 0)), -# Transform3d(Translation3d(4, 2, 3), Rotation3d(1, 5, 3)), -# [ -# TargetCorner(1, 2), -# TargetCorner(3, 4), -# TargetCorner(5, 6), -# TargetCorner(7, 8), -# ], -# [ -# TargetCorner(1, 2), -# TargetCorner(3, 4), -# TargetCorner(5, 6), -# TargetCorner(7, 8), -# ], -# 0.3, -# ), -# PhotonTrackedTarget( -# 9.0, -# -2.0, -# 19.0, -# 3.0, -# 0, -# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), -# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), -# [ -# TargetCorner(1, 2), -# TargetCorner(3, 4), -# TargetCorner(5, 6), -# TargetCorner(7, 8), -# ], -# [ -# TargetCorner(1, 2), -# TargetCorner(3, 4), -# TargetCorner(5, 6), -# TargetCorner(7, 8), -# ], -# 0.4, -# ), -# ], -# None, -# metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0), -# ) + cameraOne = PhotonCameraInjector() + cameraOne.result = PhotonPipelineResult( + int(11 * 1e6), + [ + PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 0, + Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), + Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), + [ + TargetCorner(1, 2), + TargetCorner(3, 4), + TargetCorner(5, 6), + TargetCorner(7, 8), + ], + [ + TargetCorner(1, 2), + TargetCorner(3, 4), + TargetCorner(5, 6), + TargetCorner(7, 8), + ], + 0.7, + ), + PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 1, + Transform3d(Translation3d(4, 2, 3), Rotation3d(0, 0, 0)), + Transform3d(Translation3d(4, 2, 3), Rotation3d(1, 5, 3)), + [ + TargetCorner(1, 2), + TargetCorner(3, 4), + TargetCorner(5, 6), + TargetCorner(7, 8), + ], + [ + TargetCorner(1, 2), + TargetCorner(3, 4), + TargetCorner(5, 6), + TargetCorner(7, 8), + ], + 0.3, + ), + PhotonTrackedTarget( + 9.0, + -2.0, + 19.0, + 3.0, + 0, + Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), + Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), + [ + TargetCorner(1, 2), + TargetCorner(3, 4), + TargetCorner(5, 6), + TargetCorner(7, 8), + ], + [ + TargetCorner(1, 2), + TargetCorner(3, 4), + TargetCorner(5, 6), + TargetCorner(7, 8), + ], + 0.4, + ), + ], + metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0), + multitagResult=None, + ) -# estimator = PhotonPoseEstimator( -# aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d() -# ) + estimator = PhotonPoseEstimator( + aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d() + ) -# estimatedPose = estimator.update() -# pose = estimatedPose.estimatedPose + estimatedPose = estimator.update() -# assertEquals(11 - 0.002, estimatedPose.timestampSeconds, 1e-3) -# assertEquals(1, pose.x, 0.01) -# assertEquals(3, pose.y, 0.01) -# assertEquals(2, pose.z, 0.01) + assert estimatedPose is not None + + pose = estimatedPose.estimatedPose + + assertEquals(11 - 0.002, estimatedPose.timestampSeconds, 1e-3) + assertEquals(1, pose.x, 0.01) + assertEquals(3, pose.y, 0.01) + assertEquals(2, pose.z, 0.01) -# def test_multiTagOnCoprocStrategy(): -# cameraOne = PhotonCameraInjector() -# cameraOne.result = PhotonPipelineResult( -# 11 * 1e6, -# # There needs to be at least one target present for pose estimation to work -# # Doesn't matter which/how many targets for this test -# [ -# PhotonTrackedTarget( -# 3.0, -# -4.0, -# 9.0, -# 4.0, -# 0, -# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), -# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), -# [ -# TargetCorner(1, 2), -# TargetCorner(3, 4), -# TargetCorner(5, 6), -# TargetCorner(7, 8), -# ], -# [ -# TargetCorner(1, 2), -# TargetCorner(3, 4), -# TargetCorner(5, 6), -# TargetCorner(7, 8), -# ], -# 0.7, -# ) -# ], -# multiTagResult=MultiTargetPNPResult( -# PnpResult(True, Transform3d(1, 3, 2, Rotation3d())) -# ), -# metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0), -# ) +def test_multiTagOnCoprocStrategy(): + cameraOne = PhotonCameraInjector() + cameraOne.result = PhotonPipelineResult( + int(11 * 1e6), + # There needs to be at least one target present for pose estimation to work + # Doesn't matter which/how many targets for this test + [ + PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 0, + Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), + Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), + [ + TargetCorner(1, 2), + TargetCorner(3, 4), + TargetCorner(5, 6), + TargetCorner(7, 8), + ], + [ + TargetCorner(1, 2), + TargetCorner(3, 4), + TargetCorner(5, 6), + TargetCorner(7, 8), + ], + 0.7, + ) + ], + metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0), + multitagResult=MultiTargetPNPResult( + PnpResult(Transform3d(1, 3, 2, Rotation3d())) + ), + ) -# estimator = PhotonPoseEstimator( -# AprilTagFieldLayout(), -# PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, -# cameraOne, -# Transform3d(), -# ) + estimator = PhotonPoseEstimator( + AprilTagFieldLayout(), + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + cameraOne, + Transform3d(), + ) -# estimatedPose = estimator.update() -# pose = estimatedPose.estimatedPose + estimatedPose = estimator.update() -# assertEquals(11 - 2e-3, estimatedPose.timestampSeconds, 1e-3) -# assertEquals(1, pose.x, 0.01) -# assertEquals(3, pose.y, 0.01) -# assertEquals(2, pose.z, 0.01) + assert estimatedPose is not None + + pose = estimatedPose.estimatedPose + + assertEquals(11 - 2e-3, estimatedPose.timestampSeconds, 1e-3) + assertEquals(1, pose.x, 0.01) + assertEquals(3, pose.y, 0.01) + assertEquals(2, pose.z, 0.01) -# def test_cacheIsInvalidated(): -# aprilTags = setupCommon() +def test_cacheIsInvalidated(): + aprilTags = setupCommon() -# cameraOne = PhotonCameraInjector() -# result = PhotonPipelineResult( -# 20 * 1e6, -# [ -# PhotonTrackedTarget( -# 3.0, -# -4.0, -# 9.0, -# 4.0, -# 0, -# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), -# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), -# [ -# TargetCorner(1, 2), -# TargetCorner(3, 4), -# TargetCorner(5, 6), -# TargetCorner(7, 8), -# ], -# [ -# TargetCorner(1, 2), -# TargetCorner(3, 4), -# TargetCorner(5, 6), -# TargetCorner(7, 8), -# ], -# 0.7, -# ) -# ], -# metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0), -# ) + cameraOne = PhotonCameraInjector() + result = PhotonPipelineResult( + int(20 * 1e6), + [ + PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 0, + Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), + Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)), + [ + TargetCorner(1, 2), + TargetCorner(3, 4), + TargetCorner(5, 6), + TargetCorner(7, 8), + ], + [ + TargetCorner(1, 2), + TargetCorner(3, 4), + TargetCorner(5, 6), + TargetCorner(7, 8), + ], + 0.7, + ) + ], + metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0), + ) -# estimator = PhotonPoseEstimator( -# aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d() -# ) + estimator = PhotonPoseEstimator( + aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d() + ) -# # Empty result, expect empty result -# cameraOne.result = PhotonPipelineResult(0) -# estimatedPose = estimator.update() -# assert estimatedPose is None + # Empty result, expect empty result + cameraOne.result = PhotonPipelineResult(0) + estimatedPose = estimator.update() + assert estimatedPose is None -# # Set actual result -# cameraOne.result = result -# estimatedPose = estimator.update() -# assert estimatedPose is not None -# assertEquals(20, estimatedPose.timestampSeconds, 0.01) -# assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3) + # Set actual result + cameraOne.result = result + estimatedPose = estimator.update() + assert estimatedPose is not None + assertEquals(20, estimatedPose.timestampSeconds, 0.01) + assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3) -# # And again -- pose cache should mean this is empty -# cameraOne.result = result -# estimatedPose = estimator.update() -# assert estimatedPose is None -# # Expect the old timestamp to still be here -# assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3) + # And again -- pose cache should mean this is empty + cameraOne.result = result + estimatedPose = estimator.update() + assert estimatedPose is None + # Expect the old timestamp to still be here + assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3) -# # Set new field layout -- right after, the pose cache timestamp should be -1 -# estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0) -# assertEquals(-1, estimator._poseCacheTimestampSeconds) -# # Update should cache the current timestamp (20) again -# cameraOne.result = result -# estimatedPose = estimator.update() -# assertEquals(20, estimatedPose.timestampSeconds, 0.01) -# assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3) + # Set new field layout -- right after, the pose cache timestamp should be -1 + estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0) + assertEquals(-1, estimator._poseCacheTimestampSeconds) + # Update should cache the current timestamp (20) again + cameraOne.result = result + estimatedPose = estimator.update() + + assert estimatedPose is not None + + assertEquals(20, estimatedPose.timestampSeconds, 0.01) + assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3) -# def assertEquals(expected, actual, epsilon=0.0): -# assert abs(expected - actual) <= epsilon +def assertEquals(expected, actual, epsilon=0.0): + assert abs(expected - actual) <= epsilon