diff --git a/docs/source/docs/programming/photonlib/robot-pose-estimator.md b/docs/source/docs/programming/photonlib/robot-pose-estimator.md index 3f5588b54d..b3c912d1f6 100644 --- a/docs/source/docs/programming/photonlib/robot-pose-estimator.md +++ b/docs/source/docs/programming/photonlib/robot-pose-estimator.md @@ -4,7 +4,7 @@ For more information on how to methods to get AprilTag data, look {ref}`here `. ::: -PhotonLib includes a `PhotonPoseEstimator` class, which allows you to combine the pose data from all tags in view in order to get a field relative pose. The `PhotonPoseEstimator` class works with one camera per object instance, but more than one instance may be created. +PhotonLib includes a `PhotonPoseEstimator` class, which allows you to combine the pose data from all tags in view in order to get a field relative pose. ## Creating an `AprilTagFieldLayout` @@ -14,25 +14,42 @@ The API documentation can be found in here: [Java](https://github.wpilib.org/all ```{eval-rst} .. tab-set-code:: - .. code-block:: Java + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java + :language: java + :lines: 48-49 - // The field from AprilTagFields will be different depending on the game. - AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Constants.h + :language: c++ + :lines: 46-47 - .. code-block:: C++ + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py + :language: python + :lines: 46 - // The parameter for LoadAPrilTagLayoutField will be different depending on the game. - frc::AprilTagFieldLayout aprilTagFieldLayout = frc::LoadAprilTagLayoutField(frc::AprilTagField::kDefaultField); +``` + +## Defining the Robot to Camera `Transform3d` + +Another necessary argument for creating a `PhotonPoseEstimator` is the `Transform3d` representing the robot-relative location and orientation of the camera. A `Transform3d` contains a `Translation3d` and a `Rotation3d`. The `Translation3d` is created in meters and the `Rotation3d` is created with degrees. - .. code-block:: Python +```{eval-rst} +.. tab-set-code:: + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Constants.java + :language: java + :lines: 44-45 - # Coming Soon! + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Constants.h + :language: c++ + :lines: 43-45 + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py + :language: python + :lines: 33-36 ``` ## Creating a `PhotonPoseEstimator` -The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (see above), `PoseStrategy`, `PhotonCamera`, and `Transform3d`. `PoseStrategy` has six possible values: +The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (see above), `PoseStrategy`, and `Transform3d`. `PoseStrategy` has six possible values: - MULTI_TAG_PNP_ON_COPROCESSOR - Calculates a new robot position estimate by combining all visible tag corners. Recommended for all teams as it will be the most accurate. @@ -50,88 +67,61 @@ The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (s ```{eval-rst} .. tab-set-code:: - .. code-block:: Java - - //Forward Camera - cam = new PhotonCamera("testCamera"); - Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0,0,0)); //Cam mounted facing forward, half a meter forward of center, half a meter up from center. + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java + :language: java + :lines: 59-60 - // Construct PhotonPoseEstimator - PhotonPoseEstimator photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, cam, robotToCam); + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Vision.h + :language: c++ + :lines: 141-144 - .. code-block:: C++ - - // Forward Camera - std::shared_ptr cameraOne = - std::make_shared("testCamera"); - // Camera is mounted facing forward, half a meter forward of center, half a - // meter up from center. - frc::Transform3d robotToCam = - frc::Transform3d(frc::Translation3d(0.5_m, 0_m, 0.5_m), - frc::Rotation3d(0_rad, 0_rad, 0_rad)); - - // ... Add other cameras here + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py + :language: python + :lines: 45-50 +``` - // Assemble the list of cameras & mount locations - std::vector< - std::pair, frc::Transform3d>> - cameras; - cameras.push_back(std::make_pair(cameraOne, robotToCam)); +:::{note} +Python still takes a `PhotonCamera` in the constructor, so you must create the camera as shown in the next section and then return and use it to create the `PhotonPoseEstimator`. +::: - photonlib::RobotPoseEstimator estimator( - aprilTags, photonlib::CLOSEST_TO_REFERENCE_POSE, cameras); +## Using a `PhotonPoseEstimator` - .. code-block:: Python +The final prerequisite to using your `PhotonPoseEstimator` is creating a `PhotonCamera`. To do this, you must set the name of your camera in Photon Client. From there you can define the camera in code. - kRobotToCam = wpimath.geometry.Transform3d( - wpimath.geometry.Translation3d(0.5, 0.0, 0.5), - wpimath.geometry.Rotation3d.fromDegrees(0.0, -30.0, 0.0), - ) +```{eval-rst} +.. tab-set-code:: + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java + :language: java + :lines: 57 - self.cam = PhotonCamera("YOUR CAMERA NAME") + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h + :language: c++ + :lines: 55 - self.camPoseEst = PhotonPoseEstimator( - loadAprilTagLayoutField(AprilTagField.kDefaultField), - PoseStrategy.CLOSEST_TO_REFERENCE_POSE, - self.cam, - kRobotToCam - ) + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py + :language: python + :lines: 44 ``` -## Using a `PhotonPoseEstimator` - Calling `update()` on your `PhotonPoseEstimator` will return an `EstimatedRobotPose`, which includes a `Pose3d` of the latest estimated pose (using the selected strategy) along with a `double` of the timestamp when the robot pose was estimated. 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 `PhotonPoseEstimator` every loop using `addVisionMeasurement()`. ```{eval-rst} .. tab-set-code:: - .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/357d8a518a93f7a1f8084a79449249e613b605a7/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java - :language: java - :lines: 85-88 - - .. code-block:: C++ - - std::pair getEstimatedGlobalPose( - frc::Pose3d prevEstimatedRobotPose) { - robotPoseEstimator.SetReferencePose(prevEstimatedRobotPose); - units::millisecond_t currentTime = frc::Timer::GetFPGATimestamp(); - auto result = robotPoseEstimator.Update(); - if (result.second) { - return std::make_pair<>(result.first.ToPose2d(), - currentTime - result.second); - } else { - return std::make_pair(frc::Pose2d(), 0_ms); - } - } - - .. code-block:: Python - - # Coming Soon! + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java + :language: java + :lines: 96-114 + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Vision.h + :language: c++ + :lines: 71-93 + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py + :language: python + :lines: 53 ``` -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 +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 `PhotonPoseEstimator` every loop using `addVisionMeasurement()`. TODO: add example note ## Additional `PhotonPoseEstimator` Methods @@ -142,3 +132,7 @@ Updates the stored reference pose when using the CLOSEST_TO_REFERENCE_POSE strat ### `setLastPose(Pose3d lastPose)` Update the stored last pose. Useful for setting the initial estimate when using the CLOSEST_TO_LAST_POSE strategy. + +### `setMultiTagFallbackStrategy(PoseStrategy strategy)` + +Determines the fallback strategy for pose estimation. You are strongly encouraged to set this.