Updating the sim to have two cameras, and have everything be in Elastic
This commit is contained in:
166
src/main/deploy/elastic-layout.json
Normal file
166
src/main/deploy/elastic-layout.json
Normal file
@@ -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": []
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -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 PhotonCameraSim cameraSim;
|
||||
private PhotonPoseEstimator estimator;
|
||||
private Transform3d realRobotToCamera;
|
||||
private Transform3d testRobotToCamera;
|
||||
private PhotonCamera camera1;
|
||||
private PhotonCamera camera2;
|
||||
private PhotonCameraSim cameraSim1;
|
||||
private PhotonCameraSim cameraSim2;
|
||||
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");
|
||||
cameraSim = new PhotonCameraSim(camera, cameraProperties);
|
||||
cameraSim.enableDrawWireframe(true);
|
||||
camera1 = new PhotonCamera("SimCamera1");
|
||||
cameraSim1 = new PhotonCameraSim(camera1, cameraProperties);
|
||||
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) {
|
||||
Optional<EstimatedRobotPose> cameraPose = estimator.estimateCoprocMultiTagPose(results.get(results.size() - 1));
|
||||
if(results1.size() != 0) {
|
||||
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());
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user