vision logging

src/main/java/frc/robot/Robot.java

@@ -4,6 +4,7 @@
 
 package frc.robot;
 
+
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.UsbCamera;
 import edu.wpi.first.cscore.VideoSource;
@@ -138,11 +139,11 @@ public void robotPeriodic() {
                 // Puts data on the elastic dashboard
                 SmartDashboard.putString("Alliance Color", Robot.allianceColorString());
                 SmartDashboard.putBoolean("Hub Active?", hubActive());
+                if (Constants.currentMode == Constants.Mode.SIM) {
+                    Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose().get());
+                    Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getOdom());
+                }
             }
-
-            // Puts data on the elastic dashboard
-            SmartDashboard.putString("Alliance Color", Robot.allianceColorString());
-            SmartDashboard.putBoolean("Hub Active?", hubActive());
         }
         SmartDashboard.putString("Selected Action",
                 robotContainer.getAutoChooser().getCommandDescription());

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

@@ -49,7 +49,7 @@ public void simReadings() {
                         distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation());
                         if (BasicVisionFilter.inBounds(readingPos)) {
                             addReading(new ApriltagReading(readingX, readingY, readingYaw,
-                                    distance, 0, tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, 0, Logger.getTimestamp() / 1000.0));
+                                    distance,0,tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, stdDevs.get(0),Logger.getTimestamp() / 1000.0));
                         }
                     }
                 }

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

@@ -36,7 +36,7 @@ public enum Mode {
     public static final boolean ENABLE_LOGGING = true;
 
     //Debugs
-    public static final boolean DEBUG = false;
+    public static final boolean DEBUG = true;
     public static final boolean ARM_DEBUG = true;
 
     //Joystick

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

@@ -13,16 +13,21 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import frc.robot.constants.Constants;
 
+import frc.robot.constants.GameConstants;
 import frc.robot.utils.logging.io.gyro.ThreadedGyro;
+import org.littletonrobotics.junction.Logger;
 import swervelib.SwerveController;
 import swervelib.SwerveDrive;
 import swervelib.SwerveDriveTest;
@@ -38,6 +43,8 @@
 import java.io.File;
 import java.util.Arrays;
 import java.util.Optional;
+import java.util.concurrent.ConcurrentLinkedDeque;
+import java.util.Optional;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
@@ -49,6 +56,10 @@ public class SwerveSubsystem extends SubsystemBase {
      */
     private final SwerveDrive swerveDrive;
     private Vector<N3> variance = VecBuilder.fill(0.1,0.1,0.1);
+    private SwerveDriveOdometry rawOdometry;
+    private final ConcurrentLinkedDeque<PoseErrorRecord> poseError = new ConcurrentLinkedDeque<>();
+    private record PoseErrorRecord(double timestamp, double error) {
+    }
     /**
      * Initialize {@link SwerveDrive} with the directory provided.
      * The SwerveIMU (which can be null) is the instance of the SwerveIMU to use. If non-null,
@@ -83,7 +94,14 @@ public SwerveSubsystem(File directory, SwerveIMU swerveIMU) {
         swerveDrive.setModuleEncoderAutoSynchronize(Constants.SET_MODULE_ENCODER_AUTO_SYNCHRONIZE,
                 Constants.SET_MODULE_ENCODER_AUTO_SYNCHRONIZE_DEADBAND); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving.
         // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible
-
+        if (Constants.currentMode==Constants.simMode) {
+            rawOdometry = new SwerveDriveOdometry(
+                swerveDrive.kinematics,
+                swerveDrive.getOdometryHeading(),
+                swerveDrive.getModulePositions(),
+                startingPose);
+        }
+
     }
 
     /**
@@ -105,6 +123,17 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig
     public void periodic() {
         //add vision pose here
         //addVisionMeasurement(new Pose2d(new Translation2d(16, 2), new Rotation2d()));
+        if (Constants.currentMode == GameConstants.Mode.SIM) {
+            double currentTime = Logger.getTimestamp() / 1000000.0;
+            double oneSecondAgo = currentTime - 1.0;
+            poseError.removeIf(record -> record.timestamp < oneSecondAgo);
+            poseError.add(new PoseErrorRecord(currentTime, getError()));
+            Logger.recordOutput("AveragePoseError", getAverageError());
+            rawOdometry.update(
+                swerveDrive.getOdometryHeading(),
+                swerveDrive.getModulePositions()
+        );
+        }
     }
 
     @Override
@@ -282,8 +311,16 @@ public SwerveDriveKinematics getKinematics() {
      *
      * @param initialHolonomicPose The pose to set the odometry to
      */
+    // might be broken
     public void resetOdometry(Pose2d initialHolonomicPose) {
         swerveDrive.resetOdometry(initialHolonomicPose);
+        if (Constants.currentMode==Constants.simMode) {
+            SwerveModulePosition[] modules = new SwerveModulePosition[4];
+            for (int i=0; i<4; i++) {
+                modules[i] = new SwerveModulePosition();
+            }
+            rawOdometry.resetPosition(initialHolonomicPose.getRotation(), modules, initialHolonomicPose);
+        }
     }
 
     /**
@@ -294,13 +331,23 @@ public void resetOdometry(Pose2d initialHolonomicPose) {
     public Pose2d getPose() {
         return swerveDrive.getPose();
     }
+    public double getError() {
+        return getPose().getTranslation().getDistance((getSimulationPose().get().getTranslation()));
+    }
+    public double getAverageError(){
+        return poseError.stream().mapToDouble(record -> record.error).average().orElse(0);
+    }
 
     public Optional<Pose2d> getSimulationPose() {
         return swerveDrive.getSimulationDriveTrainPose();
     }
     // Todo: fix to only get odomtry
     public Pose2d getOdom() {
-        return swerveDrive.getPose();
+        if (Constants.currentMode==Constants.simMode) {
+            return rawOdometry.getPoseMeters();
+        } else {
+            return getPose();
+        }
     }
 
     /**

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

@@ -2,20 +2,22 @@
 
 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.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.numbers.N3;
+import frc.robot.constants.GameConstants;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
 import frc.robot.subsystems.swervedrive.vision.truster.FilterResult;
 import frc.robot.subsystems.swervedrive.vision.truster.PoseDeviation;
 import frc.robot.subsystems.swervedrive.vision.truster.VisionFilter;
 import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement;
 import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster;
 
-import java.util.ArrayList;
-import java.util.LinkedHashMap;
-import java.util.List;
-import java.util.Map;
+import java.util.*;
+import java.util.concurrent.ConcurrentLinkedDeque;
+
+import frc.robot.utils.Apriltag;
 import org.littletonrobotics.junction.Logger;
 
 /**
@@ -53,29 +55,32 @@ public FilterablePoseManager(
 
   @Override
   public void processQueue() {
+    List<Pose2d> validMeasurementsPose = new ArrayList<>();
+    List<Pose2d> invalidMeasurementsPose = new ArrayList<>();
+
     LinkedHashMap<VisionMeasurement, FilterResult> filteredData =
-        filter.filter(visionMeasurementQueue);
+              filter.filter(visionMeasurementQueue);
     visionMeasurementQueue.clear();
-    List<Pose2d> validMeasurements = new ArrayList<>();
-    List<Pose2d> invalidMeasurements = new ArrayList<>();
-    for (Map.Entry<VisionMeasurement, FilterResult> entry : filteredData.entrySet()) {
-      VisionMeasurement v = entry.getKey();
-      FilterResult r = entry.getValue();
+    for (Map.Entry<VisionMeasurement, FilterResult> filterEntry : filteredData.entrySet()) {
+      VisionMeasurement v = filterEntry.getKey();
+      FilterResult r = filterEntry.getValue();
       switch (r) {
         case ACCEPTED -> {
           setVisionSTD(visionTruster.calculateTrust(v));
-          validMeasurements.add(v.measurement());
+          validMeasurementsPose.add(v.measurement());
           addVisionMeasurement(v);
+
         }
         case NOT_PROCESSED -> visionMeasurementQueue.add(v);
         case REJECTED -> {
-          invalidMeasurements.add(v.measurement());
+          invalidMeasurementsPose.add(v.measurement());
         }
       }
     }
-    Logger.recordOutput("Apriltag/acceptedMeasurements", validMeasurements.toArray(Pose2d[]::new));
-    Logger.recordOutput(
-        "Apriltag/rejectedMeasurements", invalidMeasurements.toArray(Pose2d[]::new));
+//    Logger.recordOutput("Apriltag/acceptedMeasurementsPose", validMeasurementsPose.toArray(Pose2d[]::new));
+//    Logger.recordOutput("Apriltag/rejectedMeasurementsPose", invalidMeasurementsPose.toArray(Pose2d[]::new));
+    Logger.recordOutput("Apriltag/numberAccepted", validMeasurementsPose.size());
+    Logger.recordOutput("Apriltag/numberRejected", invalidMeasurementsPose.size());
   }
 
   public VisionTruster getVisionTruster() {

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

@@ -1,12 +1,12 @@
 package frc.robot.subsystems.swervedrive.vision.truster;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import frc.robot.utils.Apriltag;
 
 /**
  * @param measurement estimated robot position (meters) calculated from apriltag tag what tag produced the
  *     position
  * @param distanceFromTag distance (meters) estimated robot pose was from the tag
  * @param timeOfMeasurement time when the pose was measured (seconds)
  */
-public record VisionMeasurement(
-    Pose2d measurement, double distanceFromTag, double timeOfMeasurement) {}
+public record VisionMeasurement(Pose2d measurement, double distanceFromTag, double timeOfMeasurement) {}

src/main/java/frc/robot/utils/Apriltag.java

@@ -82,7 +82,6 @@ public enum Apriltag {
   THIRTY_ONE(0.32, 147.47, 21.75),//Tower, Blue z rotation:0
   THIRTY_TWO(0.32, 164.47, 21.75);//Tower, Blue z rotation:0
   */
-  public record TagPose(Apriltag tag, Pose3d pose) {}
   private final double xMeters;
   private final double yMeters;
   private final double zMeters;
@@ -133,7 +132,4 @@ public Pose3d getPose() {
   public int number() {
     return ordinal()+1;
   }
-  public TagPose getTagInfo() {
-    return new TagPose(this, pose);
-  }
 }