diff --git a/photonlib-python-examples/aimandrange/robot.py b/photonlib-python-examples/aimandrange/robot.py index a175e4b4f..658211a79 100644 --- a/photonlib-python-examples/aimandrange/robot.py +++ b/photonlib-python-examples/aimandrange/robot.py @@ -64,20 +64,19 @@ def teleopPeriodic(self) -> None: results = self.cam.getAllUnreadResults() if len(results) > 0: result = results[-1] # take the most recent result the camera had - if result.hasTargets(): - # At least one apriltag was seen by the camera - for target in result.getTargets(): - if target.getFiducialId() == 7: - # Found tag 7, record its information - targetVisible = True - targetYaw = target.getYaw() - heightDelta = ( - ROBOT_TO_CAM.translation().Z() - 1.435 - ) # From 2024 game manual for ID 7 - angleDelta = -1.0 * ROBOT_TO_CAM.rotation().Y() - math.radians( - target.getPitch() - ) - targetRange = heightDelta / math.tan(angleDelta) + # At least one apriltag was seen by the camera + for target in result.getTargets(): + if target.getFiducialId() == 7: + # Found tag 7, record its information + targetVisible = True + targetYaw = target.getYaw() + heightDelta = ( + ROBOT_TO_CAM.translation().Z() - 1.435 + ) # From 2024 game manual for ID 7 + angleDelta = -1.0 * ROBOT_TO_CAM.rotation().Y() - math.radians( + target.getPitch() + ) + targetRange = heightDelta / math.tan(angleDelta) if self.controller.getAButton() and targetVisible: # Driver wants auto-alignment to tag 7 diff --git a/photonlib-python-examples/aimattarget/robot.py b/photonlib-python-examples/aimattarget/robot.py index 5266928c2..3f66961c1 100644 --- a/photonlib-python-examples/aimattarget/robot.py +++ b/photonlib-python-examples/aimattarget/robot.py @@ -54,13 +54,11 @@ def teleopPeriodic(self) -> None: results = self.cam.getAllUnreadResults() if len(results) > 0: result = results[-1] # take the most recent result the camera had - if result.hasTargets(): - # At least one apriltag was seen by the camera - for target in result.getTargets(): - if target.getFiducialId() == 7: - # Found tag 7, record its information - targetVisible = True - targetYaw = target.getYaw() + for target in result.getTargets(): + if target.getFiducialId() == 7: + # Found tag 7, record its information + targetVisible = True + targetYaw = target.getYaw() if self.controller.getAButton() and targetVisible: # Driver wants auto-alignment to tag 7 diff --git a/photonlib-python-examples/poseest/drivetrain.py b/photonlib-python-examples/poseest/drivetrain.py index d411b7b8b..4269061cc 100644 --- a/photonlib-python-examples/poseest/drivetrain.py +++ b/photonlib-python-examples/poseest/drivetrain.py @@ -28,11 +28,17 @@ import wpilib.simulation import wpimath.geometry import wpimath.kinematics +import wpimath.estimator import swervemodule kMaxSpeed = 3.0 # 3 meters per second kMaxAngularSpeed = math.pi # 1/2 rotation per second +kInitialPose = wpimath.geometry.Pose2d( + wpimath.geometry.Translation2d(1.0, 1.0), + wpimath.geometry.Rotation2d.fromDegrees(0.0), +) + class Drivetrain: """ @@ -63,7 +69,7 @@ def __init__(self) -> None: self.backRightLocation, ) - self.odometry = wpimath.kinematics.SwerveDrive4Odometry( + self.poseEst = wpimath.estimator.SwerveDrive4PoseEstimator( self.kinematics, self.gyro.getRotation2d(), ( @@ -72,6 +78,7 @@ def __init__(self) -> None: self.backLeft.getPosition(), self.backRight.getPosition(), ), + kInitialPose, ) self.targetChassisSpeeds = wpimath.kinematics.ChassisSpeeds() @@ -118,7 +125,23 @@ def drive( def updateOdometry(self) -> None: """Updates the field relative position of the robot.""" - self.odometry.update( + self.poseEst.update( + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) + + def addVisionPoseEstimate( + self, pose: wpimath.geometry.Pose3d, timestamp: float + ) -> None: + self.poseEst.addVisionMeasurement(pose, timestamp) + + def resetPose(self) -> None: + self.poseEst.resetPosition( self.gyro.getRotation2d(), ( self.frontLeft.getPosition(), @@ -126,6 +149,7 @@ def updateOdometry(self) -> None: self.backLeft.getPosition(), self.backRight.getPosition(), ), + kInitialPose, ) def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]: @@ -137,7 +161,7 @@ def getModuleStates(self) -> list[wpimath.kinematics.SwerveModuleState]: ] def getModulePoses(self) -> list[wpimath.geometry.Pose2d]: - p = self.odometry.getPose() + p = self.poseEst.getEstimatedPosition() flTrans = wpimath.geometry.Transform2d( self.frontLeftLocation, self.frontLeft.getAbsoluteHeading() ) @@ -163,7 +187,7 @@ def getChassisSpeeds(self) -> wpimath.kinematics.ChassisSpeeds: def log(self): table = "Drive/" - pose = self.odometry.getPose() + pose = self.poseEst.getEstimatedPosition() wpilib.SmartDashboard.putNumber(table + "X", pose.X()) wpilib.SmartDashboard.putNumber(table + "Y", pose.Y()) wpilib.SmartDashboard.putNumber(table + "Heading", pose.rotation().degrees()) @@ -190,7 +214,7 @@ def log(self): self.backLeft.log() self.backRight.log() - self.debugField.getRobotObject().setPose(self.odometry.getPose()) + self.debugField.getRobotObject().setPose(self.poseEst.getEstimatedPosition()) self.debugField.getObject("SwerveModules").setPoses(self.getModulePoses()) def simulationPeriodic(self): diff --git a/photonlib-python-examples/poseest/robot.py b/photonlib-python-examples/poseest/robot.py index bd476bde5..5c00ff7aa 100644 --- a/photonlib-python-examples/poseest/robot.py +++ b/photonlib-python-examples/poseest/robot.py @@ -25,16 +25,36 @@ import wpilib +import wpimath.geometry +from robotpy_apriltag import AprilTagField, loadAprilTagLayoutField import drivetrain +from photonlibpy import PhotonCamera, PhotonPoseEstimator, PoseStrategy + +ROBOT_TO_CAM = wpimath.geometry.Transform3d( + wpimath.geometry.Translation3d(0.5, 0.0, 0.5), + wpimath.geometry.Rotation3d.fromDegrees(0.0, -30.0, 0.0), +) class MyRobot(wpilib.TimedRobot): def robotInit(self) -> None: """Robot initialization function""" self.controller = wpilib.XboxController(0) self.swerve = drivetrain.Drivetrain() + self.cam = PhotonCamera("YOUR CAMERA NAME") + self.camPoseEst = PhotonPoseEstimator( + loadAprilTagLayoutField(AprilTagField.k2024Crescendo), + PoseStrategy.LOWEST_AMBIGUITY, + self.cam, + ROBOT_TO_CAM + ) def robotPeriodic(self) -> None: + + camEstPose = self.camPoseEst.update() + if(camEstPose): + self.swerve.addVisionPoseEstimate(camEstPose.estimatedPose, camEstPose.timestampSeconds) + self.swerve.updateOdometry() self.swerve.log()