Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix calibration NT table in PhotonCamera (#805)

Browse Source 
* Fix wrong table in calibration subscriber

* Update example to load 2023 layout

* Update PhotonCamera.java
This commit is contained in:
Matt
2023-02-14 13:49:28 -05:00
committed by GitHub
parent bf4a4db874
commit abe32a1aae
2 changed files with 33 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View File

@@ -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<String> cameraNames = rootTable.getInstance().getTable(kTableName).getSubTables();
Set<String> 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!",

44
photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java
View File

@@ -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<AprilTag> atList = new ArrayList<AprilTag>();
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);
}
/**