From abe32a1aaeba6cf026250b1c1582a4e412e05032 Mon Sep 17 00:00:00 2001 From: Matt Date: Tue, 14 Feb 2023 13:49:28 -0500 Subject: [PATCH] Fix calibration NT table in PhotonCamera (#805) * Fix wrong table in calibration subscriber * Update example to load 2023 layout * Update PhotonCamera.java --- .../java/org/photonvision/PhotonCamera.java | 39 ++++++++-------- .../java/frc/robot/PhotonCameraWrapper.java | 44 +++++-------------- 2 files changed, 33 insertions(+), 50 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index aaa52bd6b..95b5c6e18 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -54,7 +54,7 @@ import org.photonvision.targeting.PhotonPipelineResult; public class PhotonCamera { static final String kTableName = "photonvision"; - protected final NetworkTable rootTable; + protected final NetworkTable cameraTable; RawSubscriber rawBytesEntry; BooleanPublisher driverModePublisher; BooleanSubscriber driverModeSubscriber; @@ -125,26 +125,29 @@ public class PhotonCamera { */ public PhotonCamera(NetworkTableInstance instance, String cameraName) { name = cameraName; - var mainTable = instance.getTable(kTableName); - this.rootTable = mainTable.getSubTable(cameraName); - path = rootTable.getPath(); + var photonvision_root_table = instance.getTable(kTableName); + this.cameraTable = photonvision_root_table.getSubTable(cameraName); + path = cameraTable.getPath(); rawBytesEntry = - rootTable + cameraTable .getRawTopic("rawBytes") .subscribe( "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); - driverModePublisher = rootTable.getBooleanTopic("driverMode").publish(); - driverModeSubscriber = rootTable.getBooleanTopic("driverModeRequest").subscribe(false); - inputSaveImgEntry = rootTable.getIntegerTopic("inputSaveImgCmd").getEntry(0); - outputSaveImgEntry = rootTable.getIntegerTopic("outputSaveImgCmd").getEntry(0); - pipelineIndexRequest = rootTable.getIntegerTopic("pipelineIndexRequest").publish(); - pipelineIndexState = rootTable.getIntegerTopic("pipelineIndexState").subscribe(0); - heartbeatEntry = rootTable.getIntegerTopic("heartbeat").subscribe(-1); - ledModeRequest = mainTable.getIntegerTopic("ledModeRequest").publish(); - ledModeState = mainTable.getIntegerTopic("ledModeState").subscribe(-1); - versionEntry = mainTable.getStringTopic("version").subscribe(""); - cameraIntrinsicsSubscriber = mainTable.getDoubleArrayTopic("cameraIntrinsics").subscribe(null); - cameraDistortionSubscriber = mainTable.getDoubleArrayTopic("cameraDistortion").subscribe(null); + driverModePublisher = cameraTable.getBooleanTopic("driverMode").publish(); + driverModeSubscriber = cameraTable.getBooleanTopic("driverModeRequest").subscribe(false); + inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0); + outputSaveImgEntry = cameraTable.getIntegerTopic("outputSaveImgCmd").getEntry(0); + pipelineIndexRequest = cameraTable.getIntegerTopic("pipelineIndexRequest").publish(); + pipelineIndexState = cameraTable.getIntegerTopic("pipelineIndexState").subscribe(0); + heartbeatEntry = cameraTable.getIntegerTopic("heartbeat").subscribe(-1); + cameraIntrinsicsSubscriber = + cameraTable.getDoubleArrayTopic("cameraIntrinsics").subscribe(null); + cameraDistortionSubscriber = + cameraTable.getDoubleArrayTopic("cameraDistortion").subscribe(null); + + ledModeRequest = photonvision_root_table.getIntegerTopic("ledModeRequest").publish(); + ledModeState = photonvision_root_table.getIntegerTopic("ledModeState").subscribe(-1); + versionEntry = photonvision_root_table.getStringTopic("version").subscribe(""); m_topicNameSubscriber = new MultiSubscriber( @@ -340,7 +343,7 @@ public class PhotonCamera { // Heartbeat entry is assumed to always be present. If it's not present, we // assume that a camera with that name was never connected in the first place. if (!heartbeatEntry.exists()) { - Set cameraNames = rootTable.getInstance().getTable(kTableName).getSubTables(); + Set cameraNames = cameraTable.getInstance().getTable(kTableName).getSubTables(); if (cameraNames.isEmpty()) { DriverStation.reportError( "Could not find any PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!", diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java index 9a5af0818..a2160391d 100644 --- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java +++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java @@ -24,14 +24,11 @@ package frc.robot; -import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.Constants.FieldConstants; import frc.robot.Constants.VisionConstants; -import java.util.ArrayList; +import java.io.IOException; import java.util.Optional; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; @@ -43,38 +40,21 @@ public class PhotonCameraWrapper { public PhotonPoseEstimator photonPoseEstimator; public PhotonCameraWrapper() { - // Set up a test arena of two apriltags at the center of each driver station set - final AprilTag tag18 = - new AprilTag( - 18, - new Pose3d( - new Pose2d( - FieldConstants.length, - FieldConstants.width / 2.0, - Rotation2d.fromDegrees(180)))); - final AprilTag tag01 = - new AprilTag( - 01, - new Pose3d(new Pose2d(0.0, FieldConstants.width / 2.0, Rotation2d.fromDegrees(0.0)))); - ArrayList atList = new ArrayList(); - atList.add(tag18); - atList.add(tag01); + AprilTagFieldLayout atfl = null; + try { + atfl = AprilTagFields.k2023ChargedUp.loadAprilTagLayoutField(); + } catch (IOException e) { + e.printStackTrace(); + } - // TODO - once 2023 happens, replace this with just loading the 2023 field arrangement - AprilTagFieldLayout atfl = - new AprilTagFieldLayout(atList, FieldConstants.length, FieldConstants.width); - - // Forward Camera - photonCamera = - new PhotonCamera( - VisionConstants - .cameraName); // Change the name of your camera here to whatever it is in the - // PhotonVision UI. + // Change the name of your camera here to whatever it is in the PhotonVision UI. + photonCamera = new PhotonCamera(VisionConstants.cameraName); // Create pose estimator photonPoseEstimator = new PhotonPoseEstimator( - atfl, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, photonCamera, VisionConstants.robotToCam); + atfl, PoseStrategy.MULTI_TAG_PNP, photonCamera, VisionConstants.robotToCam); + photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); } /**