mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-21 01:01:41 +00:00
Examples Clean-Up (#1408)
This commit is contained in:
Chris Gerth
211
docs/source/docs/examples/poseest.md
Normal file
211
docs/source/docs/examples/poseest.md
Normal file
@@ -0,0 +1,211 @@
|
||||
# Using WPILib Pose Estimation, Simulation, and PhotonVision Together
|
||||
|
||||
The following example comes from the PhotonLib example repository ([Java](https://github.com/gerth2/photonvision/tree/master/photonlib-java-examples/poseest)/[C++](https://github.com/gerth2/photonvision/tree/master/photonlib-cpp-examples/poseest)/[Python](https://github.com/gerth2/photonvision/tree/master/photonlib-python-examples/poseest)). Full code is available at that links.
|
||||
|
||||
## Knowledge and Equipment Needed
|
||||
|
||||
- Everything required in {ref}`Combining Aiming and Getting in Range <docs/examples/aimandrange:Knowledge and Equipment Needed>`, plus some familiarity with WPILib pose estimation functionality.
|
||||
|
||||
## Background
|
||||
|
||||
This example demonstrates integration of swerve drive control, a basic swerve physics simulation, and PhotonLib's simulated vision system functionality.
|
||||
|
||||
## Walkthrough
|
||||
|
||||
### Estimating Pose
|
||||
|
||||
The {code}`Drivetrain` class includes functionality to fuse multiple sensor readings together (including PhotonVision) into a best-guess of the pose on the field.
|
||||
|
||||
Please reference the [WPILib documentation](https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-pose_state-estimators.html) on using the {code}`SwerveDrivePoseEstimator` class.
|
||||
|
||||
We use the 2024 game's AprilTag Locations:
|
||||
|
||||
```{eval-rst}
|
||||
.. tab-set::
|
||||
|
||||
.. tab-item:: Java
|
||||
:sync: java
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/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/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/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/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/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`:
|
||||
|
||||
|
||||
```{eval-rst}
|
||||
.. tab-set::
|
||||
|
||||
.. tab-item:: Java
|
||||
:sync: java
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/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/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/poseest/src/main/include/Vision.h
|
||||
:language: c++
|
||||
:lines: 145-145
|
||||
:linenos:
|
||||
:lineno-start: 145
|
||||
|
||||
.. tab-item:: Python
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/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`.
|
||||
|
||||
```{eval-rst}
|
||||
.. tab-set::
|
||||
|
||||
.. tab-item:: Java
|
||||
:sync: java
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/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/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp
|
||||
:language: c++
|
||||
:lines: 38-46
|
||||
:linenos:
|
||||
:lineno-start: 38
|
||||
|
||||
.. tab-item:: Python
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-python-examples/poseest/robot.py
|
||||
:language: python
|
||||
:lines: 54-56
|
||||
:linenos:
|
||||
:lineno-start: 54
|
||||
|
||||
```
|
||||
|
||||
### Simulating the Camera
|
||||
|
||||
First, we create a new {code}`VisionSystemSim` to represent our camera and coprocessor running PhotonVision, and moving around our simulated field.
|
||||
|
||||
```{eval-rst}
|
||||
.. tab-set::
|
||||
|
||||
.. tab-item:: Java
|
||||
:sync: java
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/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/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/poseest/src/main/include/Vision.h
|
||||
:language: c++
|
||||
:lines: 49-52
|
||||
:linenos:
|
||||
:lineno-start: 49
|
||||
|
||||
.. tab-item:: Python
|
||||
|
||||
# Coming Soon!
|
||||
|
||||
```
|
||||
|
||||
Then, we add configure the simulated vision system to match the camera system being simulated.
|
||||
|
||||
```{eval-rst}
|
||||
.. tab-set::
|
||||
|
||||
.. tab-item:: Java
|
||||
:sync: java
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/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/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/poseest/src/main/include/Vision.h
|
||||
:language: c++
|
||||
:lines: 53-65
|
||||
:linenos:
|
||||
:lineno-start: 53
|
||||
|
||||
.. tab-item:: Python
|
||||
|
||||
# Coming Soon!
|
||||
```
|
||||
|
||||
|
||||
### Updating the Simulated Vision System
|
||||
|
||||
During simulation, we periodically update the simulated vision system.
|
||||
|
||||
```{eval-rst}
|
||||
.. tab-set::
|
||||
|
||||
.. tab-item:: Java
|
||||
:sync: java
|
||||
|
||||
.. rli:: https://raw.githubusercontent.com/gerth2/photonvision/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/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/adb3098fbe0cdbc1a378c6d5a41126dd1d6d6955/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp
|
||||
:language: c++
|
||||
:lines: 95-109
|
||||
:linenos:
|
||||
:lineno-start: 95
|
||||
|
||||
.. tab-item:: Python
|
||||
|
||||
# Coming Soon!
|
||||
```
|
||||
|
||||
The rest is done behind the scenes.
|
||||
|
||||
```{image} images/poseest_demo.gif
|
||||
:alt: Simulated swerve drive and vision system working together in teleoperated mode.
|
||||
:width: 1200
|
||||
```
|
||||
Reference in New Issue
Block a user