Skip to content

Commit 0a719fb

Browse files
pix-xlpix-xl
authored
Vision V1
needs shuffleboard implementation for vision build fails due to drivetrain errors
2 parents 35a7213 + cbb10ae commit 0a719fb

File tree

5 files changed

+216
-49
lines changed

5 files changed

+216
-49
lines changed

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

Lines changed: 124 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,124 @@
1+
package frc.robot.vision;
2+
3+
import static frc.robot.vision.VisionConfig.multiTagStdDevs;
4+
import static frc.robot.vision.VisionConfig.singleTagStdDevs;
5+
import static frc.robot.vision.VisionConfig.tagLayout;
6+
7+
import java.util.List;
8+
import java.util.Optional;
9+
10+
import org.photonvision.EstimatedRobotPose;
11+
import org.photonvision.PhotonCamera;
12+
import org.photonvision.PhotonPoseEstimator;
13+
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
14+
import org.photonvision.targeting.PhotonTrackedTarget;
15+
16+
import edu.wpi.first.math.Matrix;
17+
import edu.wpi.first.math.VecBuilder;
18+
import edu.wpi.first.math.geometry.Transform3d;
19+
import edu.wpi.first.math.numbers.N1;
20+
import edu.wpi.first.math.numbers.N3;
21+
22+
/*
23+
* This code is a modified version of Photonvision's own example and 7028's vision code:
24+
* https://github.com/PhotonVision/photonvision/blob/main/photonlib-java-examples/poseest
25+
* https://github.com/STMARobotics/frc-7028-2025
26+
*/
27+
28+
public class Vision {
29+
30+
private final PhotonCamera cam;
31+
private final PhotonPoseEstimator photonEstimator;
32+
private Matrix<N3, N1> curStdDevs;
33+
34+
//Construct Vision Instance
35+
public Vision(String cameraName, Transform3d camTransform){
36+
cam = new PhotonCamera(cameraName);
37+
38+
photonEstimator = new PhotonPoseEstimator(
39+
tagLayout,
40+
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
41+
camTransform);
42+
photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
43+
}
44+
45+
/**
46+
* The latest estimated robot pose on the field from vision data. This may be empty. This should
47+
* only be called once per loop.
48+
*
49+
* <p>
50+
* Also includes updates for the standard deviations, which can (optionally) be retrieved with
51+
* {@link getEstimationStdDevs}
52+
*
53+
* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
54+
* used for estimation.
55+
*/
56+
public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
57+
Optional<EstimatedRobotPose> visionEst = Optional.empty();
58+
for (var change : cam.getAllUnreadResults()) {
59+
visionEst = photonEstimator.update(change);
60+
updateEstimationStdDevs(visionEst, change.getTargets());
61+
}
62+
return visionEst;
63+
}
64+
/**
65+
* Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
66+
* deviations based on number of tags, estimation strategy, and distance from the tags.
67+
*
68+
* @param estimatedPose The estimated pose to guess standard deviations for.
69+
* @param targets All targets in this camera frame
70+
*/
71+
private void updateEstimationStdDevs(Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets){
72+
if (estimatedPose.isEmpty()){
73+
//No pose input. Default to single-tag std devs
74+
curStdDevs = singleTagStdDevs;
75+
76+
} else {
77+
//Pose present, start Heuristic
78+
var estStdDevs = singleTagStdDevs;
79+
int numTags = 0;
80+
double avgDist = 0;
81+
82+
//Precalculation - see how many tags we found, and calculate an average-distance metric
83+
for (var tgt : targets) {
84+
var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
85+
if (tagPose.isEmpty()){
86+
continue;
87+
}
88+
numTags++;
89+
avgDist += tagPose.get()
90+
.toPose2d()
91+
.getTranslation()
92+
.getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
93+
}
94+
95+
if (numTags == 0){
96+
//No tags visible. Default to single-tag std devs
97+
curStdDevs = singleTagStdDevs;
98+
} else {
99+
// One or more tags visible, run the full heuristic.
100+
avgDist /= numTags;
101+
// Decrease std devs if multiple targets are visible
102+
if (numTags > 1)
103+
estStdDevs = multiTagStdDevs;
104+
// Increase std devs based on (average) distance
105+
if (numTags == 1 && avgDist > 4) {
106+
estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
107+
} else {
108+
estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
109+
}
110+
curStdDevs = estStdDevs;
111+
}
112+
}
113+
}
114+
115+
/**
116+
* Returns the latest standard deviations of the estimated pose from {@link
117+
* #getEstimatedGlobalPose()}, for use with {@link
118+
* edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should
119+
* only be used when there are targets visible.
120+
*/
121+
public Matrix<N3, N1> getEstimationStdDevs() {
122+
return curStdDevs;
123+
}
124+
}

‎src/main/java/frc/robot/vision/VisionConfig.java‎

Lines changed: 25 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -2,55 +2,38 @@
22

33
import org.photonvision.PhotonCamera;
44

5+
import edu.wpi.first.apriltag.AprilTagFieldLayout;
56
import edu.wpi.first.apriltag.AprilTagFields;
7+
import edu.wpi.first.math.Matrix;
8+
import edu.wpi.first.math.VecBuilder;
69
import edu.wpi.first.math.geometry.Rotation3d;
710
import edu.wpi.first.math.geometry.Transform3d;
811
import edu.wpi.first.math.geometry.Translation3d;
12+
import edu.wpi.first.math.numbers.N1;
13+
import edu.wpi.first.math.numbers.N3;
914
import edu.wpi.first.math.util.Units;
1015

1116
public class VisionConfig {
1217

13-
//Creates camera names; ensure these all match with the correct camera on the Photonvison Dashboard
14-
public static final String centerPoseCamName = "Center_Pose_Cam";
15-
public static final String leftPoseCamName = "Left_Pose_Cam";
16-
public static final String rightPoseCamName = "Right_Pose_Cam";
17-
public static final String driveCamName = "Drive_Cam";
18-
19-
20-
//Creates all Photoncameras that will be used in pose estimator and commands
21-
public static PhotonCamera centerCam = new PhotonCamera(centerPoseCamName);
22-
public static PhotonCamera leftCam = new PhotonCamera(leftPoseCamName);
23-
public static PhotonCamera rightCam = new PhotonCamera(rightPoseCamName);
24-
public static PhotonCamera driveCam = new PhotonCamera(driveCamName);
25-
26-
//Creates field layout for AprilTags
27-
public static AprilTagFields aprilTagField = AprilTagFields.k2025Reefscape;
28-
29-
30-
//TODO: config
31-
public static final Transform3d robotToCenterCam =
32-
new Transform3d(
33-
new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)),
34-
new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0))
35-
);
36-
37-
public static final Transform3d robotToLeftCam =
38-
new Transform3d(
39-
new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)),
40-
new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0))
41-
);
42-
43-
public static final Transform3d robotToRightCam =
44-
new Transform3d(
45-
new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)),
46-
new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0))
47-
);
48-
49-
public static final Transform3d robotToDriveCam =
50-
new Transform3d(
51-
new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0)),
52-
new Rotation3d(Units.inchesToMeters(0), Units.inchesToMeters(0), Units.inchesToMeters(0))
53-
);
18+
// Creates camera names; ensure these all match with the correct camera on the Photonvison Dashboard
19+
public static final String[] camNames = new String[] {"Center_Cam", "Left_Cam", "Right_Cam", "Drive_Cam"};
20+
21+
//Camera Positions
22+
// TODO: config camera transforms
23+
public static final Transform3d[] robotToCamTransforms = new Transform3d[] {
24+
new Transform3d(new Translation3d(), new Rotation3d()),
25+
new Transform3d(new Translation3d(), new Rotation3d()),
26+
new Transform3d(new Translation3d(), new Rotation3d()),
27+
new Transform3d(new Translation3d(), new Rotation3d())
28+
};
29+
30+
// Creates field layout for AprilTags
31+
public static final AprilTagFieldLayout tagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
32+
33+
// Standard deviation of vision poses, this helps with correction or something idk thats what photon said
34+
// TODO: experiment with standard deviation values and set them to whatever gives the most correct pose
35+
public static final Matrix<N3, N1> singleTagStdDevs = VecBuilder.fill(4, 4, 8); // TODO: example values, change when testing
36+
public static final Matrix<N3, N1> multiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); //TODO: change values when testing
5437

5538

56-
}
39+
}

‎src/main/java/frc/robot/vision/VisionConsumer.java‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
package frc.robot.vision;
2+
3+
import edu.wpi.first.math.Matrix;
4+
import edu.wpi.first.math.geometry.Pose2d;
5+
import edu.wpi.first.math.numbers.N1;
6+
import edu.wpi.first.math.numbers.N3;
7+
8+
@FunctionalInterface
9+
public interface VisionConsumer {
10+
/**
11+
* Adds a vision measurement.
12+
* <p>
13+
* Note that the vision measurement standard deviations passed into this method
14+
* will continue to apply to future measurements.
15+
*
16+
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
17+
* @param timestampSeconds The timestamp of the vision measurement in seconds.
18+
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement in the form [x, y, theta]ᵀ, with
19+
* units in meters and radians.
20+
*/
21+
public void addVisionMeasurement(
22+
Pose2d visionRobotPoseMeters,
23+
double timestampSeconds,
24+
Matrix<N3, N1> visionMeasurementStdDevs);
25+
}

‎src/main/java/frc/robot/vision/VisionPoseEstimator.java‎

Lines changed: 0 additions & 7 deletions
This file was deleted.

‎src/main/java/frc/robot/vision/commands/PhotonEstimatorCommand.java‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
package frc.robot.vision.commands;
2+
3+
import static frc.robot.vision.VisionConfig.camNames;
4+
import static frc.robot.vision.VisionConfig.robotToCamTransforms;
5+
6+
import java.util.Arrays;
7+
8+
import edu.wpi.first.wpilibj2.command.Command;
9+
import frc.robot.vision.Vision;
10+
import frc.robot.vision.VisionConfig.*;
11+
import frc.robot.vision.VisionConsumer;
12+
13+
public class PhotonEstimatorCommand extends Command{
14+
private final Vision[] visions;
15+
private final VisionConsumer visionConsumer;
16+
17+
public PhotonEstimatorCommand(VisionConsumer consumer){
18+
visions = new Vision[camNames.length];
19+
for (int i = 0; i < camNames.length; i++){
20+
visions[i] = new Vision(camNames[i], robotToCamTransforms[i]);
21+
}
22+
this.visionConsumer = consumer;
23+
}
24+
25+
@Override
26+
public void execute(){
27+
Arrays.stream(visions).forEach(vision -> {
28+
var visionEst = vision.getEstimatedGlobalPose();
29+
30+
visionEst.ifPresent(est -> {
31+
var estStdDevs = vision.getEstimationStdDevs();
32+
33+
visionConsumer.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
34+
});
35+
});
36+
}
37+
38+
@Override
39+
public boolean runsWhenDisabled(){
40+
return true;
41+
}
42+
}

0 commit comments

Comments
 (0)