Ls vision simulation

src/main/java/frc/robot/RobotContainer.java

@@ -61,6 +61,8 @@
 //import frc.robot.subsystems.RollerSubsystem;
 //import frc.robot.subsystems.TiltSubsystem;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster;
+import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster;
 import frc.robot.utils.logging.io.gyro.RealGyroIo;
 import frc.robot.utils.logging.io.gyro.ThreadedGyro;
 import frc.robot.utils.logging.io.gyro.ThreadedGyroSwerveIMU;
@@ -79,6 +81,8 @@
 import choreo.auto.AutoRoutine;
 import choreo.auto.AutoTrajectory;
 
+import static frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator.visionMeasurementStdDevs2;
+
 /**
  * This class is where the bulk of the robot should be declared. Since
  * Command-based is a
@@ -116,6 +120,7 @@ public class RobotContainer {
         private AutoFactory autoFactory;
         private static AutoRoutine straightRoutine;
         private static AutoTrajectory straightTrajectory;
+        private final VisionTruster truster = new ConstantVisionTruster(visionMeasurementStdDevs2);
 
     // Replace with CommandPS4Controller or CommandJoystick if needed
         // new CommandXboxController(OperatorConstants.kDriverControllerPort);private
@@ -149,7 +154,7 @@ public RobotContainer() {
 
                                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(
                                                 new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null;
-                            apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase) : null;
+                            apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null;
                             controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
 
             }
@@ -167,7 +172,7 @@ public RobotContainer() {
                 // No GyroSubsystem in REPLAY for now
                 // create the drive subsystem with null gyro (use default json)
                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null;
-                apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase) : null;
+                apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase, truster) : null;
                 controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
 
             }
@@ -188,7 +193,7 @@ public RobotContainer() {
                 // create the drive subsystem with null gyro (use default json)
                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null;
                 controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
-                apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase) : null;
+                apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(truster,drivebase), drivebase, truster) : null;
             }
 
                         default -> {
@@ -512,7 +517,7 @@ public void putShuffleboardCommands() {
     public Command getAutonomousCommand() {
         return autoChooser.getCommand();
     // return straightRoutine.cmd(straightTrajectory.done());
-                
+
             //    return new ExampleAuto(drivebase, autoFactory);
         }
 
@@ -536,7 +541,7 @@ public SwerveSubsystem getDriveBase() {
         return drivebase;
     }
 
-        public ShootingState getShootingState() {
+    public ShootingState getShootingState() {
                 return shootState;
         }
 }

src/main/java/frc/robot/apriltags/SimApriltagIO.java

@@ -1,14 +1,63 @@
 package frc.robot.apriltags;
 
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.numbers.N3;
 import frc.robot.constants.Constants;
+import frc.robot.subsystems.ApriltagSubsystem;
+import frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator;
+import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter;
+import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement;
+import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster;
 import frc.robot.utils.Apriltag;
 import frc.robot.utils.logging.io.BaseIoImpl;
+
+import java.util.Optional;
 import java.util.Queue;
+import java.util.Random;
+import java.util.function.Supplier;
+
+import frc.robot.utils.math.ObjectUtils;
 import org.littletonrobotics.junction.Logger;
 
 public class SimApriltagIO extends TCPApriltagIo {
-    public SimApriltagIO(String name, ApriltagInputs inputs, SimTCPServer server) {
+    private final Random random = new Random();
+    private final VisionTruster truster;
+    private final Supplier<Optional<Pose2d>> robotPoseSupplier;
+    public SimApriltagIO(String name, ApriltagInputs inputs, SimTCPServer server, VisionTruster truster, Supplier<Optional<Pose2d>> robotPoseSupplier) {
         super(name, inputs, server);
+        this.truster = truster;
+        this.robotPoseSupplier = robotPoseSupplier;
+    }
+    public void simReadings() {
+        if (robotPoseSupplier.get().isPresent()) {
+            for (Apriltag tag : Apriltag.values()) {
+                Pose3d cameraPos = new Pose3d(robotPoseSupplier.get().get()).transformBy(Constants.ROBOT_TO_CAMERA);
+                if (ObjectUtils.canSee(tag.getPose(), cameraPos, Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) {
+                    Pose3d adjPose = tag.getPose().relativeTo(cameraPos);
+                    double cosIncidenceAngle = (-adjPose.getX() * Math.cos(adjPose.getRotation().getZ()) - adjPose.getY() * Math.sin(adjPose.getRotation().getZ())) / (adjPose.getTranslation().getNorm());
+                    double distance = tag.getTranslation().getDistance(cameraPos.getTranslation());
+                    if (distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) {
+                        VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0);
+                        Vector<N3> stdDevs = truster.calculateTrust(measurement);
+                        double readingX = robotPoseSupplier.get().get().getX() + random.nextGaussian() * stdDevs.get(0);
+                        double readingY = robotPoseSupplier.get().get().getY() + random.nextGaussian() * stdDevs.get(1);
+                        double readingYaw = robotPoseSupplier.get().get().getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2);
+                        Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw));
+                        distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation());
+                        if (BasicVisionFilter.inBounds(readingPos)) {
+                            addReading(new ApriltagReading(readingX, readingY, readingYaw,
+                                    distance, tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, Logger.getTimestamp() / 1000.0));
+                        }
+                    }
+                }
+            }
+        }
+    }
+    @Override
+    public void periodic() {
+        updateInputs(inputs);
+        Logger.processInputs(prefix, inputs);
+        simReadings();
     }
 }

src/main/java/frc/robot/commands/drive/DriveCircle.java

@@ -0,0 +1,44 @@
+package frc.robot.commands.drive;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+public class DriveCircle extends LoggableCommand {
+    private final SwerveSubsystem drivebase;
+    private final double radiusMeters;
+    private final double angularVelocityRadPerSec;
+    private final double time;
+    private final Timer timer;
+    public DriveCircle(SwerveSubsystem drivebase, double radiusMeters, double angularVelocityRadPerSec, double time) {
+        this.drivebase = drivebase;
+        this.radiusMeters = radiusMeters;
+        this.angularVelocityRadPerSec = angularVelocityRadPerSec;
+        this.time = time;
+        this.timer = new Timer();
+        addRequirements(drivebase);
+    }
+
+    @Override
+    public void initialize() {
+        timer.restart();
+    }
+
+    @Override
+    public void execute() {
+        double tangentialSpeed = radiusMeters * angularVelocityRadPerSec;
+        double yVelocity = -tangentialSpeed;
+        drivebase.drive(new Translation2d(0, yVelocity), angularVelocityRadPerSec, false);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return timer.hasElapsed(time);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        drivebase.drive(new Translation2d(), 0, false);
+    }
+}

src/main/java/frc/robot/constants/GameConstants.java

@@ -1,6 +1,8 @@
 package frc.robot.constants;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
 import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
@@ -105,7 +107,7 @@ public enum Mode {
     public static final double ANGLER_FIXED_ANGLE = 10; //Fixed encoder position of Angler in Fixed ShootState
     public static final double ANGLER_LIMIT_SPEED = 0.2;
 
-    
+
  // turret (pan angle) PID
     public static final double TURRET_P = .5;
     public static final double TURRET_I = 0.000001;
@@ -149,4 +151,12 @@ public enum Mode {
     public static final double VISION_CONSISTENCY_THRESHOLD = 0.25; //How close 2 vision measurements have to be (needs to be tuned potentially but seemingly from my testing it also might not be needed)
     public static final boolean ENABLE_VISION = true;
     public static final double POSE_BUFFER_STORAGE_TIME = 2; //how many past measurements are stored in the buffer (might increase if we need further back)
+
+    // Vision
+    public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(0,0,0, new Rotation3d(0,0,0)); // TODO: change
+    public static final double HORIZONTAL_FOV = Units.degreesToRadians(110); // radians; TODO: Change Later
+    public static final double VERTICAL_FOV = Units.degreesToRadians(90); // radians; TODO: Change Later
+    public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later
+    public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later
+    public static final double MAX_VISION_DISTANCE_SIMULATION = 6;
 }

src/main/java/frc/robot/subsystems/ApriltagSubsystem.java

@@ -1,12 +1,26 @@
 package frc.robot.subsystems;
 
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.apriltags.*;
+import frc.robot.constants.Constants;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
 import frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator;
+import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter;
+import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement;
+import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster;
+import frc.robot.utils.Apriltag;
 import frc.robot.utils.logging.io.BaseIoImpl;
+import frc.robot.utils.math.ObjectUtils;
+import org.littletonrobotics.junction.Logger;
+
+import java.util.Random;
+import java.util.function.Supplier;
 
 public class ApriltagSubsystem extends SubsystemBase {
 
@@ -15,10 +29,11 @@ public class ApriltagSubsystem extends SubsystemBase {
     private final PoseEstimator estimator;
     private final SwerveSubsystem drivebase;
 
-    public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase) {
+
+    public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase, VisionTruster truster) {
         this.drivebase = drivebase;
         this.io = io;
-       estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this);
+        estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this, truster);
     }
 
     public static ApriltagIO createRealIo() {
@@ -30,8 +45,8 @@ public static ApriltagIO createMockIo() {
         return new MockApriltagIo(LOGGING_NAME, new ApriltagInputs());
     }
 
-    public static ApriltagIO createSimIo() {
-        return new SimApriltagIO(LOGGING_NAME, new ApriltagInputs(), new SimTCPServer(0)); // port doesnt matter at all
+    public static ApriltagIO createSimIo(VisionTruster truster, SwerveSubsystem drivebase) {
+        return new SimApriltagIO(LOGGING_NAME, new ApriltagInputs(), new SimTCPServer(0), truster, drivebase::getSimulationPose); // port doesnt matter at all
     }
     // This is used to inject april tag readings manually and will pretty much only be used for simulation.
     public void addSimReading(ApriltagReading reading) {
@@ -40,11 +55,16 @@ public void addSimReading(ApriltagReading reading) {
     public ApriltagIO getIO(){
         return io;
     }
-
+    public VisionTruster getVisionTruster() {
+        return estimator.getVisionTruster();
+    }
     @Override
     public void periodic() {
         estimator.updateVision();
         estimator.updatePosition(drivebase.getOdom());
         io.periodic();
     }
+    public Vector<N3> calculateTrust(VisionMeasurement measurement) {
+        return estimator.getVisionTruster().calculateTrust(measurement);
+    }
 }

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

@@ -37,6 +37,7 @@
 
 import java.io.File;
 import java.util.Arrays;
+import java.util.Optional;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
@@ -297,6 +298,9 @@ public void resetOdometry(Pose2d initialHolonomicPose) {
     public Pose2d getPose() {
         return swerveDrive.getPose();
     }
+    public Optional<Pose2d> getSimulationPose() {
+        return swerveDrive.getSimulationDriveTrainPose();
+    }
     // Todo: fix to only get odomtry
     public Pose2d getOdom() {
         return swerveDrive.getPose();

src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java

@@ -19,6 +19,7 @@
 import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter;
 import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster;
 import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement;
+import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster;
 import frc.robot.utils.Apriltag;
 import frc.robot.utils.math.ArrayUtils;
 
@@ -61,7 +62,8 @@ public PoseEstimator(
       SwerveDriveKinematics kinematics,
       SwerveSubsystem drivebase,
       double initGyroValueDeg,
-      ApriltagSubsystem apriltagSystem) {
+      ApriltagSubsystem apriltagSystem,
+      VisionTruster truster) {
    /*this.frontLeft = frontLeftMotor;
     this.frontRight = frontRightMotor;
     this.backLeft = backLeftMotor;
@@ -89,8 +91,7 @@ public PoseEstimator(
               public Pose2d getVisionPose(VisionMeasurement measurement) {
                 return measurement.measurement();
               }
-            },
-            new ConstantVisionTruster(visionMeasurementStdDevs2));
+            }, truster);
     SmartDashboard.putData(field);
   }
 
@@ -144,7 +145,7 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) {
     double serverTime = apriltagSystem.getIO().getInputs().serverTime[index];
     //double timestamp = 0; // latency is not right we are assuming zero
     double timestamp = apriltagSystem.getIO().getInputs().timestamp[index];
-    Pose2d visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation());
+    Pose2d visionPose = new Pose2d(pos[0], pos[1], Rotation2d.fromDegrees(pos[2]));
     double distanceFromTag = apriltagSystem.getIO().getInputs().distanceToTag[index];
     return new VisionMeasurement(visionPose, distanceFromTag, timestamp/1000);
   }
@@ -202,4 +203,7 @@ public void addMockVisionMeasurement() {
     poseManager.registerVisionMeasurement(
         new VisionMeasurement(getEstimatedPose(), 0, Logger.getTimestamp() / 1e6));
   }
+    public VisionTruster getVisionTruster() {
+        return poseManager.getVisionTruster();
+    }
 }

src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java

@@ -79,7 +79,7 @@ private boolean filterVision(Pose2d m1Pose, Pose2d m2Pose, double m1Time, double
     return Math.abs(diff) <= Constants.VISION_CONSISTENCY_THRESHOLD;
   }
 
-  private boolean inBounds(Pose2d pose2d) {
-    return pose2d.getX() > 0 && pose2d.getX() < 20 && pose2d.getY() > 0 && pose2d.getY() < 20;
+  public static boolean inBounds(Pose2d pose2d) {
+    return pose2d.getX() > 0 && pose2d.getX() < 16.5 && pose2d.getY() > 0 && pose2d.getY() < 8.1;
   }
 }