Skip to content

Commit 921462c

Browse files
mimizh2418mimizh2418
committed
feat: fuse vision poses with drivetrain poses
1 parent e786243 commit 921462c

File tree

5 files changed

+74
-5
lines changed

5 files changed

+74
-5
lines changed

‎src/main/java/org/team1540/robot2024/Constants.java

+16
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package org.team1540.robot2024;
22

3+
import edu.wpi.first.math.geometry.Pose3d;
34
import edu.wpi.first.math.geometry.Rotation2d;
45
import edu.wpi.first.math.util.Units;
56

@@ -62,6 +63,21 @@ public static class Drivetrain {
6263
public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
6364
}
6465

66+
public static class Vision {
67+
public static final String FRONT_CAMERA_NAME = "limelight-front";
68+
public static final String REAR_CAMERA_NAME = "limelight-rear";
69+
70+
// TODO: measure these offsets
71+
public static final Pose3d FRONT_CAMERA_POSE = new Pose3d();
72+
public static final Pose3d REAR_CAMERA_POSE = new Pose3d();
73+
74+
public static final double MAX_VISION_DELAY_SECS = 0.08;
75+
public static final double MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC = 1.0;
76+
public static final double MAX_ACCEPTED_LINEAR_SPEED_MPS = 4.0;
77+
public static final double MIN_ACCEPTED_NUM_TAGS = 1;
78+
public static final double MAX_ACCEPTED_AVG_TAG_DIST_METERS = 8.0;
79+
}
80+
6581
public static class Shooter {
6682
public static class Flywheels {
6783
// TODO: determine ids

‎src/main/java/org/team1540/robot2024/RobotContainer.java

+10
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,9 @@
1414
import org.team1540.robot2024.commands.SwerveDriveCommand;
1515
import org.team1540.robot2024.subsystems.drive.*;
1616
import org.team1540.robot2024.subsystems.shooter.*;
17+
import org.team1540.robot2024.subsystems.vision.AprilTagVision;
18+
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIO;
19+
import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight;
1720
import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
1821
import org.team1540.robot2024.util.swerve.SwerveFactory;
1922
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
@@ -30,6 +33,7 @@ public class RobotContainer {
3033
// Subsystems
3134
public final Drivetrain drivetrain;
3235
public final Shooter shooter;
36+
public final AprilTagVision aprilTagVision;
3337

3438
// Controller
3539
public final CommandXboxController driver = new CommandXboxController(0);
@@ -59,6 +63,10 @@ public RobotContainer() {
5963
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
6064
new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
6165
shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
66+
aprilTagVision = new AprilTagVision(
67+
new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
68+
new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
69+
drivetrain::addVisionMeasurement);
6270
break;
6371

6472
case SIM:
@@ -71,6 +79,7 @@ public RobotContainer() {
7179
new ModuleIOSim(),
7280
new ModuleIOSim());
7381
shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
82+
aprilTagVision = new AprilTagVision(new AprilTagVisionIO() {}, new AprilTagVisionIO() {}, (ignored) -> {});
7483
break;
7584

7685
default:
@@ -83,6 +92,7 @@ public RobotContainer() {
8392
new ModuleIO() {},
8493
new ModuleIO() {});
8594
shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {});
95+
aprilTagVision = new AprilTagVision(new AprilTagVisionIO() {}, new AprilTagVisionIO() {}, (ignored) -> {});
8696
break;
8797
}
8898

‎src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java

+8-3
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,8 @@
1919
import org.team1540.robot2024.util.LocalADStarAK;
2020
import org.littletonrobotics.junction.AutoLogOutput;
2121
import org.littletonrobotics.junction.Logger;
22+
import org.team1540.robot2024.util.vision.TimestampedVisionPose;
23+
import org.team1540.robot2024.util.vision.VisionPoseAcceptor;
2224

2325
import static org.team1540.robot2024.Constants.Drivetrain.*;
2426

@@ -52,7 +54,7 @@ public Drivetrain(
5254
new Pose2d(),
5355
// TODO: tune std devs (scale vision by distance?)
5456
VecBuilder.fill(0.1, 0.1, 0.1),
55-
VecBuilder.fill(0.5, 0.5, 0.5));
57+
VecBuilder.fill(0.5, 0.5, 5.0)); // Trust the gyro more than the AprilTags
5658

5759
// Configure AutoBuilder for PathPlanner
5860
AutoBuilder.configureHolonomic(
@@ -200,8 +202,11 @@ public void setPose(Pose2d pose) {
200202
poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose);
201203
}
202204

203-
public void addVisionMeasurement(Pose2d visionPose, double timestampSeconds) {
204-
poseEstimator.addVisionMeasurement(visionPose, timestampSeconds);
205+
public void addVisionMeasurement(TimestampedVisionPose visionPose) {
206+
if (visionPose == null) return;
207+
if (VisionPoseAcceptor.shouldAcceptVision(visionPose, kinematics.toChassisSpeeds(getModuleStates()))) {
208+
poseEstimator.addVisionMeasurement(visionPose.poseMeters(), visionPose.timestampSecs());
209+
}
205210
}
206211

207212
public SwerveModulePosition[] getModulePositions() {

‎src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java

+14-2
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
import org.team1540.robot2024.util.vision.TimestampedVisionPose;
88

99
import java.util.Arrays;
10+
import java.util.function.Consumer;
1011

1112
public class AprilTagVision extends SubsystemBase {
1213
private final AprilTagVisionIO frontCameraIO;
@@ -15,12 +16,18 @@ public class AprilTagVision extends SubsystemBase {
1516
private final AprilTagVisionIO rearCameraIO;
1617
private final AprilTagVisionIOInputsAutoLogged rearCameraInputs = new AprilTagVisionIOInputsAutoLogged();
1718

19+
private final Consumer<TimestampedVisionPose> visionPoseConsumer;
20+
1821
private TimestampedVisionPose frontPose;
1922
private TimestampedVisionPose rearPose;
2023

21-
public AprilTagVision(AprilTagVisionIO frontCameraIO, AprilTagVisionIO rearCameraIO) {
24+
public AprilTagVision(
25+
AprilTagVisionIO frontCameraIO,
26+
AprilTagVisionIO rearCameraIO,
27+
Consumer<TimestampedVisionPose> visionPoseConsumer) {
2228
this.frontCameraIO = frontCameraIO;
2329
this.rearCameraIO = rearCameraIO;
30+
this.visionPoseConsumer = visionPoseConsumer;
2431
}
2532

2633
@Override
@@ -44,10 +51,15 @@ public void periodic() {
4451
rearCameraInputs.seenTagIDs,
4552
rearPose.tagPosesMeters());
4653
}
54+
55+
TimestampedVisionPose latestPose = getEstimatedPose();
56+
Logger.recordOutput("Vision/EstimatedPose", latestPose.poseMeters());
57+
visionPoseConsumer.accept(latestPose);
4758
}
4859

4960
/**
50-
* Gets the estimated pose by fusing individual computed poses from each camera
61+
* Gets the estimated pose by fusing individual computed poses from each camera.
62+
* Returns null if no tags seen or in simulation
5163
*/
5264
public TimestampedVisionPose getEstimatedPose() {
5365
if (Constants.currentMode == Constants.Mode.SIM) return null;

‎src/main/java/org/team1540/robot2024/util/vision/VisionPoseAcceptor.java

+26
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
package org.team1540.robot2024.util.vision;
2+
3+
import edu.wpi.first.math.kinematics.ChassisSpeeds;
4+
import edu.wpi.first.wpilibj.Timer;
5+
6+
import static org.team1540.robot2024.Constants.Vision.*;
7+
8+
public class VisionPoseAcceptor {
9+
public static boolean shouldAcceptVision(TimestampedVisionPose visionPose, ChassisSpeeds robotVelocity) {
10+
// Do not accept poses that have too much delay
11+
if (Timer.getFPGATimestamp() - visionPose.timestampSecs() >= MAX_VISION_DELAY_SECS) return false;
12+
13+
// Do not accept poses that see too little tags
14+
if (visionPose.getNumTagsSeen() < MIN_ACCEPTED_NUM_TAGS) return false;
15+
16+
// Do not accept poses that have an average tag distance that is too far away
17+
if (visionPose.getAverageTagDistance() > MAX_ACCEPTED_AVG_TAG_DIST_METERS) return false;
18+
19+
// Do not accept poses taken when the robot has too much rotational or translational velocity
20+
boolean rotatingTooFast = robotVelocity.omegaRadiansPerSecond > MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC;
21+
boolean translatingTooFast =
22+
Math.hypot(robotVelocity.vxMetersPerSecond, robotVelocity.vyMetersPerSecond)
23+
> MAX_ACCEPTED_LINEAR_SPEED_MPS;
24+
return !rotatingTooFast && !translatingTooFast;
25+
}
26+
}

0 commit comments

Comments
 (0)