PoseEst sample code added. Streamlined other examples.

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

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

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,14 +125,31 @@ 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(),
                 self.frontRight.getPosition(),
                 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):

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()