Updating the sim to have two cameras, and have everything be in Elastic

src/main/deploy/elastic-layout.json

@@ -0,0 +1,166 @@
+{
+  "version": 1.0,
+  "grid_size": 128,
+  "tabs": [
+    {
+      "name": "Teleoperated",
+      "grid_layout": {
+        "layouts": [
+          {
+            "title": "Robot Pose Modifier",
+            "x": 896.0,
+            "y": 512.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "xTranslation",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/SmartDashboard/Robot Pose Modifier/xTranslation",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              },
+              {
+                "title": "yTranslation",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/SmartDashboard/Robot Pose Modifier/yTranslation",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              },
+              {
+                "title": "zRotation",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/SmartDashboard/Robot Pose Modifier/zRotation",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": true
+                }
+              }
+            ]
+          }
+        ],
+        "containers": [
+          {
+            "title": "Sim Field",
+            "x": 896.0,
+            "y": 0.0,
+            "width": 896.0,
+            "height": 512.0,
+            "type": "Field",
+            "properties": {
+              "topic": "/SmartDashboard/Sim Field",
+              "period": 0.06,
+              "field_game": "Rebuilt",
+              "robot_width": 0.85,
+              "robot_length": 0.85,
+              "show_other_objects": true,
+              "show_trajectories": true,
+              "field_rotation": 0.0,
+              "robot_color": 4294198070,
+              "trajectory_color": 4294967295,
+              "show_robot_outside_widget": true
+            }
+          },
+          {
+            "title": "SimCamera1-processed",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 896.0,
+            "height": 384.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/SimCamera1-processed",
+              "period": 0.06,
+              "rotation_turns": 0
+            }
+          },
+          {
+            "title": "SimCamera2-processed",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 896.0,
+            "height": 384.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/SimCamera2-processed",
+              "period": 0.06,
+              "rotation_turns": 0
+            }
+          },
+          {
+            "title": "Robot",
+            "x": 1152.0,
+            "y": 512.0,
+            "width": 512.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Sim Field/Robot",
+              "period": 0.06,
+              "data_type": "double[]",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "GHOST 1",
+            "x": 1152.0,
+            "y": 640.0,
+            "width": 512.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Sim Field/GHOST 1",
+              "period": 0.06,
+              "data_type": "double[]",
+              "show_submit_button": true
+            }
+          },
+          {
+            "title": "GHOST 2",
+            "x": 1152.0,
+            "y": 768.0,
+            "width": 512.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Sim Field/GHOST 2",
+              "period": 0.06,
+              "data_type": "double[]",
+              "show_submit_button": true
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Autonomous",
+      "grid_layout": {
+        "layouts": [],
+        "containers": []
+      }
+    }
+  ]
+}

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

@@ -26,8 +26,10 @@ import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.net.WebServer;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
@@ -40,11 +42,16 @@ public class Robot extends TimedRobot {
 
   private VisionSystemSim visionSim;
   private SimCameraProperties cameraProperties;
-  private PhotonCamera camera;
+  private PhotonCamera camera1;
-  private PhotonCameraSim cameraSim;
+  private PhotonCamera camera2;
-  private PhotonPoseEstimator estimator;
+  private PhotonCameraSim cameraSim1;
-  private Transform3d realRobotToCamera;
+  private PhotonCameraSim cameraSim2;
-  private Transform3d testRobotToCamera;
+  private PhotonPoseEstimator estimator1;
+  private PhotonPoseEstimator estimator2;
+  private Transform3d realRobotToCamera1;
+  private Transform3d testRobotToCamera1;
+  private Transform3d realRobotToCamera2;
+  private Transform3d testRobotToCamera2;
 
   private double xTranslation;
   private double yTranslation;
@@ -55,6 +62,8 @@ public class Robot extends TimedRobot {
    * initialization code.
    */
   public Robot() {
+    WebServer.start(5800, Filesystem.getDeployDirectory().getPath());
+    
     visionSim = new VisionSystemSim("main");
 
     visionSim.addAprilTags(AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded));
@@ -70,11 +79,15 @@ public class Robot extends TimedRobot {
     cameraProperties.setAvgLatencyMs(35);
     cameraProperties.setLatencyStdDevMs(5);
 
-    camera = new PhotonCamera("SimCamera");
+    camera1 = new PhotonCamera("SimCamera1");
-    cameraSim = new PhotonCameraSim(camera, cameraProperties);
+    cameraSim1 = new PhotonCameraSim(camera1, cameraProperties);
-    cameraSim.enableDrawWireframe(true);
+    cameraSim1.enableDrawWireframe(true);
 
-    realRobotToCamera = new Transform3d(
+    camera2 = new PhotonCamera("SimCamera2");
+    cameraSim2 = new PhotonCameraSim(camera2, cameraProperties);
+    cameraSim2.enableDrawWireframe(true);
+
+    realRobotToCamera1 = new Transform3d(
         Units.inchesToMeters(1.5),
         Units.inchesToMeters(-8.5),
         Units.inchesToMeters(28.5),
@@ -85,7 +98,7 @@ public class Robot extends TimedRobot {
         )
     );
 
-    testRobotToCamera = new Transform3d(
+    testRobotToCamera1 = new Transform3d(
       Units.inchesToMeters(1.5),
         Units.inchesToMeters(-8.5),
         Units.inchesToMeters(28.5),
@@ -96,12 +109,41 @@ public class Robot extends TimedRobot {
         )
     );
 
-    visionSim.addCamera(cameraSim, realRobotToCamera);
+    realRobotToCamera2 = new Transform3d(
+      Units.inchesToMeters(1.5), 
+      Units.inchesToMeters(-10.5), 
+      Units.inchesToMeters(28.5), 
+      new Rotation3d(
+        Units.degreesToRadians(0), 
+        Units.degreesToRadians(-24), 
+        Units.degreesToRadians(-10)
+      )
+    );
 
-    estimator = new PhotonPoseEstimator(
+    testRobotToCamera2 = new Transform3d(
+      Units.inchesToMeters(1.5), 
+      Units.inchesToMeters(-10.5), 
+      Units.inchesToMeters(28.5), 
+      new Rotation3d(
+        Units.degreesToRadians(0), 
+        Units.degreesToRadians(-24), 
+        Units.degreesToRadians(-10)
+      )
+    );
+
+    visionSim.addCamera(cameraSim1, realRobotToCamera1);
+    visionSim.addCamera(cameraSim2, realRobotToCamera2);
+
+    estimator1 = new PhotonPoseEstimator(
       AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded), 
       PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
-      testRobotToCamera
+      testRobotToCamera1
+    );
+
+    estimator2 = new PhotonPoseEstimator(
+      AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded), 
+      PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
+      testRobotToCamera2
     );
 
     xTranslation = 510;
@@ -142,13 +184,22 @@ public class Robot extends TimedRobot {
     visionSim.update(robotPose);
     SmartDashboard.putData("Sim Field", visionSim.getDebugField());
 
-    List<PhotonPipelineResult> results = camera.getAllUnreadResults();
+    List<PhotonPipelineResult> results1 = camera1.getAllUnreadResults();
+    List<PhotonPipelineResult> results2 = camera2.getAllUnreadResults();
 
-    if(results.size() != 0) {
+    if(results1.size() != 0) {
-      Optional<EstimatedRobotPose> cameraPose = estimator.estimateCoprocMultiTagPose(results.get(results.size() - 1));
+      Optional<EstimatedRobotPose> cameraPose = estimator1.estimateCoprocMultiTagPose(results1.get(results1.size() - 1));
 
       cameraPose.ifPresent((p) -> {
-        visionSim.getDebugField().getObject("GHOST").setPose(p.estimatedPose.toPose2d());
+        visionSim.getDebugField().getObject("GHOST 1").setPose(p.estimatedPose.toPose2d());
+      });
+    }
+
+    if(results2.size() != 0) {
+      Optional<EstimatedRobotPose> cameraPose = estimator2.estimateCoprocMultiTagPose(results2.get(results2.size() - 1));
+
+      cameraPose.ifPresent((p) -> {
+        visionSim.getDebugField().getObject("GHOST 2").setPose(p.estimatedPose.toPose2d());
       });
     }
   }