Skip to content

Commit

Permalink
RLI's updated
Browse files Browse the repository at this point in the history
  • Loading branch information
@gerth2
gerth2 committed Sep 8, 2024
1 parent 56513ce commit adb3098
Show file tree
Hide file tree
Showing 5 changed files with 145 additions and 23 deletions.
20 changes: 16 additions & 4 deletions docs/source/docs/examples/aimandrange.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,22 +17,34 @@ 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:
:lineno-start: 84
.. 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
```
24 changes: 18 additions & 6 deletions docs/source/docs/examples/aimingatatarget.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
```
95 changes: 89 additions & 6 deletions docs/source/docs/examples/poseest.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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`:


Expand All @@ -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`.
Expand All @@ -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
Expand All @@ -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.
Expand All @@ -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
```


Expand All @@ -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.
17 changes: 15 additions & 2 deletions docs/source/docs/programming/photonlib/robot-pose-estimator.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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`
Expand Down Expand Up @@ -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
Expand Down
12 changes: 7 additions & 5 deletions photonlib-python-examples/poseest/robot.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"""
Expand All @@ -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()
Expand Down

0 comments on commit adb3098

Please sign in to comment.