added individual camera logging for advantage scope

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

@@ -1,5 +1,7 @@
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -198,6 +200,8 @@ protected void checkVision() {
             est -> {
               m_swerve.addVisionMeasurement(
                   est.pose().estimatedPose.toPose2d(), est.pose().timestampSeconds, est.stdev());
+                  var publisher = NetworkTableInstance.getDefault().getStructTopic(est.name(), Pose2d.struct).publish();
+                  publisher.set(est.pose().estimatedPose.toPose2d());
             });
   }
 }

src/main/java/frc/robot/drivetrain/Drivetrain.java

@@ -21,8 +21,6 @@
 import edu.wpi.first.networktables.StructPublisher;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.LinearVelocity;
-import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-// import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
@@ -87,7 +85,6 @@ public class Drivetrain extends SubsystemBase {
           RotationControllerGains.kP, RotationControllerGains.kI, RotationControllerGains.kD);
 
   private final Canandgyro canandgyro = new Canandgyro(0);
-  private final Field2d m_field = new Field2d();
 
   private final SwerveDriveOdometry m_odometry =
       new SwerveDriveOdometry(
@@ -100,18 +97,18 @@ public class Drivetrain extends SubsystemBase {
             m_rearRight.getPosition()
           });
 
-  // private final Field2d m_field = new Field2d();
-  private StructPublisher<Pose2d> m_publisher =
+  private StructPublisher<Pose2d> m_odometryPublisher =
       NetworkTableInstance.getDefault().getStructTopic("Odometry", Pose2d.struct).publish();
 
+  private StructPublisher<Pose2d> m_visionPosePublisher = 
+      NetworkTableInstance.getDefault().getStructTopic("Vision", Pose2d.struct).publish();
+
   public Drivetrain(BooleanSupplier fieldRotatedSupplier) {
 
     this.m_fieldRotatedSupplier = fieldRotatedSupplier;
 
     canandgyro.setYaw(0);
 
-    // SmartDashboard.putData("Field", m_field);
-
     for (SwerveModule module : modules) {
       module.resetDriveEncoder();
       module.initializeAbsoluteTurningEncoder();
@@ -139,9 +136,8 @@ public Drivetrain(BooleanSupplier fieldRotatedSupplier) {
   public void periodic() {
 
     updateOdometry();
-    // m_field.setRobotPose(getPose());
-    m_field.getObject("odo").setPose(m_odometry.getPoseMeters());
-    m_publisher.set(m_odometry.getPoseMeters());
+    m_odometryPublisher.set(m_odometry.getPoseMeters());
+    m_visionPosePublisher.set(getPose());
 
     for (SwerveModule module : modules) {
       SmartDashboard.putNumber(

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

@@ -1,26 +1,29 @@
 package frc.robot.vision;
 
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import java.util.Arrays;
 import java.util.List;
 import java.util.stream.Collectors;
+
 import org.photonvision.EstimatedRobotPose;
 
 public class Vision {
-  public record EstimatedPoseWithStdevs(EstimatedRobotPose pose, Matrix<N3, N1> stdev) {}
+  public record EstimatedPoseWithStdevs(EstimatedRobotPose pose, Matrix<N3, N1> stdev, String name) {}
 
   /*
    * Returns (possibly empty) list of camera pose estimates sorted by
    * ascending timestamp.
    */
   public List<EstimatedPoseWithStdevs> getPoseEstimates() {
-    return Arrays.stream(Camera.values())
-        .map(cam -> new Pair<>(cam.getEstimatedGlobalPose(), cam.getEstimationStdDevs()))
-        .filter(pair -> pair.getFirst().isPresent())
-        .map(pair -> new EstimatedPoseWithStdevs(pair.getFirst().get(), pair.getSecond()))
+    record Tuple<T1,T2,T3> (T1 t1, T2 t2, T3 t3) {}
+
+    // return Arrays.stream(Camera.values())
+    return List.of(Camera.FrontRight).stream()
+        .map(cam -> new Tuple<>(cam.getEstimatedGlobalPose(), cam.getEstimationStdDevs(), cam.toString()))
+        .filter(tuple -> tuple.t1.isPresent())
+        .map(tuple -> new EstimatedPoseWithStdevs(tuple.t1.get(), tuple.t2, tuple.t3))
         .sorted(
             (lhs, rhs) -> (int) Math.signum(lhs.pose.timestampSeconds - rhs.pose.timestampSeconds))
         .collect(Collectors.toList());