Skip to content

Commit aecd526

Browse files
Etaash-mathamsettyEtaash-mathamsetty
committed
Vision configurator
Cook of the century
1 parent 073aac6 commit aecd526

File tree

6 files changed

+191
-59
lines changed

6 files changed

+191
-59
lines changed

‎src/main/java/team1403/robot/swerve/SwerveSubsystem.java‎

Lines changed: 40 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,6 @@
55
import java.util.ArrayList;
66
import java.util.function.Supplier;
77

8-
import org.littletonrobotics.junction.Logger;
9-
108
import com.ctre.phoenix6.SignalLogger;
119
import com.ctre.phoenix6.Utils;
1210
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
@@ -21,13 +19,10 @@
2119
import com.pathplanner.lib.pathfinding.LocalADStar;
2220
import com.pathplanner.lib.pathfinding.Pathfinding;
2321
import com.pathplanner.lib.util.DriveFeedforwards;
24-
import com.pathplanner.lib.util.PathPlannerLogging;
25-
2622
import edu.wpi.first.math.Matrix;
23+
import edu.wpi.first.math.VecBuilder;
2724
import edu.wpi.first.math.geometry.Pose2d;
28-
import edu.wpi.first.math.geometry.Pose3d;
2925
import edu.wpi.first.math.geometry.Rotation2d;
30-
import edu.wpi.first.math.geometry.Rotation3d;
3126
import edu.wpi.first.math.geometry.Translation2d;
3227
import edu.wpi.first.math.kinematics.ChassisSpeeds;
3328
import edu.wpi.first.math.kinematics.SwerveModuleState;
@@ -41,7 +36,6 @@
4136
import edu.wpi.first.wpilibj.Notifier;
4237
import edu.wpi.first.wpilibj.RobotController;
4338
import edu.wpi.first.wpilibj.Alert.AlertType;
44-
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
4539
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
4640
import edu.wpi.first.wpilibj2.command.Command;
4741
import edu.wpi.first.wpilibj2.command.Commands;
@@ -55,6 +49,7 @@
5549
import team1403.robot.vision.AprilTagCamera;
5650
import team1403.robot.vision.ITagCamera;
5751
import team1403.robot.vision.LimelightWrapper;
52+
import team1403.robot.vision.VisionConfigurator;
5853
import team1403.robot.vision.VisionSimUtil;
5954
import team1403.robot.vision.ITagCamera.VisionData;
6055

@@ -174,18 +169,45 @@ private void onConstruct() {
174169
).schedule();
175170

176171
VisionSimUtil.initVisionSim();
177-
m_cameras.add(new LimelightWrapper("limelight",
178-
() -> Constants.Vision.kLimelightTransform,
179-
() -> new Rotation3d(getRotation())));
180-
m_cameras.add(new LimelightWrapper("limelight-twoplus",
181-
() -> Constants.Vision.kLimelight2Transform,
182-
() -> new Rotation3d(getRotation())));
172+
173+
VisionConfigurator config = new VisionConfigurator()
174+
.withRobotPose(this::getPose, () -> m_state.Timestamp);
175+
176+
if (Robot.isReal())
177+
{
178+
m_cameras.add(new LimelightWrapper(config
179+
.withName("limelight")
180+
.withTransform(() -> Constants.Vision.kLimelightTransform)
181+
.withDeviations(VecBuilder.fill(2, 2, 3))
182+
.withDeviationsTrig(VecBuilder.fill(2, 2, Double.POSITIVE_INFINITY))
183+
.withTrigSolve(true)
184+
));
185+
m_cameras.add(new LimelightWrapper(config
186+
.withName("limelight-twoplus")
187+
.withTransform(() -> Constants.Vision.kLimelight2Transform)
188+
.withDeviations(VecBuilder.fill(2, 2, 3))
189+
.withDeviationsTrig(VecBuilder.fill(1, 1, Double.POSITIVE_INFINITY))
190+
.withTrigSolve(true)
191+
));
192+
}
183193
//test camera for simulation
184-
if(Robot.isSimulation())
185-
m_cameras.add(new AprilTagCamera("simlimelight",
186-
() -> Constants.Vision.kLimelightTransform,
187-
() -> m_state.Timestamp,
188-
() -> getPose()));
194+
else
195+
{
196+
m_cameras.add(new AprilTagCamera(config
197+
.withName("simlimelight")
198+
.withTransform(() -> Constants.Vision.kLimelightTransform)
199+
.withDeviations(VecBuilder.fill(2, 2, 3))
200+
.withDeviationsTrig(VecBuilder.fill(1, 1, Double.POSITIVE_INFINITY))
201+
.withTrigSolve(false) // FIXME: implement it into photon code
202+
));
203+
m_cameras.add(new AprilTagCamera(config
204+
.withName("simlimelight-2")
205+
.withTransform(() -> Constants.Vision.kLimelight2Transform)
206+
.withDeviations(VecBuilder.fill(2, 2, 3))
207+
.withDeviationsTrig(VecBuilder.fill(1, 1, Double.POSITIVE_INFINITY))
208+
.withTrigSolve(false) // FIXME: implement it into photon code
209+
));
210+
}
189211

190212
SmartDashboard.putData("Gyro", super.getPigeon2());
191213

‎src/main/java/team1403/robot/swerve/Telemetry.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,11 +29,11 @@ public Telemetry(double maxSpeed) {
2929
SignalLogger.start();
3030

3131
PathPlannerLogging.setLogActivePathCallback((activePath) -> {
32-
Logger.recordOutput("Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
32+
Logger.recordOutput("Swerve/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
3333
m_field.getObject("traj").setPoses(activePath);
3434
});
3535
PathPlannerLogging.setLogTargetPoseCallback((targetPose) -> {
36-
Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
36+
Logger.recordOutput("Swerve/TrajectorySetpoint", targetPose);
3737
});
3838

3939
SmartDashboard.putData("Field", m_field);

‎src/main/java/team1403/robot/vision/AprilTagCamera.java‎

Lines changed: 19 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -42,15 +42,17 @@ public class AprilTagCamera extends SubsystemBase implements ITagCamera {
4242
private final Supplier<Pose2d> m_referencePose;
4343
private final DoubleSupplier m_poseTimestamp;
4444
private final Alert m_cameraAlert;
45-
private static final Matrix<N3, N1> kDefaultStdv = VecBuilder.fill(2, 2, 3);
45+
private final Matrix<N3, N1> kDefaultStdv;
46+
private final Matrix<N3, N1> kDefaultStdvTrig;
47+
private final boolean kTrigSolveEnabled;
4648

4749
private final VisionData m_returnedData = new VisionData();
4850

49-
public AprilTagCamera(String cameraName, Supplier<Transform3d> cameraTransform, DoubleSupplier poseTimestamp, Supplier<Pose2d> referenceSupplier) {
51+
public AprilTagCamera(VisionConfigurator config) {
5052
// Photonvision
5153
// PortForwarder.add(5800,
5254
// "photonvision.local", 5800);
53-
m_camera = new PhotonCamera(cameraName);
55+
m_camera = new PhotonCamera(config.getName());
5456

5557
if (Robot.isSimulation()) {
5658
SimCameraProperties cameraProp = new SimCameraProperties();
@@ -67,21 +69,24 @@ public AprilTagCamera(String cameraName, Supplier<Transform3d> cameraTransform,
6769

6870
m_cameraSim = new PhotonCameraSim(m_camera, cameraProp);
6971

70-
VisionSimUtil.addCamera(m_cameraSim, cameraTransform.get());
72+
VisionSimUtil.addCamera(m_cameraSim, config.getTransform3d().get());
7173
} else {
7274
m_cameraSim = null;
7375
}
7476
m_camera.setPipelineIndex(0);
7577

76-
m_poseEstimator = new PhotonPoseEstimator(Constants.Vision.kFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraTransform.get());
78+
m_poseEstimator = new PhotonPoseEstimator(Constants.Vision.kFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, config.getTransform3d().get());
7779
m_poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.AVERAGE_BEST_TARGETS);
7880

7981
m_estPos = null;
80-
m_referencePose = referenceSupplier;
81-
m_cameraTransform = cameraTransform;
82-
m_poseTimestamp = poseTimestamp;
83-
84-
m_cameraAlert = new Alert("Photon Camera " + cameraName + " Disconnected!", AlertType.kError);
82+
m_referencePose = config.getRobotPose();
83+
m_cameraTransform = config.getTransform3d();
84+
m_poseTimestamp = config.getPoseTimestamp();
85+
kDefaultStdv = config.getDeviations();
86+
kDefaultStdvTrig = config.getDeviationsTrig();
87+
kTrigSolveEnabled = config.getTrigSolveEnabled();
88+
89+
m_cameraAlert = new Alert("Photon Camera " + m_camera.getName() + " Disconnected!", AlertType.kError);
8590
}
8691

8792
@Override
@@ -166,9 +171,11 @@ public void refreshEstimate(Consumer<VisionData> data) {
166171
@Override
167172
public void periodic() {
168173

169-
m_poseEstimator.setReferencePose(m_referencePose.get());
174+
if (m_referencePose != null)
175+
m_poseEstimator.setReferencePose(m_referencePose.get());
170176
//think about if we want to use the trig solver or constrained solve pnp
171-
m_poseEstimator.addHeadingData(m_poseTimestamp.getAsDouble(), m_referencePose.get().getRotation());
177+
if (kTrigSolveEnabled)
178+
m_poseEstimator.addHeadingData(m_poseTimestamp.getAsDouble(), m_referencePose.get().getRotation());
172179
m_poseEstimator.setRobotToCameraTransform(m_cameraTransform.get());
173180

174181
if(m_cameraSim != null) {

‎src/main/java/team1403/robot/vision/LimelightWrapper.java‎

Lines changed: 17 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
import org.littletonrobotics.junction.Logger;
1212
import edu.wpi.first.math.Matrix;
1313
import edu.wpi.first.math.VecBuilder;
14+
import edu.wpi.first.math.geometry.Pose2d;
1415
import edu.wpi.first.math.geometry.Pose3d;
1516
import edu.wpi.first.math.geometry.Rotation3d;
1617
import edu.wpi.first.math.geometry.Transform3d;
@@ -27,21 +28,25 @@
2728

2829
public class LimelightWrapper extends SubsystemBase implements ITagCamera {
2930
private final String m_name;
30-
private final Supplier<Rotation3d> m_imuRotation;
31+
private final Supplier<Pose2d> m_robotPose;
3132
private final Supplier<Transform3d> m_camTransform;
3233
private LimelightHelpers.PoseEstimate m_poseEstimateMT1;
3334
private LimelightHelpers.PoseEstimate m_poseEstimateMT2;
34-
private final static Matrix<N3, N1> kDefaultStdv = VecBuilder.fill(2, 2, 3); //TODO: adjust this
35-
private final static Matrix<N3, N1> kDefaultStdvMT2 = VecBuilder.fill(2, 2, Double.POSITIVE_INFINITY);
35+
private final boolean kMT2Enabled;
36+
private final Matrix<N3, N1> kDefaultStdv;
37+
private final Matrix<N3, N1> kDefaultStdvMT2;
3638
private final Alert m_camDisconnected;
3739
private final DoubleSubscriber m_latencySubscriber;
3840

3941
private final VisionData m_dataReturned = new VisionData(); //data we return to swerve subsystem
4042

41-
public LimelightWrapper(String name, Supplier<Transform3d> cameraTransform, Supplier<Rotation3d> imuRotation) {
42-
m_name = name.toLowerCase(); //hostname must be lowercase
43-
m_imuRotation = imuRotation;
44-
m_camTransform = cameraTransform;
43+
public LimelightWrapper(VisionConfigurator config) {
44+
m_name = config.getName().toLowerCase(); //hostname must be lowercase
45+
m_robotPose = config.getRobotPose();
46+
m_camTransform = config.getTransform3d();
47+
kDefaultStdv = config.getDeviations();
48+
kDefaultStdvMT2 = config.getDeviationsTrig();
49+
kMT2Enabled = config.getTrigSolveEnabled();
4550
m_poseEstimateMT1 = null;
4651
m_poseEstimateMT2 = null;
4752

@@ -131,11 +136,13 @@ public void refreshEstimate(Consumer<VisionData> data) {
131136
private final ArrayList<Pose3d> targets = new ArrayList<>();
132137

133138
@Override
134-
public void periodic() {
135-
LimelightHelpers.SetRobotOrientation(m_name, m_imuRotation.get());
139+
public void periodic() {
140+
if (kMT2Enabled) LimelightHelpers.SetRobotOrientation(m_name,
141+
new Rotation3d(m_robotPose.get().getRotation()));
136142
LimelightHelpers.setCameraPose_RobotSpace(m_name, m_camTransform.get());
137143
m_poseEstimateMT1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(m_name);
138-
m_poseEstimateMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(m_name);
144+
if (kMT2Enabled) m_poseEstimateMT2 =
145+
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(m_name);
139146

140147
m_camDisconnected.set(!isConnected());
141148

‎src/main/java/team1403/robot/vision/VisionConfigurator.java‎

Lines changed: 113 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,113 @@
1+
package team1403.robot.vision;
2+
3+
import java.util.function.DoubleSupplier;
4+
import java.util.function.Supplier;
5+
6+
import edu.wpi.first.math.Matrix;
7+
import edu.wpi.first.math.VecBuilder;
8+
import edu.wpi.first.math.geometry.Pose2d;
9+
import edu.wpi.first.math.geometry.Rotation2d;
10+
import edu.wpi.first.math.geometry.Rotation3d;
11+
import edu.wpi.first.math.geometry.Transform3d;
12+
import edu.wpi.first.math.numbers.N1;
13+
import edu.wpi.first.math.numbers.N3;
14+
15+
public class VisionConfigurator {
16+
private String m_name;
17+
private Supplier<Transform3d> m_cameraTransform = () -> Transform3d.kZero;
18+
private Supplier<Pose2d> m_robotPose = null;
19+
private DoubleSupplier m_poseTimestamp = null;
20+
private DoubleSupplier m_yawRate = () -> 0.0;
21+
22+
// Default deviations for normal april tag detection
23+
private Matrix<N3, N1> m_deviations = VecBuilder.fill(2, 2, 3);
24+
25+
// Deviations for trig solve (make sure to set third element to infinity)
26+
private Matrix<N3, N1> m_deviationsTrig = VecBuilder.fill(1, 1, Double.POSITIVE_INFINITY);
27+
28+
// Should we even do trig solve ?
29+
// Requirements: Set Robot Rotation Supplier and Trig Deviations
30+
private boolean m_enableTrigSolve = false;
31+
32+
public VisionConfigurator() {}
33+
34+
/* Configuration chaining methods */
35+
36+
// In case we want to chnage name after init (for a second camera perhaps)
37+
public VisionConfigurator withName(String name)
38+
{
39+
m_name = name;
40+
return this;
41+
}
42+
43+
public VisionConfigurator withTrigSolve(boolean enabled)
44+
{
45+
m_enableTrigSolve = enabled;
46+
return this;
47+
}
48+
49+
public VisionConfigurator withDeviations(Matrix<N3, N1> dev)
50+
{
51+
m_deviations = dev;
52+
return this;
53+
}
54+
55+
public VisionConfigurator withDeviationsTrig(Matrix<N3, N1> trigDev)
56+
{
57+
m_deviationsTrig = trigDev;
58+
return this;
59+
}
60+
61+
public VisionConfigurator withTransform(Supplier<Transform3d> transform)
62+
{
63+
m_cameraTransform = transform;
64+
return this;
65+
}
66+
67+
public VisionConfigurator withRobotPose(Supplier<Pose2d> pose, DoubleSupplier pose_timestamp)
68+
{
69+
m_robotPose = pose;
70+
m_poseTimestamp = pose_timestamp;
71+
return this;
72+
}
73+
74+
public VisionConfigurator withYawRate(DoubleSupplier rate)
75+
{
76+
m_yawRate = rate;
77+
return this;
78+
}
79+
80+
/* Getter methods */
81+
82+
public boolean getTrigSolveEnabled() {
83+
return m_enableTrigSolve && m_robotPose != null && m_poseTimestamp != null;
84+
}
85+
86+
public String getName() {
87+
return m_name;
88+
}
89+
90+
public Supplier<Transform3d> getTransform3d() {
91+
return m_cameraTransform;
92+
}
93+
94+
public Supplier<Pose2d> getRobotPose() {
95+
return m_robotPose;
96+
}
97+
98+
public DoubleSupplier getPoseTimestamp() {
99+
return m_poseTimestamp;
100+
}
101+
102+
public DoubleSupplier getYawRate() {
103+
return m_yawRate;
104+
}
105+
106+
public Matrix<N3, N1> getDeviations() {
107+
return m_deviations;
108+
}
109+
110+
public Matrix<N3, N1> getDeviationsTrig() {
111+
return m_deviationsTrig;
112+
}
113+
}

‎src/main/java/team1403/robot/vision/VisionUtil.java‎

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

0 commit comments

Comments
 (0)