############################################################################### ## Copyright (C) Photon Vision. ############################################################################### ## This program is free software: you can redistribute it and/or modify ## it under the terms of the GNU General Public License as published by ## the Free Software Foundation, either version 3 of the License, or ## (at your option) any later version. ## ## This program is distributed in the hope that it will be useful, ## but WITHOUT ANY WARRANTY; without even the implied warranty of ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ## GNU General Public License for more details. ## ## You should have received a copy of the GNU General Public License ## 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 # class PhotonCameraInjector: # result: PhotonPipelineResult # 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) # fieldLength = 54 / 3.281 # 54 ft -> meters # fieldWidth = 27 / 3.281 # 24 ft -> meters # return AprilTagFieldLayout(tagList, fieldLength, fieldWidth) # 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), # ) # estimator = PhotonPoseEstimator( # aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d() # ) # estimatedPose = estimator.update() # 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), # ) # estimator = PhotonPoseEstimator( # AprilTagFieldLayout(), # PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, # cameraOne, # Transform3d(), # ) # estimatedPose = estimator.update() # 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() # 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), # ) # estimator = PhotonPoseEstimator( # aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d() # ) # # 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) # # 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) # def assertEquals(expected, actual, epsilon=0.0): # assert abs(expected - actual) <= epsilon