Publish all cameras, steer via odometry.

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

@@ -25,10 +25,11 @@
 import frc.robot.autos.ExampleAuto;
 import frc.robot.drivetrain.Drivetrain;
 import frc.robot.drivetrain.commands.ZorroDriveCommand;
-import frc.robot.vision.Camera;
 import frc.robot.vision.Vision;
 import java.util.ArrayList;
+import java.util.HashMap;
 import java.util.List;
+import java.util.Map;
 
 public class Robot extends TimedRobot {
 
@@ -46,7 +47,7 @@ public class Robot extends TimedRobot {
 
   private int m_usb_check_delay = OIConstants.kUSBCheckNumLoops;
 
-  private StructPublisher<Pose2d> publisher = NetworkTableInstance.getDefault().getStructTopic(Camera.FrontRight.toString(), Pose2d.struct).publish();
+  private Map<String, StructPublisher<Pose2d>> posePublishers = new HashMap<>();
 
   public Robot() {
     m_allianceSelector = new AllianceSelector(AutoConstants.kAllianceColorSelectorPort);
@@ -197,14 +198,22 @@ public double getPDHCurrent(int CANBusPort) {
     return m_powerDistribution.getCurrent(CANBusPort - 10);
   }
 
+  private synchronized StructPublisher<Pose2d> getPose2dPublisher(String name) {
+    var publisher = posePublishers.get(name);
+    if (publisher == null) {
+      publisher = NetworkTableInstance.getDefault().getStructTopic(name, Pose2d.struct).publish();
+    }
+    return publisher;
+  }
+
   protected void checkVision() {
     m_vision
         .getPoseEstimates()
         .forEach(
             est -> {
               m_swerve.addVisionMeasurement(
                   est.pose().estimatedPose.toPose2d(), est.pose().timestampSeconds, est.stdev());
-                  publisher.set(est.pose().estimatedPose.toPose2d());
+              getPose2dPublisher(est.name()).set(est.pose().estimatedPose.toPose2d());
             });
   }
 }

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

@@ -3,7 +3,6 @@
 import choreo.trajectory.SwerveSample;
 import com.reduxrobotics.canand.CanandEventLoop;
 import com.reduxrobotics.sensors.canandgyro.Canandgyro;
-
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.controller.PIDController;
@@ -100,7 +99,7 @@ public class Drivetrain extends SubsystemBase {
   private StructPublisher<Pose2d> m_odometryPublisher =
       NetworkTableInstance.getDefault().getStructTopic("Odometry", Pose2d.struct).publish();
 
-  private StructPublisher<Pose2d> m_visionPosePublisher = 
+  private StructPublisher<Pose2d> m_visionPosePublisher =
       NetworkTableInstance.getDefault().getStructTopic("Vision", Pose2d.struct).publish();
 
   public Drivetrain(BooleanSupplier fieldRotatedSupplier) {
@@ -204,21 +203,24 @@ public void zeroAbsTurningEncoderOffsets() {
    * @return The direction of the robot pose
    */
   public Rotation2d getHeading() {
-    return poseEstimator.getEstimatedPosition().getRotation();
+    // return poseEstimator.getEstimatedPosition().getRotation();
+    return m_odometry.getPoseMeters().getRotation();
   }
 
   /**
    * @return The robot pose
    */
   public Pose2d getPose() {
-    return poseEstimator.getEstimatedPosition();
+    // return poseEstimator.getEstimatedPosition();
+    return m_odometry.getPoseMeters();
   }
 
   /**
    * @param pose The robot pose
    */
   public void setPose(Pose2d pose) {
     m_odometry.resetPosition(canandgyro.getRotation2d(), getSwerveModulePositions(), pose);
+    poseEstimator.resetPosition(canandgyro.getRotation2d(), getSwerveModulePositions(), pose);
   }
 
   public void resetHeading() {

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

@@ -3,10 +3,8 @@
 import edu.wpi.first.math.Matrix;
 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 {