diff --git a/.gitignore b/.gitignore index 68d549817..63d518c8d 100644 --- a/.gitignore +++ b/.gitignore @@ -7,8 +7,7 @@ __pycache__/ backend/settings/ .vscode/ # Docs -source/_build -source/docs/_build +_build # Compiled class file *.class diff --git a/docs/source/docs/apriltag-pipelines/multitag.md b/docs/source/docs/apriltag-pipelines/multitag.md index f7ed78a9d..e894b955c 100644 --- a/docs/source/docs/apriltag-pipelines/multitag.md +++ b/docs/source/docs/apriltag-pipelines/multitag.md @@ -23,30 +23,41 @@ Ensure that your camera is calibrated and 3D mode is enabled. Navigate to the Ou By default, enabling multi-target will disable calculating camera-to-target transforms for each observed AprilTag target to increase performance; the X/Y/angle numbers shown in the target table of the UI are instead calculated using the tag's expected location (per the field layout JSON) and the field-to-camera transform calculated using MultiTag. If you additionally want the individual camera-to-target transform calculated using SolvePNP for each target, enable "Always Do Single-Target Estimation". ::: -This multi-target pose estimate can be accessed using PhotonLib. We suggest using {ref}`the PhotonPoseEstimator class ` with the `MULTI_TAG_PNP_ON_COPROCESSOR` strategy to simplify code, but the transform can be directly accessed using `getMultiTagResult`/`MultiTagResult()` (Java/C++). +This multi-target pose estimate can be accessed using PhotonLib. We suggest using {ref}`the PhotonPoseEstimator class ` with the `MULTI_TAG_PNP_ON_COPROCESSOR` strategy to simplify code, but the transform can be directly accessed using `getMultiTagResult`/`MultiTagResult()`/`multitagResult` (Java/C++/Python). ```{eval-rst} .. tab-set-code:: .. code-block:: Java - var result = camera.getLatestResult(); - if (result.getMultiTagResult().estimatedPose.isPresent) { - Transform3d fieldToCamera = result.getMultiTagResult().estimatedPose.best; + var results = camera.getAllUnreadResults(); + for (var result : results) { + var multiTagResult = result.getMultiTagResult(); + if (multiTagResult.isPresent()) { + var fieldToCamera = multiTagResult.get().estimatedPose.best; + } } .. code-block:: C++ - auto result = camera.GetLatestResult(); - if (result.MultiTagResult().result.isPresent) { - frc::Transform3d fieldToCamera = result.MultiTagResult().result.best; + auto results = camera.GetAllUnreadResults(); + for (auto &result : results) + { + auto multiTagResult = result.MultiTagResult(); + if (multiTagResult.has_value()) { + frc::Transform3d fieldToCamera = multiTagResult->estimatedPose.best; } + } + .. code-block:: Python - # Coming Soon! - + results = camera.getAllUnreadResults() + for result in results: + multitagResult = result.multitagResult + if multitagResult is not None: + fieldToCamera = multitagResult.estimatedPose.best ``` :::{note} diff --git a/docs/source/docs/examples/aimandrange.md b/docs/source/docs/examples/aimandrange.md index 86a2d3173..417c44a3b 100644 --- a/docs/source/docs/examples/aimandrange.md +++ b/docs/source/docs/examples/aimandrange.md @@ -1,6 +1,6 @@ # Combining Aiming and Getting in Range -The following example is from the PhotonLib example repository ([Java](https://github.com/PhotonVision/photonvision/tree/main/photonlib-java-examples/aimandrange)/[C++](https://github.com/PhotonVision/photonvision/tree/main/photonlib-cpp-examples/aimandrange)). +The following example is from the PhotonLib example repository ([Java](https://github.com/PhotonVision/photonvision/tree/main/photonlib-java-examples/aimandrange)/[C++](https://github.com/PhotonVision/photonvision/tree/main/photonlib-cpp-examples/aimandrange)/[Python](https://github.com/PhotonVision/photonvision/tree/main/photonlib-python-examples/aimandrange)) ## Knowledge and Equipment Needed @@ -10,7 +10,7 @@ The following example is from the PhotonLib example repository ([Java](https://g Now that you know how to aim toward the AprilTag, let's also drive the correct distance from the AprilTag. -To do this, we'll use the *pitch* of the target in the camera image and trigonometry to figure out how far away the robot is from the AprilTag. Then, like before, we'll use the P term of a PID controller to drive the robot to the correct distance. +To do this, we'll use the _pitch_ of the target in the camera image and trigonometry to figure out how far away the robot is from the AprilTag. Then, like before, we'll use the P term of a PID controller to drive the robot to the correct distance. ```{eval-rst} .. tab-set:: @@ -43,8 +43,8 @@ To do this, we'll use the *pitch* of the target in the camera image and trigonom .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/abe95dfaa055bbe3609f72cfcaaba0f96ee7978c/photonlib-python-examples/aimandrange/robot.py :language: python - :lines: 44-95 + :lines: 52-91 :linenos: - :lineno-start: 44 + :lineno-start: 52 ``` diff --git a/docs/source/docs/programming/photonlib/getting-target-data.md b/docs/source/docs/programming/photonlib/getting-target-data.md index 7729f43b7..69389eeb4 100644 --- a/docs/source/docs/programming/photonlib/getting-target-data.md +++ b/docs/source/docs/programming/photonlib/getting-target-data.md @@ -86,7 +86,7 @@ Each pipeline result has a `hasTargets()`/`HasTargets()` (Java and C++ respectiv ``` :::{warning} -In Java/C++, You must *always* check if the result has a target via `hasTargets()`/`HasTargets()` before getting targets or else you may get a null pointer exception. Further, you must use the same result in every subsequent call in that loop. +In Java/C++, You must _always_ check if the result has a target via `hasTargets()`/`HasTargets()` before getting targets or else you may get a null pointer exception. Further, you must use the same result in every subsequent call in that loop. ::: ## Getting a List of Targets @@ -140,7 +140,7 @@ You can get the {ref}`best target `. ::: -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. For each camera, a separate instance of the `PhotonPoseEstimator` class should be created. ## Creating an `AprilTagFieldLayout` `AprilTagFieldLayout` is used to represent a layout of AprilTags within a space (field, shop at home, classroom, etc.). WPILib provides a JSON that describes the layout of AprilTags on the field which you can then use in the AprilTagFieldLayout constructor. You can also specify a custom layout. -The API documentation can be found in here: [Java](https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/apriltag/AprilTagFieldLayout.html) and [C++](https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_april_tag_field_layout.html). +The API documentation can be found in here: [Java](https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/apriltag/AprilTagFieldLayout.html), [C++](https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_april_tag_field_layout.html), and [Python](https://robotpy.readthedocs.io/projects/apriltag/en/stable/robotpy_apriltag/AprilTagFieldLayout.html#robotpy_apriltag.AprilTagFieldLayout). ```{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` - .. code-block:: Python +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 radians. For more information on the coordinate system, please see the {ref}`Coordinate Systems ` documentation. - # Coming Soon! +```{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 + .. 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` @@ -35,127 +51,116 @@ The API documentation can be found in here: [Java](https://github.wpilib.org/all The PhotonPoseEstimator has a constructor that takes an `AprilTagFieldLayout` (see above), `PoseStrategy`, `PhotonCamera`, and `Transform3d`. `PoseStrategy` has nine 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. - - Must configure the AprilTagFieldLayout properly in the UI, please see {ref}`here ` for more information. + - Calculates a new robot position estimate by combining all visible tag corners. Recommended for all teams as it will be the most accurate. + - Must configure the AprilTagFieldLayout properly in the UI, please see {ref}`here ` for more information. - LOWEST_AMBIGUITY - - Choose the Pose with the lowest ambiguity. + - Choose the Pose with the lowest ambiguity. - CLOSEST_TO_CAMERA_HEIGHT - - Choose the Pose which is closest to the camera height. + - Choose the Pose which is closest to the camera height. - CLOSEST_TO_REFERENCE_POSE - - Choose the Pose which is closest to the pose from setReferencePose(). + - Choose the Pose which is closest to the pose from setReferencePose(). - CLOSEST_TO_LAST_POSE - - Choose the Pose which is closest to the last pose calculated. + - Choose the Pose which is closest to the last pose calculated. - AVERAGE_BEST_TARGETS - - Choose the Pose which is the average of all the poses from each tag. + - Choose the Pose which is the average of all the poses from each tag. - MULTI_TAG_PNP_ON_RIO - - A slower, older version of MULTI_TAG_PNP_ON_COPROCESSOR, not recommended for use. + - A slower, older version of MULTI_TAG_PNP_ON_COPROCESSOR, not recommended for use. - PNP_DISTANCE_TRIG_SOLVE - - Use distance data from best visible tag to compute a Pose. This runs on the RoboRIO in order - to access the robot's yaw heading, and MUST have addHeadingData called every frame so heading - data is up-to-date. Based on a reference implementation by [FRC Team 6328 Mechanical Advantage](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98). + - Use distance data from best visible tag to compute a Pose. This runs on the RoboRIO in order + to access the robot's yaw heading, and MUST have addHeadingData called every frame so heading + data is up-to-date. Based on a reference implementation by [FRC Team 6328 Mechanical Advantage](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98). - CONSTRAINED_SOLVEPNP - - Solve a constrained version of the Perspective-n-Point problem with the robot's drivebase - flat on the floor. This computation takes place on the RoboRIO, and should not take more than 2ms. - This also requires addHeadingData to be called every frame so heading data is up to date. - If Multi-Tag PNP is enabled on the coprocessor, it will be used to provide an initial seed to - the optimization algorithm -- otherwise, the multi-tag fallback strategy will be used as the - seed. + - Solve a constrained version of the Perspective-n-Point problem with the robot's drivebase + flat on the floor. This computation takes place on the RoboRIO, and should not take more than 2ms. + This also requires addHeadingData to be called every frame so heading data is up to date. + If Multi-Tag PNP is enabled on the coprocessor, it will be used to provide an initial seed to + the optimization algorithm -- otherwise, the multi-tag fallback strategy will be used as the + seed. ```{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/Vision.java + :language: java + :lines: 65-66 - //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-cpp-examples/poseest/src/main/include/Vision.h + :language: c++ + :lines: 150-153 - // Construct PhotonPoseEstimator - PhotonPoseEstimator photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, cam, robotToCam); - - .. 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 - - // Assemble the list of cameras & mount locations - std::vector< - std::pair, frc::Transform3d>> - cameras; - cameras.push_back(std::make_pair(cameraOne, robotToCam)); - - photonlib::RobotPoseEstimator estimator( - aprilTags, photonlib::CLOSEST_TO_REFERENCE_POSE, cameras); - - .. code-block:: Python - - 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.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: 45-50 ``` +:::{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`. +::: + ## 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()`. +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. ```{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: 63 + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h + :language: c++ + :lines: 55 + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py + :language: python + :lines: 44 ``` -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 +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. + +```{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: 93-116 + + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Vision.h + :language: c++ + :lines: 80-100 + + .. 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 `PhotonPoseEstimator` every loop using `addVisionMeasurement()`. + +```{eval-rst} +.. tab-set-code:: + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java + :language: java + :lines: 49 + + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-cpp-examples/poseest/src/main/include/Robot.h + :language: c++ + :lines: 54-57 + + .. rli:: https://raw.githubusercontent.com/PhotonVision/photonvision/refs/heads/main/photonlib-python-examples/poseest/robot.py + :language: python + :lines: 54-57 +``` + +## Complete Examples + +The complete examples for the `PhotonPoseEstimator` can be found in the following locations: + +- [Java](https://github.com/PhotonVision/photonvision/tree/main/photonlib-java-examples/poseest) +- [C++](https://github.com/PhotonVision/photonvision/tree/main/photonlib-cpp-examples/poseest) +- [Python](https://github.com/PhotonVision/photonvision/tree/main/photonlib-python-examples/poseest) ## Additional `PhotonPoseEstimator` Methods -### `setReferencePose(Pose3d referencePose)` +For more information on the `PhotonPoseEstimator` class, please see the API documentation. -Updates the stored reference pose when using the CLOSEST_TO_REFERENCE_POSE strategy. - -### `setLastPose(Pose3d lastPose)` - -Update the stored last pose. Useful for setting the initial estimate when using the CLOSEST_TO_LAST_POSE strategy. - -### `addHeadingData(double timestampSeconds, Rotation2d heading)` - -Adds robot heading data to be stored in buffer. Must be called periodically with a proper timestamp for the PNP_DISTANCE_TRIG_SOLVE and CONSTRAINED_SOLVEPNP strategies +- [Java Documentation](https://javadocs.photonvision.org/release/org/photonvision/PhotonPoseEstimator.html) +- [C++ Documentation](https://cppdocs.photonvision.org/release/classphoton_1_1_photon_pose_estimator.html) +- [Python Documentation](https://pydocs.photonvision.org/release/reference/photonPoseEstimator/) diff --git a/docs/source/docs/troubleshooting/camera-troubleshooting.md b/docs/source/docs/troubleshooting/camera-troubleshooting.md index 9c3f2ba9e..6ec73b82f 100644 --- a/docs/source/docs/troubleshooting/camera-troubleshooting.md +++ b/docs/source/docs/troubleshooting/camera-troubleshooting.md @@ -12,7 +12,7 @@ If you haven't yet, please refer to {ref}`the Pi CSI Camera Configuration page < If the driver isn't loaded, you may be using a non-official Pi image, or an image not new enough. Try updating to the most recent image available (one released for 2023) -- if that doesn't resolve the problem, {ref}`contact us` with your settings ZIP file and Pi version/camera version/config.txt file used. -If the camera is not detected, the most likely cause is either a config.txt file incorrectly set-up, or a ribbon cable attached backwards. Review the {ref}`picam configuration page `, and verify the ribbon cable is properly oriented at both ends, and that it is \_fully\_ inserted into the FFC connector. Then, {ref}`contact us` with your settings ZIP file and Pi version/camera version/config.txt file used. +If the camera is not detected, the most likely cause is either a config.txt file incorrectly set-up, or a ribbon cable attached backwards. Review the {ref}`picam configuration page `, and verify the ribbon cable is properly oriented at both ends, and that it is _fully_ inserted into the FFC connector. Then, {ref}`contact us` with your settings ZIP file and Pi version/camera version/config.txt file used. ## USB cameras