Skip to content

Commit 8ce80a3

Browse files
MqxSMqxS
committed
maybe PV replay?
1 parent 442e728 commit 8ce80a3

File tree

3 files changed

+70
-18
lines changed

3 files changed

+70
-18
lines changed

‎src/main/java/frc/robot/subsystems/vision/PhotonVision.java

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -116,9 +116,12 @@ yield new SimVisionRunner(
116116
case REPLAY -> new ReplayVisionRunner(
117117
PhotonVision.apriltagFieldLayout,
118118
PhotonVision.makeVisionIOInputsMap(
119-
new ReplayVisionRunner.VisionIOApriltagsReplay(TitanCamera.PHOTON_FL_APRILTAG),
120-
new ReplayVisionRunner.VisionIOApriltagsReplay(TitanCamera.PHOTON_FC_APRILTAG),
121-
new ReplayVisionRunner.VisionIOApriltagsReplay(TitanCamera.PHOTON_FR_APRILTAG)
119+
new ReplayVisionRunner.VisionIOReplay(TitanCamera.PHOTON_FL_APRILTAG),
120+
new ReplayVisionRunner.VisionIOReplay(TitanCamera.PHOTON_FC_APRILTAG),
121+
new ReplayVisionRunner.VisionIOReplay(TitanCamera.PHOTON_FR_APRILTAG)
122+
),
123+
PhotonVision.makeVisionIOInputsMap(
124+
new ReplayVisionRunner.VisionIOReplay(TitanCamera.PHOTON_BC_NOTE_TRACKING)
122125
)
123126
);
124127
case DISABLED -> new PhotonVisionRunner() {};

‎src/main/java/frc/robot/subsystems/vision/ReplayVisionRunner.java

Lines changed: 63 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
import edu.wpi.first.math.geometry.Pose3d;
55
import frc.robot.constants.Constants;
66
import frc.robot.subsystems.vision.cameras.TitanCamera;
7+
import frc.robot.subsystems.vision.result.NoteTrackingResult;
78
import frc.robot.utils.closeables.ToClose;
89
import org.littletonrobotics.junction.Logger;
910
import org.photonvision.EstimatedRobotPose;
@@ -15,31 +16,35 @@
1516
import java.util.Map;
1617

1718
public class ReplayVisionRunner implements PhotonVisionRunner {
18-
public static class VisionIOApriltagsReplay implements VisionIO {
19+
public static class VisionIOReplay implements VisionIO {
1920
private final TitanCamera titanCamera;
2021
private final PhotonCamera photonCamera;
2122

22-
public VisionIOApriltagsReplay(final TitanCamera titanCamera) {
23+
public VisionIOReplay(final TitanCamera titanCamera) {
2324
this.titanCamera = titanCamera;
2425
this.photonCamera = titanCamera.getPhotonCamera();
2526
}
2627
}
2728

28-
private final Map<ReplayVisionRunner.VisionIOApriltagsReplay, String> visionIONames;
29-
private final Map<ReplayVisionRunner.VisionIOApriltagsReplay, VisionIO.VisionIOInputs> visionIOInputsMap;
30-
private final Map<ReplayVisionRunner.VisionIOApriltagsReplay, PhotonPoseEstimator> photonPoseEstimatorMap;
29+
private final Map<ReplayVisionRunner.VisionIOReplay, String> visionIONames;
30+
private final Map<ReplayVisionRunner.VisionIOReplay, VisionIO.VisionIOInputs> apriltagVisionIOInputsMap;
31+
private final Map<ReplayVisionRunner.VisionIOReplay, VisionIO.VisionIOInputs> noteTrackingVisionIOInputsMap;
32+
private final Map<ReplayVisionRunner.VisionIOReplay, PhotonPoseEstimator> photonPoseEstimatorMap;
3133

3234
private final Map<VisionIO, EstimatedRobotPose> estimatedRobotPoseMap;
35+
private final Map<VisionIO, NoteTrackingResult> noteTrackingResultMap;
3336

3437
public ReplayVisionRunner(
3538
final AprilTagFieldLayout aprilTagFieldLayout,
36-
final Map<ReplayVisionRunner.VisionIOApriltagsReplay, VisionIO.VisionIOInputs> visionIOInputsMap
39+
final Map<ReplayVisionRunner.VisionIOReplay, VisionIO.VisionIOInputs> apriltagVisionIOInputsMap,
40+
final Map<ReplayVisionRunner.VisionIOReplay, VisionIO.VisionIOInputs> noteTrackingVisionIOInputsMap
3741
) {
38-
this.visionIOInputsMap = visionIOInputsMap;
42+
this.apriltagVisionIOInputsMap = apriltagVisionIOInputsMap;
43+
this.noteTrackingVisionIOInputsMap = noteTrackingVisionIOInputsMap;
3944

40-
final Map<ReplayVisionRunner.VisionIOApriltagsReplay, String> visionIONames = new HashMap<>();
41-
final Map<ReplayVisionRunner.VisionIOApriltagsReplay, PhotonPoseEstimator> poseEstimatorMap = new HashMap<>();
42-
for (final ReplayVisionRunner.VisionIOApriltagsReplay visionIOApriltagsReplay : visionIOInputsMap.keySet()) {
45+
final Map<ReplayVisionRunner.VisionIOReplay, String> visionIONames = new HashMap<>();
46+
final Map<ReplayVisionRunner.VisionIOReplay, PhotonPoseEstimator> poseEstimatorMap = new HashMap<>();
47+
for (final ReplayVisionRunner.VisionIOReplay visionIOApriltagsReplay : apriltagVisionIOInputsMap.keySet()) {
4348
final PhotonPoseEstimator photonPoseEstimator = new PhotonPoseEstimator(
4449
aprilTagFieldLayout,
4550
Constants.Vision.MULTI_TAG_POSE_STRATEGY,
@@ -52,9 +57,14 @@ public ReplayVisionRunner(
5257
visionIONames.put(visionIOApriltagsReplay, visionIOApriltagsReplay.photonCamera.getName());
5358
}
5459

60+
for (final ReplayVisionRunner.VisionIOReplay visionIONoteTrackReplay : noteTrackingVisionIOInputsMap.keySet()) {
61+
visionIONames.put(visionIONoteTrackReplay, visionIONoteTrackReplay.photonCamera.getName());
62+
}
63+
5564
this.visionIONames = visionIONames;
5665
this.photonPoseEstimatorMap = poseEstimatorMap;
5766
this.estimatedRobotPoseMap = new HashMap<>();
67+
this.noteTrackingResultMap = new HashMap<>();
5868
}
5969

6070
@SuppressWarnings("DuplicatedCode")
@@ -65,10 +75,10 @@ public void periodic() {
6575
}
6676

6777
for (
68-
final Map.Entry<ReplayVisionRunner.VisionIOApriltagsReplay, VisionIO.VisionIOInputs>
69-
photonVisionIOInputsEntry : visionIOInputsMap.entrySet()
78+
final Map.Entry<ReplayVisionRunner.VisionIOReplay, VisionIO.VisionIOInputs>
79+
photonVisionIOInputsEntry : apriltagVisionIOInputsMap.entrySet()
7080
) {
71-
final ReplayVisionRunner.VisionIOApriltagsReplay visionIO = photonVisionIOInputsEntry.getKey();
81+
final ReplayVisionRunner.VisionIOReplay visionIO = photonVisionIOInputsEntry.getKey();
7282
final VisionIO.VisionIOInputs inputs = photonVisionIOInputsEntry.getValue();
7383

7484
visionIO.periodic();
@@ -81,13 +91,41 @@ public void periodic() {
8191

8292
final PhotonPipelineResult result = inputs.latestResult;
8393
VisionUtils.correctPipelineResultTimestamp(result);
94+
8495
VisionUtils.updatePoseEstimator(
8596
photonPoseEstimatorMap.get(visionIO),
8697
result
8798
).ifPresent(
8899
estimatedRobotPose -> estimatedRobotPoseMap.put(visionIO, estimatedRobotPose)
89100
);
90101
}
102+
103+
for (
104+
final Map.Entry<ReplayVisionRunner.VisionIOReplay, VisionIO.VisionIOInputs>
105+
photonVisionIOInputsEntry : noteTrackingVisionIOInputsMap.entrySet()
106+
) {
107+
final ReplayVisionRunner.VisionIOReplay visionIO = photonVisionIOInputsEntry.getKey();
108+
final VisionIO.VisionIOInputs inputs = photonVisionIOInputsEntry.getValue();
109+
110+
visionIO.periodic();
111+
visionIO.updateInputs(inputs);
112+
113+
Logger.processInputs(
114+
String.format("%s/%s", PhotonVision.PhotonLogKey, visionIONames.get(visionIO)),
115+
inputs
116+
);
117+
118+
final PhotonPipelineResult pipelineResult = inputs.latestResult;
119+
VisionUtils.correctPipelineResultTimestamp(pipelineResult);
120+
121+
Logger.recordOutput(
122+
String.format("%s/%s/HasTarget", PhotonVision.PhotonLogKey, visionIONames.get(visionIO)),
123+
pipelineResult.hasTargets()
124+
);
125+
126+
final NoteTrackingResult noteTrackingResult = new NoteTrackingResult(inputs.robotToCamera, pipelineResult);
127+
noteTrackingResultMap.put(visionIO, noteTrackingResult);
128+
}
91129
}
92130

93131
/**
@@ -98,12 +136,22 @@ public void periodic() {
98136
public void resetRobotPose(final Pose3d robotPose) {}
99137

100138
@Override
101-
public Map<ReplayVisionRunner.VisionIOApriltagsReplay, VisionIO.VisionIOInputs> getApriltagVisionIOInputsMap() {
102-
return visionIOInputsMap;
139+
public Map<ReplayVisionRunner.VisionIOReplay, VisionIO.VisionIOInputs> getApriltagVisionIOInputsMap() {
140+
return apriltagVisionIOInputsMap;
141+
}
142+
143+
@Override
144+
public Map<ReplayVisionRunner.VisionIOReplay, VisionIO.VisionIOInputs> getNoteTrackingVisionIOInputsMap() {
145+
return noteTrackingVisionIOInputsMap;
103146
}
104147

105148
@Override
106149
public EstimatedRobotPose getEstimatedRobotPose(final VisionIO visionIO) {
107150
return estimatedRobotPoseMap.get(visionIO);
108151
}
152+
153+
@Override
154+
public NoteTrackingResult getNoteTrackingResult(final VisionIO visionIO) {
155+
return noteTrackingResultMap.get(visionIO);
156+
}
109157
}

‎src/main/java/frc/robot/subsystems/vision/SimVisionRunner.java

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -193,6 +193,7 @@ public void periodic() {
193193

194194
final PhotonPipelineResult result = inputs.latestResult;
195195
VisionUtils.correctPipelineResultTimestamp(result);
196+
196197
VisionUtils.updatePoseEstimator(
197198
photonPoseEstimatorMap.get(visionIO),
198199
result

0 commit comments

Comments
 (0)