diff --git a/docs/source/docs/examples/aimandrange.md b/docs/source/docs/examples/aimandrange.md index a3fec9d7f..0ada58659 100644 --- a/docs/source/docs/examples/aimandrange.md +++ b/docs/source/docs/examples/aimandrange.md @@ -17,7 +17,7 @@ To do this, we'll use the *pitch* of the target in the camera image and trigonom .. tab-item:: Java - .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/d6c37ead118ae58d224c0c4fd7f7a2b7931940b5/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java :language: java :lines: 84-131 :linenos: @@ -25,14 +25,26 @@ To do this, we'll use the *pitch* of the target in the camera image and trigonom .. tab-item:: C++ (Header) - TODO + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-cpp-examples/aimandrange/src/main/inclde/Robot.h + :language: c++ + :lines: 25-64 + :linenos: + :lineno-start: 25 .. tab-item:: C++ (Source) - TODO + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp + :language: c++ + :lines: 58-107 + :linenos: + :lineno-start: 58 .. tab-item:: Python - TODO + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-python-examples/aimandrange/robot.py + :language: python + :lines: 44-103 + :linenos: + :lineno-start: 44 ``` diff --git a/docs/source/docs/examples/aimingatatarget.md b/docs/source/docs/examples/aimingatatarget.md index 9e43e8d6b..e394b522d 100644 --- a/docs/source/docs/examples/aimingatatarget.md +++ b/docs/source/docs/examples/aimingatatarget.md @@ -21,22 +21,34 @@ In this example, while the operator holds a button down, the robot will turn tow .. tab-item:: Java - .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/d6c37ead118ae58d224c0c4fd7f7a2b7931940b5/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java :language: java - :lines: 79-114 + :lines: 77-117 :linenos: - :lineno-start: 79 + :lineno-start: 77 .. tab-item:: C++ (Header) - TODO + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-cpp-examples/aimattarget/src/main/inclde/Robot.h + :language: c++ + :lines: 25-61 + :linenos: + :lineno-start: 25 .. tab-item:: C++ (Source) - TODO + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp + :language: c++ + :lines: 56-96 + :linenos: + :lineno-start: 56 .. tab-item:: Python - TODO + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-python-examples/aimattarget/robot.py + :language: python + :lines: 46-70 + :linenos: + :lineno-start: 46 ``` diff --git a/docs/source/docs/examples/poseest.md b/docs/source/docs/examples/poseest.md index c2ffda316..96c072fe9 100644 --- a/docs/source/docs/examples/poseest.md +++ b/docs/source/docs/examples/poseest.md @@ -26,14 +26,32 @@ We use the 2024 game's AprilTag Locations: .. tab-item:: Java :sync: java - .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/d6c37ead118ae58d224c0c4fd7f7a2b7931940b5/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java :language: java :lines: 68-68 :linenos: :lineno-start: 68 + .. tab-item:: C++ + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-cpp-examples/poseest/src/main/include/Constants.h + :language: c++ + :lines: 42-43 + :linenos: + :lineno-start: 42 + + .. tab-item:: Python + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-python-examples/poseest/robot.py + :language: python + :lines: 46-46 + :linenos: + :lineno-start: 46 + ``` + + To incorporate PhotonVision, we need to create a {code}`PhotonCamera`: @@ -43,12 +61,27 @@ To incorporate PhotonVision, we need to create a {code}`PhotonCamera`: .. tab-item:: Java :sync: java - .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/d6c37ead118ae58d224c0c4fd7f7a2b7931940b5/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java :language: java :lines: 57-57 :linenos: :lineno-start: 57 + .. tab-item:: C++ + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-cpp-examples/poseest/src/main/include/Constants.h + :language: c++ + :lines: 145-145 + :linenos: + :lineno-start: 145 + + .. tab-item:: Python + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-python-examples/poseest/robot.py + :language: python + :lines: 44-44 + :linenos: + :lineno-start: 44 ``` During periodic execution, we read back camera results. If we see AprilTags in the image, we calculate the camera-measured pose of the robot and pass it to the {code}`Drivetrain`. @@ -59,12 +92,28 @@ During periodic execution, we read back camera results. If we see AprilTags in t .. tab-item:: Java :sync: java - .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/d6c37ead118ae58d224c0c4fd7f7a2b7931940b5/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java :language: java :lines: 64-74 :linenos: :lineno-start: 64 + .. tab-item:: C++ + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-cpp-examples/poseest/src/main/src/Robot.cpp + :language: c++ + :lines: 38-46 + :linenos: + :lineno-start: 38 + + .. tab-item:: Python + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-python-examples/poseest/robot.py + :language: python + :lines: 54-56 + :linenos: + :lineno-start: 54 + ``` ### Simulating the Camera @@ -77,12 +126,24 @@ First, we create a new {code}`VisionSystemSim` to represent our camera and copro .. tab-item:: Java :sync: java - .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/d6c37ead118ae58d224c0c4fd7f7a2b7931940b5/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java :language: java :lines: 65-69 :linenos: :lineno-start: 65 + .. tab-item:: C++ + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-cpp-examples/poseest/src/main/include/Vision.h + :language: c++ + :lines: 49-52 + :linenos: + :lineno-start: 49 + + .. tab-item:: Python + + # TODO + ``` Then, we add configure the simulated vision system to match the camera system being simulated. @@ -93,12 +154,23 @@ Then, we add configure the simulated vision system to match the camera system be .. tab-item:: Java :sync: java - .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/d6c37ead118ae58d224c0c4fd7f7a2b7931940b5/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java :language: java :lines: 69-82 :linenos: :lineno-start: 69 + .. tab-item:: C++ + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-cpp-examples/poseest/src/main/include/Vision.h + :language: c++ + :lines: 53-65 + :linenos: + :lineno-start: 53 + + .. tab-item:: Python + + # TODO ``` @@ -112,12 +184,23 @@ During simulation, we periodically update the simulated vision system. .. tab-item:: Java :sync: java - .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/d6c37ead118ae58d224c0c4fd7f7a2b7931940b5/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java :language: java :lines: 114-132 :linenos: :lineno-start: 114 + .. tab-item:: C++ + + .. rli:: https://raw.githubusercontent.com/gerth2/photonvision/56513cef973b3b7f8de5b354467f88dd9fbaea71/photonlib-cpp-examples/poseest/src/main/src/Robot.cpp + :language: c++ + :lines: 95-109 + :linenos: + :lineno-start: 95 + + .. tab-item:: Python + + # TODO ``` The rest is done behind the scenes. diff --git a/docs/source/docs/programming/photonlib/robot-pose-estimator.md b/docs/source/docs/programming/photonlib/robot-pose-estimator.md index b878be271..e15ef6686 100644 --- a/docs/source/docs/programming/photonlib/robot-pose-estimator.md +++ b/docs/source/docs/programming/photonlib/robot-pose-estimator.md @@ -83,7 +83,19 @@ The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (s .. code-block:: Python - # TODO + kRobotToCam = wpimath.geometry.Transform3d( + wpimath.geometry.Translation3d(0.5, 0.0, 0.5), + wpimath.geometry.Rotation3d.fromDegrees(0.0, -30.0, 0.0), + ) + + self.cam = PhotonCamera("YOUR CAMERA NAME") + + self.camPoseEst = PhotonPoseEstimator( + loadAprilTagLayoutField(AprilTagField.k2024Crescendo), + PoseStrategy.CLOSEST_TO_REFERENCE_POSE, + self.cam, + kRobotToCam + ) ``` ## Using a `PhotonPoseEstimator` @@ -113,7 +125,8 @@ Calling `update()` on your `PhotonPoseEstimator` will return an `EstimatedRobotP .. code-block:: Python - # TODO + + ``` You should be updating your [drivetrain pose estimator](https://docs.wpilib.org/en/latest/docs/software/advanced-controls/state-space/state-space-pose-estimators.html) with the result from the `RobotPoseEstimator` every loop using `addVisionMeasurement()`. TODO: add example note diff --git a/photonlib-python-examples/poseest/robot.py b/photonlib-python-examples/poseest/robot.py index 5c00ff7aa..5396c93ab 100644 --- a/photonlib-python-examples/poseest/robot.py +++ b/photonlib-python-examples/poseest/robot.py @@ -31,11 +31,12 @@ from photonlibpy import PhotonCamera, PhotonPoseEstimator, PoseStrategy -ROBOT_TO_CAM = wpimath.geometry.Transform3d( +kRobotToCam = 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""" @@ -46,14 +47,15 @@ def robotInit(self) -> None: loadAprilTagLayoutField(AprilTagField.k2024Crescendo), PoseStrategy.LOWEST_AMBIGUITY, self.cam, - ROBOT_TO_CAM + kRobotToCam, ) def robotPeriodic(self) -> None: - camEstPose = self.camPoseEst.update() - if(camEstPose): - self.swerve.addVisionPoseEstimate(camEstPose.estimatedPose, camEstPose.timestampSeconds) + if camEstPose: + self.swerve.addVisionPoseEstimate( + camEstPose.estimatedPose, camEstPose.timestampSeconds + ) self.swerve.updateOdometry() self.swerve.log()