Skip to content

Commit

Permalink
bring in the rest of the little stuff
Browse files Browse the repository at this point in the history
  • Loading branch information
@srimanachanta
srimanachanta committed Nov 14, 2023
1 parent e764326 commit 28dedd2
Show file tree
Hide file tree
Showing 4 changed files with 11 additions and 71 deletions.
70 changes: 5 additions & 65 deletions photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,6 @@

package org.photonvision;

import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
Expand All @@ -38,12 +36,10 @@
import edu.wpi.first.networktables.IntegerEntry;
import edu.wpi.first.networktables.IntegerPublisher;
import edu.wpi.first.networktables.IntegerSubscriber;
import edu.wpi.first.networktables.MultiSubscriber;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.PubSubOption;
import edu.wpi.first.networktables.RawSubscriber;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
Expand Down Expand Up @@ -73,9 +69,8 @@ public class PhotonCamera implements AutoCloseable {
IntegerPublisher pipelineIndexRequest, ledModeRequest;
IntegerSubscriber pipelineIndexState, ledModeState;
IntegerSubscriber heartbeatEntry;
private DoubleArraySubscriber cameraIntrinsicsSubscriber;
private DoubleArraySubscriber cameraDistortionSubscriber;
private StringPublisher atflPublisher;
DoubleArraySubscriber cameraIntrinsicsSubscriber;
DoubleArraySubscriber cameraDistortionSubscriber;

@Override
public void close() {
Expand All @@ -99,7 +94,6 @@ public void close() {
pipelineIndexRequest.close();
cameraIntrinsicsSubscriber.close();
cameraDistortionSubscriber.close();
atflPublisher.close();
}

private final String path;
Expand All @@ -111,20 +105,14 @@ public void close() {

private long prevHeartbeatValue = -1;
private double prevHeartbeatChangeTime = 0;
private static final double HEARBEAT_DEBOUNCE_SEC = 0.5;
private static final double HEARTBEAT_DEBOUNCE_SEC = 0.5;

public static void setVersionCheckEnabled(boolean enabled) {
VERSION_CHECK_ENABLED = enabled;
}

Packet packet = new Packet(1);

private static AprilTagFieldLayout lastSetTagLayout =
AprilTagFields.kDefaultField.loadAprilTagLayoutField();

// Existing is enough to make this multisubscriber do its thing
private final MultiSubscriber m_topicNameSubscriber;

/**
* Constructs a PhotonCamera from a root table.
*
Expand Down Expand Up @@ -158,14 +146,6 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) {
ledModeRequest = photonvision_root_table.getIntegerTopic("ledModeRequest").publish();
ledModeState = photonvision_root_table.getIntegerTopic("ledModeState").subscribe(-1);
versionEntry = photonvision_root_table.getStringTopic("version").subscribe("");

atflPublisher = photonvision_root_table.getStringTopic("apriltag_field_layout").publish();
// Save the layout locally on Rio; on reboot, should be pushed out to NT clients
atflPublisher.getTopic().setPersistent(true);

m_topicNameSubscriber =
new MultiSubscriber(
instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true));
}

/**
Expand Down Expand Up @@ -329,49 +309,9 @@ public boolean isConnected() {
prevHeartbeatValue = curHeartbeat;
}

return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC;
return (now - prevHeartbeatChangeTime) < HEARTBEAT_DEBOUNCE_SEC;
}

// TODO: Implement ATFL subscribing in backend
// /**
// * Get the last set {@link AprilTagFieldLayout}. The tag layout is shared between all
// PhotonVision
// * coprocessors on the same NT instance-- this method returns the most recent layout set. If no
// * layout has been set by the user, this is equal to {@link AprilTagFields#kDefaultField}.
// *
// * @return The last set layout
// */
// public AprilTagFieldLayout getAprilTagFieldLayout() {
// return lastSetTagLayout;
// }

// /**
// * Set the {@link AprilTagFieldLayout} used by all PhotonVision coprocessors that are (or might
// * later) connect to this robot. The topic is marked as persistent, so even if you only call
// this
// * once ever, it will be saved on the RoboRIO and pushed out to all NT clients when code
// reboots.
// * PhotonVision will also store a copy of this layout locally on the coprocessor, but
// subscribes
// * to this topic and the local copy will be updated when this function is called.
// *
// * @param layout The layout to use for *all* PhotonVision cameras
// * @return Success of serializing the JSON. This does *not* mean that all PhotonVision clients
// * have updated their internal layouts.
// */
// public boolean setAprilTagFieldLayout(AprilTagFieldLayout layout) {
// try {
// var layout_json = new ObjectMapper().writeValueAsString(layout);
// atflPublisher.set(layout_json);
// lastSetTagLayout = layout;
// return true;
// } catch (JsonProcessingException e) {
// MathSharedStore.reportError("Error setting ATFL in " + this.getName(),
// e.getStackTrace());
// }
// return false;
// }

public Optional<Matrix<N3, N3>> getCameraMatrix() {
var cameraMatrix = cameraIntrinsicsSubscriber.get();
if (cameraMatrix != null && cameraMatrix.length == 9) {
Expand Down Expand Up @@ -428,7 +368,7 @@ else if (!isConnected()) {

// Check for version. Warn if the versions aren't aligned.
String versionString = versionEntry.get("");
if (!versionString.equals("") && !PhotonVersion.versionMatches(versionString)) {
if (!versionString.isEmpty() && !PhotonVersion.versionMatches(versionString)) {
// Error on a verified version mismatch
// But stay silent otherwise
DriverStation.reportWarning(
Expand Down
4 changes: 2 additions & 2 deletions photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,8 +170,8 @@ public static Point[] cornersToPoints(TargetCorner... corners) {

public static List<TargetCorner> pointsToCorners(Point... points) {
var corners = new ArrayList<TargetCorner>(points.length);
for (int i = 0; i < points.length; i++) {
corners.add(new TargetCorner(points[i].x, points[i].y));
for (Point point : points) {
corners.add(new TargetCorner(point.x, point.y));
}
return corners;
}
Expand Down
2 changes: 1 addition & 1 deletion photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,7 +151,7 @@ public TargetModel(List<Translation3d> vertices) {
*/
public List<Translation3d> getFieldVertices(Pose3d targetPose) {
var basisChange = new RotTrlTransform3d(targetPose.getRotation(), targetPose.getTranslation());
return vertices.stream().map(t -> basisChange.apply(t)).collect(Collectors.toList());
return vertices.stream().map(basisChange::apply).collect(Collectors.toList());
}

/**
Expand Down
6 changes: 3 additions & 3 deletions photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,8 +74,8 @@ public static PNPResult estimateCamPosePNP(
TargetModel tagModel) {
if (tagLayout == null
|| visTags == null
|| tagLayout.getTags().size() == 0
|| visTags.size() == 0) {
|| tagLayout.getTags().isEmpty()
|| visTags.isEmpty()) {
return new PNPResult();
}

Expand All @@ -92,7 +92,7 @@ public static PNPResult estimateCamPosePNP(
corners.addAll(tgt.getDetectedCorners());
});
}
if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) {
return new PNPResult();
}
Point[] points = OpenCVHelp.cornersToPoints(corners);
Expand Down

0 comments on commit 28dedd2

Please sign in to comment.