Skip to content

Commit 44b696a

Browse files
Tortuga-AMTortuga-AM
committed
Refactor VisionIO and VisionConstants for improved clarity and organization
1 parent 266c181 commit 44b696a

File tree

3 files changed

+131
-117
lines changed

3 files changed

+131
-117
lines changed

‎src/main/java/frc/robot/Subsystems/Vision/Vision.java‎

Lines changed: 106 additions & 85 deletions
Original file line numberDiff line numberDiff line change
@@ -15,9 +15,13 @@
1515
import java.util.List;
1616
import org.littletonrobotics.junction.Logger;
1717

18+
/**
19+
* Vision subsystem responsible for processing camera inputs and providing pose observations
20+
* Bascically stolen from Pioneer's code but modified for number of cameras and camera positions
21+
*/
1822
public class Vision extends SubsystemBase {
1923

20-
// Akit my savior
24+
// Vision IO instances and inputs
2125
private final VisionIO[] io;
2226
private final VisionIOInputsAutoLogged[] inputs;
2327
private final Alert[] disconnectedAlerts;
@@ -44,11 +48,7 @@ public static Vision getInstance() {
4448
Drive.getInstance()::getPose
4549
),
4650
};
47-
case TESTING -> new VisionIO[] {
48-
new VisionIOPhotonVision(FRONT_LEFT_CAM_NAME, ROBOT_TO_FRONT_LEFT_CAMERA),
49-
new VisionIOPhotonVision(FRONT_RIGHT_CAM_NAME, ROBOT_TO_FRONT_RIGHT_CAMERA),
50-
};
51-
case REPLAY -> new VisionIO[] {
51+
case TESTING, REPLAY -> new VisionIO[] {
5252
new VisionIOPhotonVision(FRONT_LEFT_CAM_NAME, ROBOT_TO_FRONT_LEFT_CAMERA),
5353
new VisionIOPhotonVision(FRONT_RIGHT_CAM_NAME, ROBOT_TO_FRONT_RIGHT_CAMERA),
5454
};
@@ -71,27 +71,22 @@ private Vision(VisionIO... io) {
7171
this.disconnectedAlerts = new Alert[io.length];
7272
for (int i = 0; i < inputs.length; i++) {
7373
disconnectedAlerts[i] = new Alert(
74-
"Vision camera " + Integer.toString(i) + " is disconnected.",
74+
"Vision camera " + i + " is disconnected.",
7575
AlertType.kWarning
7676
);
7777
}
7878
}
7979

80-
/**
81-
* Returns the X angle to the best target, which can be used for simple servoing
82-
* with vision.
83-
*
84-
* @param cameraIndex The index of the camera to use.
85-
*/
8680
public Rotation2d getTargetX(int cameraIndex) {
8781
return inputs[cameraIndex].latestTargetObservation.tx();
8882
}
8983

9084
@Override
9185
public void periodic() {
86+
// Update inputs and log data for each camera
9287
for (int i = 0; i < io.length; i++) {
9388
io[i].updateInputs(inputs[i]);
94-
Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]);
89+
Logger.processInputs("Vision/Camera" + i, inputs[i]);
9590
}
9691

9792
// Initialize logging values
@@ -100,12 +95,12 @@ public void periodic() {
10095
List<Pose3d> allRobotPosesAccepted = new LinkedList<>();
10196
List<Pose3d> allRobotPosesRejected = new LinkedList<>();
10297

103-
// Loop over cameras
98+
// Process data for each camera
10499
for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) {
105-
// Update disconnected alert
100+
// Update disconnected alerts
106101
disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected);
107102

108-
// Initialize logging values
103+
// Initialize logging values for this camera
109104
List<Pose3d> tagPoses = new LinkedList<>();
110105
List<Pose3d> robotPoses = new LinkedList<>();
111106
List<Pose3d> robotPosesAccepted = new LinkedList<>();
@@ -114,43 +109,15 @@ public void periodic() {
114109
// Add tag poses
115110
for (int tagId : inputs[cameraIndex].tagIds) {
116111
var tagPose = APRIL_TAG_FIELD_LAYOUT.getTagPose(tagId);
117-
if (tagPose.isPresent()) {
118-
tagPoses.add(tagPose.get());
119-
}
112+
tagPose.ifPresent(tagPoses::add);
120113
}
121114

122-
// Loop over pose observations
115+
// Process pose observations
123116
for (var observation : inputs[cameraIndex].poseObservations) {
124-
// Check whether to reject pose
125-
boolean rejectPose =
126-
observation.tagCount() == 0 || // Must have at least one tag
127-
(observation.tagCount() == 1 && observation.ambiguity() > maxAmbiguity) || // Cannot be high ambiguity
128-
Math.abs(observation.pose().getZ()) > maxZError || // Must have realistic Z coordinate
129-
// Must be within the field boundaries
130-
observation.pose().getX() <
131-
0.0 ||
132-
observation.pose().getX() > APRIL_TAG_FIELD_LAYOUT.getFieldLength() ||
133-
observation.pose().getY() < 0.0 ||
134-
observation.pose().getY() > APRIL_TAG_FIELD_LAYOUT.getFieldWidth();
135-
136-
Logger.recordOutput(
137-
"Vision/Camera" + Integer.toString(cameraIndex) + "/Tag Count",
138-
observation.tagCount() == 0
139-
);
140-
Logger.recordOutput(
141-
"Vision/Camera" + Integer.toString(cameraIndex) + "/Ambiguous",
142-
(observation.tagCount() == 1 && observation.ambiguity() > maxAmbiguity)
143-
);
144-
Logger.recordOutput(
145-
"Vision/Camera" + Integer.toString(cameraIndex) + "/Outside of Field X",
146-
observation.pose().getX() < 0.0 ||
147-
observation.pose().getX() > APRIL_TAG_FIELD_LAYOUT.getFieldLength()
148-
);
149-
Logger.recordOutput(
150-
"Vision/Camera" + Integer.toString(cameraIndex) + "/Outside of Field Y",
151-
observation.pose().getY() < 0.0 ||
152-
observation.pose().getY() > APRIL_TAG_FIELD_LAYOUT.getFieldWidth()
153-
);
117+
boolean rejectPose = shouldRejectPose(observation);
118+
119+
// Log rejection reasons
120+
logRejectionReasons(cameraIndex, observation);
154121

155122
// Add pose to log
156123
robotPoses.add(observation.pose());
@@ -166,68 +133,122 @@ public void periodic() {
166133
}
167134

168135
// Calculate standard deviations
169-
double stdDevFactor =
170-
Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount();
171-
double linearStdDev = linearStdDevBaseline * stdDevFactor;
172-
double angularStdDev = angularStdDevBaseline * stdDevFactor;
173-
if (observation.type() == PoseObservationType.MEGATAG_2) {
174-
linearStdDev *= linearStdDevMegatag2Factor;
175-
angularStdDev *= angularStdDevMegatag2Factor;
176-
}
177-
if (cameraIndex < cameraStdDevFactors.length) {
178-
linearStdDev *= cameraStdDevFactors[cameraIndex];
179-
angularStdDev *= cameraStdDevFactors[cameraIndex];
180-
}
136+
double[] stdDevs = calculateStandardDeviations(cameraIndex, observation);
181137

182138
// Send vision observation
183139
Drive.getInstance()
184140
.addVisionMeasurement(
185141
observation.pose().toPose2d(),
186142
observation.timestamp(),
187-
VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev)
143+
VecBuilder.fill(stdDevs[0], stdDevs[0], stdDevs[1])
188144
);
189145
}
190146

191-
// Log camera datadata
192-
Logger.recordOutput(
193-
"Vision/Camera" + Integer.toString(cameraIndex) + "/TagPoses",
194-
tagPoses.toArray(new Pose3d[tagPoses.size()])
195-
);
196-
Logger.recordOutput(
197-
"Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPoses",
198-
robotPoses.toArray(new Pose3d[robotPoses.size()])
199-
);
200-
Logger.recordOutput(
201-
"Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesAccepted",
202-
robotPosesAccepted.toArray(new Pose3d[robotPosesAccepted.size()])
203-
);
204-
Logger.recordOutput(
205-
"Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesRejected",
206-
robotPosesRejected.toArray(new Pose3d[robotPosesRejected.size()])
207-
);
147+
// Log camera data
148+
logCameraData(cameraIndex, tagPoses, robotPoses, robotPosesAccepted, robotPosesRejected);
208149

150+
// Aggregate data
209151
allTagPoses.addAll(tagPoses);
210152
allRobotPoses.addAll(robotPoses);
211153
allRobotPosesAccepted.addAll(robotPosesAccepted);
212154
allRobotPosesRejected.addAll(robotPosesRejected);
213155
}
214156

215157
// Log summary data
158+
logSummaryData(allTagPoses, allRobotPoses, allRobotPosesAccepted, allRobotPosesRejected);
159+
}
160+
161+
162+
// Determines whether a pose observation should be rejected.
163+
private boolean shouldRejectPose(VisionIO.PoseObservation observation) {
164+
return observation.tagCount() == 0 ||
165+
(observation.tagCount() == 1 && observation.ambiguity() > maxAmbiguity) ||
166+
Math.abs(observation.pose().getZ()) > maxZError ||
167+
observation.pose().getX() < 0.0 ||
168+
observation.pose().getX() > APRIL_TAG_FIELD_LAYOUT.getFieldLength() ||
169+
observation.pose().getY() < 0.0 ||
170+
observation.pose().getY() > APRIL_TAG_FIELD_LAYOUT.getFieldWidth();
171+
}
172+
173+
// Adds rejection reasons to logs
174+
private void logRejectionReasons(int cameraIndex, VisionIO.PoseObservation observation) {
175+
Logger.recordOutput(
176+
"Vision/Camera" + cameraIndex + "/Tag Count",
177+
observation.tagCount() == 0
178+
);
179+
Logger.recordOutput(
180+
"Vision/Camera" + cameraIndex + "/Ambiguous",
181+
(observation.tagCount() == 1 && observation.ambiguity() > maxAmbiguity)
182+
);
183+
Logger.recordOutput(
184+
"Vision/Camera" + cameraIndex + "/Outside of Field X",
185+
observation.pose().getX() < 0.0 ||
186+
observation.pose().getX() > APRIL_TAG_FIELD_LAYOUT.getFieldLength()
187+
);
188+
Logger.recordOutput(
189+
"Vision/Camera" + cameraIndex + "/Outside of Field Y",
190+
observation.pose().getY() < 0.0 ||
191+
observation.pose().getY() > APRIL_TAG_FIELD_LAYOUT.getFieldWidth()
192+
);
193+
}
194+
195+
// Calculates standard deviations for a pose observation.
196+
private double[] calculateStandardDeviations(int cameraIndex, VisionIO.PoseObservation observation) {
197+
double stdDevFactor = Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount();
198+
double linearStdDev = linearStdDevBaseline * stdDevFactor;
199+
double angularStdDev = angularStdDevBaseline * stdDevFactor;
200+
if (observation.type() == PoseObservationType.MEGATAG_2) {
201+
linearStdDev *= linearStdDevMegatag2Factor;
202+
angularStdDev *= angularStdDevMegatag2Factor;
203+
}
204+
if (cameraIndex < cameraStdDevFactors.length) {
205+
linearStdDev *= cameraStdDevFactors[cameraIndex];
206+
angularStdDev *= cameraStdDevFactors[cameraIndex];
207+
}
208+
return new double[] { linearStdDev, angularStdDev };
209+
}
210+
211+
212+
// Logs camera-specific data.
213+
private void logCameraData(int cameraIndex, List<Pose3d> tagPoses, List<Pose3d> robotPoses,
214+
List<Pose3d> robotPosesAccepted, List<Pose3d> robotPosesRejected) {
215+
Logger.recordOutput(
216+
"Vision/Camera" + cameraIndex + "/TagPoses",
217+
tagPoses.toArray(new Pose3d[0])
218+
);
219+
Logger.recordOutput(
220+
"Vision/Camera" + cameraIndex + "/RobotPoses",
221+
robotPoses.toArray(new Pose3d[0])
222+
);
223+
Logger.recordOutput(
224+
"Vision/Camera" + cameraIndex + "/RobotPosesAccepted",
225+
robotPosesAccepted.toArray(new Pose3d[0])
226+
);
227+
Logger.recordOutput(
228+
"Vision/Camera" + cameraIndex + "/RobotPosesRejected",
229+
robotPosesRejected.toArray(new Pose3d[0])
230+
);
231+
}
232+
233+
234+
// Logs summary data for all cameras.
235+
private void logSummaryData(List<Pose3d> allTagPoses, List<Pose3d> allRobotPoses,
236+
List<Pose3d> allRobotPosesAccepted, List<Pose3d> allRobotPosesRejected) {
216237
Logger.recordOutput(
217238
"Vision/Summary/TagPoses",
218-
allTagPoses.toArray(new Pose3d[allTagPoses.size()])
239+
allTagPoses.toArray(new Pose3d[0])
219240
);
220241
Logger.recordOutput(
221242
"Vision/Summary/RobotPoses",
222-
allRobotPoses.toArray(new Pose3d[allRobotPoses.size()])
243+
allRobotPoses.toArray(new Pose3d[0])
223244
);
224245
Logger.recordOutput(
225246
"Vision/Summary/RobotPosesAccepted",
226-
allRobotPosesAccepted.toArray(new Pose3d[allRobotPosesAccepted.size()])
247+
allRobotPosesAccepted.toArray(new Pose3d[0])
227248
);
228249
Logger.recordOutput(
229250
"Vision/Summary/RobotPosesRejected",
230-
allRobotPosesRejected.toArray(new Pose3d[allRobotPosesRejected.size()])
251+
allRobotPosesRejected.toArray(new Pose3d[0])
231252
);
232253
}
233254
}

‎src/main/java/frc/robot/Subsystems/Vision/VisionConstants.java‎

Lines changed: 23 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -9,9 +9,12 @@
99
import edu.wpi.first.math.util.Units;
1010
import org.team7525.misc.CameraResolution;
1111

12+
/**
13+
* Constants for the Vision subsystem.
14+
*/
1215
public class VisionConstants {
1316

14-
// Front
17+
// Front-left camera configuration
1518
public static final String FRONT_LEFT_CAM_NAME = "Front_Left_Camera";
1619
public static final Translation3d ROBOT_TO_FRONT_LEFT_CAMERA_TRANSLATION = new Translation3d(
1720
Units.inchesToMeters(-11.113),
@@ -28,7 +31,7 @@ public class VisionConstants {
2831
ROBOT_TO_FRONT_LEFT_CAMERA_ROTATION
2932
);
3033

31-
//Back
34+
// Front-right camera configuration
3235
public static final String FRONT_RIGHT_CAM_NAME = "Front_Right_Camera";
3336
public static final Translation3d ROBOT_TO_FRONT_RIGHT_CAMERA_TRANSLATION = new Translation3d(
3437
Units.inchesToMeters(11.113),
@@ -45,53 +48,43 @@ public class VisionConstants {
4548
ROBOT_TO_FRONT_RIGHT_CAMERA_ROTATION
4649
);
4750

51+
// Camera settings
4852
public static final double CAMERA_DEBOUNCE_TIME = 0.5;
53+
public static final CameraResolution FRONT_RIGHT_CAMERA_RESOLUTION = CameraResolution.HIGH_RESOLUTION;
54+
public static final CameraResolution FRONT_LEFT_CAMERA_RESOLUTION = CameraResolution.HIGH_RESOLUTION;
4955

50-
// 1080p is high
51-
public static final CameraResolution FRONT_RIGHT_CAMERA_RESOLUTION =
52-
CameraResolution.HIGH_RESOLUTION;
53-
public static final CameraResolution FRONT_LEFT_CAMERA_RESOLUTION =
54-
CameraResolution.HIGH_RESOLUTION;
55-
56-
// Other
57-
// INSANE skill issue from First
58-
// This is comp dependent
59-
public static final boolean USE_WELDED_FIELD = false;
60-
56+
// Field layout configuration
57+
public static final boolean USE_WELDED_FIELD = false; // Adjust based on competition setup
6158
public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadField(
6259
USE_WELDED_FIELD
6360
? AprilTagFields.k2025ReefscapeWelded
6461
: AprilTagFields.k2025ReefscapeAndyMark
6562
);
6663

67-
public static final int CAMERA_WIDTH = 1200;
68-
public static final int CAMERA_HEIGHT = 800;
64+
// Camera properties
65+
public static final int CAMERA_WIDTH = 1200;
66+
public static final int CAMERA_HEIGHT = 800;
6967
public static final Rotation2d CAMERA_FOV = Rotation2d.fromDegrees(84.47);
7068
public static final double CALIB_ERROR_AVG = 0.25;
7169
public static final double CALIB_ERROR_STD_DEV = 0.08;
7270
public static final int CAMERA_FPS = 40;
73-
public static final int AVG_LATENCY_MS = 40;
71+
public static final int AVG_LATENCY_MS = 40;
7472
public static final int LATENCY_STD_DEV_MS = 10;
7573

76-
// AKIT TEMPLATE STUFF
77-
78-
// Basic filtering thresholds
79-
public static final double maxAmbiguity = 0.3;
80-
public static final double maxZError = 0.75;
74+
// Filtering thresholds for pose observations
75+
public static final double maxAmbiguity = 0.3;
76+
public static final double maxZError = 0.75;
8177

82-
// Standard deviation baselines, for 1 meter distance and 1 tag
83-
// (Adjusted automatically based on distance and # of tags)
84-
public static final double linearStdDevBaseline = 0.02; // Meters
85-
public static final double angularStdDevBaseline = 0.06; // Radians
78+
public static final double linearStdDevBaseline = 0.02;
79+
public static final double angularStdDevBaseline = 0.06;
8680

87-
// Standard deviation multipliers for each camera
88-
// (Adjust to trust some cameras more than others)
81+
// Camera-specific standard deviation multipliers
8982
public static final double[] cameraStdDevFactors = new double[] {
9083
1.0, // Camera 0
9184
1.0, // Camera 1
9285
};
9386

94-
// Multipliers to apply for MegaTag 2 observations
95-
public static final double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve
96-
public static final double angularStdDevMegatag2Factor = Double.POSITIVE_INFINITY; // No rotation data available
87+
// Multipliers for MegaTag 2 observations
88+
public static final double linearStdDevMegatag2Factor = 0.5;
89+
public static final double angularStdDevMegatag2Factor = Double.POSITIVE_INFINITY;
9790
}

‎src/main/java/frc/robot/Subsystems/Vision/VisionIO.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,10 +17,10 @@ public static class VisionIOInputs {
1717
public int[] tagIds = new int[0];
1818
}
1919

20-
/** Represents the angle to a simple target, not used for pose estimation. */
20+
// Represents the angle to a simple target, not used for pose estimation.
2121
public static record TargetObservation(Rotation2d tx, Rotation2d ty) {}
2222

23-
/** Represents a robot pose sample used for pose estimation. */
23+
// Represents a robot pose sample used for pose estimation.
2424
public static record PoseObservation(
2525
double timestamp,
2626
Pose3d pose,

0 commit comments

Comments
 (0)