Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-03 03:01:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update to wpilib 2023 beta 7 (#607)

Browse Source 
We now need platform specific jars -- reworks actions to support that. Currently only generates 32 bit pi images.
This commit is contained in:
shueja-personal
2022-12-16 17:05:23 -08:00
committed by GitHub
parent da1aabae3a
commit bb63af601d
198 changed files with 6339 additions and 4525 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
photon-core/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java Normal file
View File

@@ -0,0 +1,102 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.apriltag.jni;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.concurrent.atomic.AtomicBoolean;
import org.opencv.core.Mat;
public class AprilTagJNI {
static boolean libraryLoaded = false;
static RuntimeLoader<AprilTagJNI> loader = null;
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
public static boolean getExtractOnStaticLoad() {
return extractOnStaticLoad.get();
}
public static void setExtractOnStaticLoad(boolean load) {
extractOnStaticLoad.set(load);
}
}
static {
if (Helper.getExtractOnStaticLoad()) {
try {
loader =
new RuntimeLoader<>(
"apriltagjni", RuntimeLoader.getDefaultExtractionRoot(), AprilTagJNI.class);
loader.loadLibrary();
} catch (IOException ex) {
ex.printStackTrace();
System.exit(1);
}
libraryLoaded = true;
}
}
// Returns a pointer to a apriltag_detector_t
public static native long aprilTagCreate(
String fam, double decimate, double blur, int threads, boolean debug, boolean refine_edges);
// Destroy and free a previously created detector.
public static native void aprilTagDestroy(long detector);
private static native Object[] aprilTagDetectInternal(
long detector,
long imgAddr,
int rows,
int cols,
boolean doPoseEstimation,
double tagWidth,
double fx,
double fy,
double cx,
double cy,
int nIters);
// Detect targets given a GRAY frame. Returns a pointer toa zarray
public static DetectionResult[] aprilTagDetect(
long detector,
Mat img,
boolean doPoseEstimation,
double tagWidth,
double fx,
double fy,
double cx,
double cy,
int nIters) {
return (DetectionResult[])
aprilTagDetectInternal(
detector,
img.dataAddr(),
img.rows(),
img.cols(),
doPoseEstimation,
tagWidth,
fx,
fy,
cx,
cy,
nIters);
}
}

246
photon-core/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java Normal file
View File

@@ -0,0 +1,246 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package edu.wpi.first.apriltag.jni;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.N3;
import java.util.Arrays;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
import org.ejml.simple.SimpleMatrix;
public class DetectionResult {
public int getId() {
return m_id;
}
public int getHamming() {
return m_hamming;
}
public float getDecisionMargin() {
return m_decisionMargin;
}
public void setDecisionMargin(float decisionMargin) {
this.m_decisionMargin = decisionMargin;
}
@SuppressWarnings("PMD.MethodReturnsInternalArray")
public double[] getHomography() {
return m_homography;
}
@SuppressWarnings("PMD.ArrayIsStoredDirectly")
public void setHomography(double[] homography) {
this.m_homography = homography;
}
public double getCenterX() {
return m_centerX;
}
public void setCenterX(double centerX) {
this.m_centerX = centerX;
}
public double getCenterY() {
return m_centerY;
}
public void setCenterY(double centerY) {
this.m_centerY = centerY;
}
@SuppressWarnings("PMD.MethodReturnsInternalArray")
public double[] getCorners() {
return m_corners;
}
@SuppressWarnings("PMD.ArrayIsStoredDirectly")
public void setCorners(double[] corners) {
this.m_corners = corners;
}
public double getError1() {
return m_error1;
}
public double getError2() {
return m_error2;
}
public Transform3d getPoseResult1() {
return m_poseResult1;
}
public Transform3d getPoseResult2() {
return m_poseResult2;
}
private final int m_id;
private final int m_hamming;
private float m_decisionMargin;
private double[] m_homography;
private double m_centerX;
private double m_centerY;
private double[] m_corners;
private final Transform3d m_poseResult1;
private final double m_error1;
private final Transform3d m_poseResult2;
private final double m_error2;
/**
* Constructs a new detection result. Used from JNI.
*
* @param id id
* @param hamming hamming
* @param decisionMargin dm
* @param homography homography
* @param centerX centerX
* @param centerY centerY
* @param corners corners
* @param pose1TransArr pose1TransArr
* @param pose1RotArr pose1RotArr
* @param err1 err1
* @param pose2TransArr pose2TransArr
* @param pose2RotArr pose2RotArr
* @param err2 err2
*/
@SuppressWarnings("PMD.ArrayIsStoredDirectly")
public DetectionResult(
int id,
int hamming,
float decisionMargin,
double[] homography,
double centerX,
double centerY,
double[] corners,
double[] pose1TransArr,
double[] pose1RotArr,
double err1,
double[] pose2TransArr,
double[] pose2RotArr,
double err2) {
this.m_id = id;
this.m_hamming = hamming;
this.m_decisionMargin = decisionMargin;
this.m_homography = homography;
this.m_centerX = centerX;
this.m_centerY = centerY;
this.m_corners = corners;
this.m_error1 = err1;
var rot1 = new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr);
if (rot1.normF() > 0) {
this.m_poseResult1 =
new Transform3d(
new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]),
new Rotation3d(orthogonalizeRotationMatrix(rot1)));
} else {
this.m_poseResult1 = new Transform3d();
}
this.m_error2 = err2;
var rot2 = new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr);
if (rot2.normF() > 0) {
this.m_poseResult2 =
new Transform3d(
new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]),
new Rotation3d(orthogonalizeRotationMatrix(rot2)));
} else {
this.m_poseResult2 = new Transform3d();
}
}
/**
* Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be
* ambiguous.
*
* @return The ratio of pose reprojection errors.
*/
public double getPoseAmbiguity() {
var min = Math.min(m_error1, m_error2);
var max = Math.max(m_error1, m_error2);
if (max > 0) {
return min / max;
} else {
return -1;
}
}
@Override
public String toString() {
return "DetectionResult [centerX="
+ m_centerX
+ ", centerY="
+ m_centerY
+ ", corners="
+ Arrays.toString(m_corners)
+ ", decisionMargin="
+ m_decisionMargin
+ ", error1="
+ m_error1
+ ", error2="
+ m_error2
+ ", hamming="
+ m_hamming
+ ", homography="
+ Arrays.toString(m_homography)
+ ", id="
+ m_id
+ ", poseResult1="
+ m_poseResult1
+ ", poseResult2="
+ m_poseResult2
+ "]";
}
private static Matrix<N3, N3> orthogonalizeRotationMatrix(Matrix<N3, N3> input) {
var a = DecompositionFactory_DDRM.qr(3, 3);
if (!a.decompose(input.getStorage().getDDRM())) {
// best we can do is return the input
return input;
}
// Grab results (thanks for this _great_ api, EJML)
var Q = new DMatrixRMaj(3, 3);
var R = new DMatrixRMaj(3, 3);
a.getQ(Q, false);
a.getR(R, false);
// Fix signs in R if they're < 0 so it's close to an identity matrix
// (our QR decomposition implementation sometimes flips the signs of columns)
for (int colR = 0; colR < 3; ++colR) {
if (R.get(colR, colR) < 0) {
for (int rowQ = 0; rowQ < 3; ++rowQ) {
Q.set(rowQ, colR, -Q.get(rowQ, colR));
}
}
}
return new Matrix<>(new SimpleMatrix(Q));
}
}

23
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataChangeListener.java
View File

@@ -17,22 +17,29 @@
package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.networktables.EntryListenerFlags;
import edu.wpi.first.networktables.EntryNotification;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableEvent;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.Subscriber;
import java.util.EnumSet;
import java.util.function.Consumer;
public class NTDataChangeListener {
private final NetworkTableEntry watchedEntry;
private final NetworkTableInstance instance;
private final Subscriber watchedEntry;
private final int listenerID;
public NTDataChangeListener(
NetworkTableEntry watchedEntry, Consumer<EntryNotification> dataChangeConsumer) {
this.watchedEntry = watchedEntry;
listenerID = watchedEntry.addListener(dataChangeConsumer, EntryListenerFlags.kUpdate);
NetworkTableInstance instance,
Subscriber watchedSubscriber,
Consumer<NetworkTableEvent> dataChangeConsumer) {
this.watchedEntry = watchedSubscriber;
this.instance = instance;
listenerID =
this.instance.addListener(
watchedEntry, EnumSet.of(NetworkTableEvent.Kind.kValueAll), dataChangeConsumer);
}
public void remove() {
watchedEntry.removeListener(listenerID);
this.instance.removeListener(listenerID);
}
}

139
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View File

@@ -17,9 +17,8 @@
package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.networktables.EntryNotification;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableEvent;
import java.util.ArrayList;
import java.util.List;
import java.util.function.BooleanSupplier;
@@ -30,6 +29,7 @@ import org.photonvision.common.dataflow.CVPipelineResultConsumer;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
@@ -37,32 +37,21 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.target.TrackedTarget;
public class NTDataPublisher implements CVPipelineResultConsumer {
private final Logger logger = new Logger(NTDataPublisher.class, LogGroup.General);
private final NetworkTable rootTable = NetworkTablesManager.getInstance().kRootTable;
private final Logger logger = new Logger(NTDataPublisher.class, LogGroup.Data);
private NetworkTable subTable;
private NetworkTableEntry rawBytesEntry;
private NetworkTableEntry pipelineIndexEntry;
private final Consumer<Integer> pipelineIndexConsumer;
private NTDataChangeListener pipelineIndexListener;
private NetworkTableEntry driverModeEntry;
private final Consumer<Boolean> driverModeConsumer;
private NTDataChangeListener driverModeListener;
private NetworkTableEntry latencyMillisEntry;
private NetworkTableEntry hasTargetEntry;
private NetworkTableEntry targetPitchEntry;
private NetworkTableEntry targetYawEntry;
private NetworkTableEntry targetAreaEntry;
private NetworkTableEntry targetPoseEntry;
private NetworkTableEntry targetSkewEntry;
// The raw position of the best target, in pixels.
private NetworkTableEntry bestTargetPosX;
private NetworkTableEntry bestTargetPosY;
private NTTopicSet ts = new NTTopicSet();
NTDataChangeListener pipelineIndexListener;
private final Supplier<Integer> pipelineIndexSupplier;
private final Consumer<Integer> pipelineIndexConsumer;
NTDataChangeListener driverModeListener;
private final BooleanSupplier driverModeSupplier;
private final Consumer<Boolean> driverModeConsumer;
private long heartbeatCounter = 0;
public NTDataPublisher(
String cameraNickname,
@@ -79,13 +68,13 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
updateEntries();
}
private void onPipelineIndexChange(EntryNotification entryNotification) {
var newIndex = (int) entryNotification.value.getDouble();
private void onPipelineIndexChange(NetworkTableEvent entryNotification) {
var newIndex = (int) entryNotification.valueData.value.getInteger();
var originalIndex = pipelineIndexSupplier.get();
// ignore indexes below 0
if (newIndex < 0) {
pipelineIndexEntry.forceSetNumber(originalIndex);
ts.pipelineIndexPublisher.set(originalIndex);
return;
}
@@ -97,14 +86,14 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
pipelineIndexConsumer.accept(newIndex);
var setIndex = pipelineIndexSupplier.get();
if (newIndex != setIndex) { // set failed
pipelineIndexEntry.forceSetNumber(setIndex);
logger.warn("Failed to set pipeline index to " + newIndex);
ts.pipelineIndexPublisher.set(setIndex);
// TODO: Log
}
logger.debug("Successfully set pipeline index to " + newIndex);
}
private void onDriverModeChange(EntryNotification entryNotification) {
var newDriverMode = entryNotification.value.getBoolean();
private void onDriverModeChange(NetworkTableEvent entryNotification) {
var newDriverMode = entryNotification.valueData.value.getBoolean();
var originalDriverMode = driverModeSupplier.getAsBoolean();
if (newDriverMode == originalDriverMode) {
@@ -116,56 +105,30 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
logger.debug("Successfully set driver mode to " + newDriverMode);
}
@SuppressWarnings("DuplicatedCode")
private void removeEntries() {
if (rawBytesEntry != null) rawBytesEntry.delete();
if (pipelineIndexListener != null) pipelineIndexListener.remove();
if (pipelineIndexEntry != null) pipelineIndexEntry.delete();
if (driverModeListener != null) driverModeListener.remove();
if (driverModeEntry != null) driverModeEntry.delete();
if (latencyMillisEntry != null) latencyMillisEntry.delete();
if (hasTargetEntry != null) hasTargetEntry.delete();
if (targetPitchEntry != null) targetPitchEntry.delete();
if (targetAreaEntry != null) targetAreaEntry.delete();
if (targetYawEntry != null) targetYawEntry.delete();
if (targetPoseEntry != null) targetPoseEntry.delete();
if (targetSkewEntry != null) targetSkewEntry.delete();
if (bestTargetPosX != null) bestTargetPosX.delete();
if (bestTargetPosY != null) bestTargetPosY.delete();
ts.removeEntries();
}
private void updateEntries() {
rawBytesEntry = subTable.getEntry("rawBytes");
if (pipelineIndexListener != null) pipelineIndexListener.remove();
if (driverModeListener != null) driverModeListener.remove();
ts.updateEntries();
if (pipelineIndexListener != null) {
pipelineIndexListener.remove();
}
pipelineIndexEntry = subTable.getEntry("pipelineIndex");
pipelineIndexListener =
new NTDataChangeListener(pipelineIndexEntry, this::onPipelineIndexChange);
new NTDataChangeListener(
ts.subTable.getInstance(), ts.pipelineIndexSubscriber, this::onPipelineIndexChange);
if (driverModeListener != null) {
driverModeListener.remove();
}
driverModeEntry = subTable.getEntry("driverMode");
driverModeListener = new NTDataChangeListener(driverModeEntry, this::onDriverModeChange);
latencyMillisEntry = subTable.getEntry("latencyMillis");
hasTargetEntry = subTable.getEntry("hasTarget");
targetPitchEntry = subTable.getEntry("targetPitch");
targetAreaEntry = subTable.getEntry("targetArea");
targetYawEntry = subTable.getEntry("targetYaw");
targetPoseEntry = subTable.getEntry("targetPose");
targetSkewEntry = subTable.getEntry("targetSkew");
bestTargetPosX = subTable.getEntry("targetPixelsX");
bestTargetPosY = subTable.getEntry("targetPixelsY");
driverModeListener =
new NTDataChangeListener(
ts.subTable.getInstance(), ts.driverModeSubscriber, this::onDriverModeChange);
}
public void updateCameraNickname(String newCameraNickname) {
removeEntries();
subTable = rootTable.getSubTable(newCameraNickname);
ts.subTable = rootTable.getSubTable(newCameraNickname);
updateEntries();
}
@@ -177,23 +140,23 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
Packet packet = new Packet(simplified.getPacketSize());
simplified.populatePacket(packet);
rawBytesEntry.forceSetRaw(packet.getData());
ts.rawBytesEntry.set(packet.getData());
pipelineIndexEntry.forceSetNumber(pipelineIndexSupplier.get());
driverModeEntry.forceSetBoolean(driverModeSupplier.getAsBoolean());
latencyMillisEntry.forceSetDouble(result.getLatencyMillis());
hasTargetEntry.forceSetBoolean(result.hasTargets());
ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get());
ts.driverModePublisher.set(driverModeSupplier.getAsBoolean());
ts.latencyMillisEntry.set(result.getLatencyMillis());
ts.hasTargetEntry.set(result.hasTargets());
if (result.hasTargets()) {
var bestTarget = result.targets.get(0);
targetPitchEntry.forceSetDouble(bestTarget.getPitch());
targetYawEntry.forceSetDouble(bestTarget.getYaw());
targetAreaEntry.forceSetDouble(bestTarget.getArea());
targetSkewEntry.forceSetDouble(bestTarget.getSkew());
ts.targetPitchEntry.set(bestTarget.getPitch());
ts.targetYawEntry.set(bestTarget.getYaw());
ts.targetAreaEntry.set(bestTarget.getArea());
ts.targetSkewEntry.set(bestTarget.getSkew());
var pose = bestTarget.getBestCameraToTarget3d();
targetPoseEntry.forceSetDoubleArray(
ts.targetPoseEntry.set(
new double[] {
pose.getTranslation().getX(),
pose.getTranslation().getY(),
@@ -205,17 +168,21 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
});
var targetOffsetPoint = bestTarget.getTargetOffsetPoint();
bestTargetPosX.forceSetDouble(targetOffsetPoint.x);
bestTargetPosY.forceSetDouble(targetOffsetPoint.y);
ts.bestTargetPosX.set(targetOffsetPoint.x);
ts.bestTargetPosY.set(targetOffsetPoint.y);
} else {
targetPitchEntry.forceSetDouble(0);
targetYawEntry.forceSetDouble(0);
targetAreaEntry.forceSetDouble(0);
targetSkewEntry.forceSetDouble(0);
targetPoseEntry.forceSetDoubleArray(new double[] {0, 0, 0});
bestTargetPosX.forceSetDouble(0);
bestTargetPosY.forceSetDouble(0);
ts.targetPitchEntry.set(0);
ts.targetYawEntry.set(0);
ts.targetAreaEntry.set(0);
ts.targetSkewEntry.set(0);
ts.targetPoseEntry.set(new double[] {0, 0, 0});
ts.bestTargetPosX.set(0);
ts.bestTargetPosY.set(0);
}
ts.heartbeatPublisher.set(heartbeatCounter++);
// TODO...nt4... is this needed?
rootTable.getInstance().flush();
}

16
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
View File

@@ -17,8 +17,8 @@
package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.networktables.LogMessage;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEvent;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.HashMap;
import java.util.function.Consumer;
@@ -41,7 +41,7 @@ public class NetworkTablesManager {
private boolean isRetryingConnection = false;
private NetworkTablesManager() {
ntInstance.addLogger(new NTLogger(), 0, 255); // to hide error messages
ntInstance.addLogger(0, 255, new NTLogger()); // to hide error messages
TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000);
}
@@ -54,17 +54,17 @@ public class NetworkTablesManager {
private static final Logger logger = new Logger(NetworkTablesManager.class, LogGroup.General);
private static class NTLogger implements Consumer<LogMessage> {
private static class NTLogger implements Consumer<NetworkTableEvent> {
private boolean hasReportedConnectionFailure = false;
private long lastConnectMessageMillis = 0;
@Override
public void accept(LogMessage logMessage) {
if (!hasReportedConnectionFailure && logMessage.message.contains("timed out")) {
public void accept(NetworkTableEvent event) {
if (!hasReportedConnectionFailure && event.logMessage.message.contains("timed out")) {
logger.error("NT Connection has failed! Will retry in background.");
hasReportedConnectionFailure = true;
getInstance().broadcastConnectedStatus();
} else if (logMessage.message.contains("connected")
} else if (event.logMessage.message.contains("connected")
&& System.currentTimeMillis() - lastConnectMessageMillis > 125) {
logger.info("NT Connected!");
hasReportedConnectionFailure = false;
@@ -115,8 +115,8 @@ public class NetworkTablesManager {
private void setClientMode(int teamNumber) {
if (!isRetryingConnection) logger.info("Starting NT Client");
ntInstance.stopServer();
ntInstance.startClientTeam(teamNumber);
ntInstance.startClient4("photonvision");
ntInstance.setServerTeam(teamNumber);
ntInstance.startDSClient();
broadcastVersion();
}

14
photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java
View File

@@ -17,7 +17,7 @@
package org.photonvision.common.hardware;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.IntegerEntry;
import java.io.IOException;
import org.photonvision.common.ProgramStatus;
import org.photonvision.common.configuration.ConfigManager;
@@ -45,7 +45,7 @@ public class HardwareManager {
private final StatusLED statusLED;
@SuppressWarnings("FieldCanBeLocal")
private final NetworkTableEntry ledModeEntry;
private final IntegerEntry ledModeEntry;
@SuppressWarnings({"FieldCanBeLocal", "unused"})
private final NTDataChangeListener ledModeListener;
@@ -89,12 +89,16 @@ public class HardwareManager {
hasBrightnessRange ? hardwareConfig.ledBrightnessRange.get(1) : 100,
pigpioSocket);
ledModeEntry = NetworkTablesManager.getInstance().kRootTable.getEntry("ledMode");
ledModeEntry.setNumber(VisionLEDMode.kDefault.value);
ledModeEntry =
NetworkTablesManager.getInstance().kRootTable.getIntegerTopic("ledMode").getEntry(0);
ledModeEntry.set(VisionLEDMode.kDefault.value);
ledModeListener =
visionLED == null
? null
: new NTDataChangeListener(ledModeEntry, visionLED::onLedModeChange);
: new NTDataChangeListener(
NetworkTablesManager.getInstance().kRootTable.getInstance(),
ledModeEntry,
visionLED::onLedModeChange);
Runtime.getRuntime().addShutdownHook(new Thread(this::onJvmExit));

17
photon-core/src/main/java/org/photonvision/common/hardware/Platform.java
View File

@@ -18,7 +18,10 @@
package org.photonvision.common.hardware;
import edu.wpi.first.util.RuntimeDetector;
import java.io.BufferedReader;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Paths;
import org.photonvision.common.util.ShellExec;
@SuppressWarnings("unused")
@@ -105,7 +108,7 @@ public enum Platform {
if (RuntimeDetector.isLinux()) {
if (RuntimeDetector.is32BitIntel()) return UNSUPPORTED;
if (RuntimeDetector.is64BitIntel()) return LINUX_64;
if (RuntimeDetector.isRaspbian()) return LINUX_RASPBIAN;
if (isRaspbian()) return LINUX_RASPBIAN;
}
System.out.println(UnknownPlatformString);
@@ -137,4 +140,16 @@ public enum Platform {
return "";
}
private static boolean isRaspbian() {
try (BufferedReader reader = Files.newBufferedReader(Paths.get("/etc/os-release"))) {
String value = reader.readLine();
if (value == null) {
return false;
}
return value.contains("Raspbian");
} catch (IOException ex) {
return false;
}
}
}

9
photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java
View File

@@ -17,7 +17,7 @@
package org.photonvision.common.hardware;
import edu.wpi.first.networktables.EntryNotification;
import edu.wpi.first.networktables.NetworkTableEvent;
import java.util.ArrayList;
import java.util.List;
import java.util.function.BooleanSupplier;
@@ -122,8 +122,8 @@ public class VisionLED {
setInternal(on ? VisionLEDMode.kOn : VisionLEDMode.kOff, false);
}
void onLedModeChange(EntryNotification entryNotification) {
var newLedModeRaw = (int) entryNotification.value.getDouble();
void onLedModeChange(NetworkTableEvent entryNotification) {
var newLedModeRaw = (int) entryNotification.valueData.value.getDouble();
if (newLedModeRaw != currentLedMode.value) {
VisionLEDMode newLedMode;
switch (newLedModeRaw) {
@@ -185,6 +185,9 @@ public class VisionLED {
case kOn:
setStateImpl(true);
break;
case kBlink:
blinkImpl(85, -1);
break;
}
}
logger.info("Changing LED internal state to " + newLedMode.toString());

56
photon-core/src/main/java/org/photonvision/common/util/TestUtils.java
View File

@@ -18,23 +18,54 @@
package org.photonvision.common.util;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.apriltag.jni.AprilTagJNI;
import edu.wpi.first.cscore.CameraServerCvJNI;
import edu.wpi.first.cscore.CameraServerJNI;
import edu.wpi.first.hal.JNIWrapper;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.net.WPINetJNI;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.util.CombinedRuntimeLoader;
import edu.wpi.first.util.RuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
import java.awt.*;
import java.io.File;
import java.io.IOException;
import java.nio.file.Path;
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.highgui.HighGui;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
public class TestUtils {
public static void loadLibraries() {
public static boolean loadLibraries() {
JNIWrapper.Helper.setExtractOnStaticLoad(false);
WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
WPINetJNI.Helper.setExtractOnStaticLoad(false);
CameraServerJNI.Helper.setExtractOnStaticLoad(false);
CameraServerCvJNI.Helper.setExtractOnStaticLoad(false);
AprilTagJNI.Helper.setExtractOnStaticLoad(false);
try {
CameraServerCvJNI.forceLoad();
// PicamJNI.forceLoad();
} catch (IOException ex) {
// ignored
var loader =
new RuntimeLoader<>(
Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
loader.loadLibrary();
CombinedRuntimeLoader.loadLibraries(
TestUtils.class,
"wpiutiljni",
"ntcorejni",
"wpinetjni",
"wpiHaljni",
"cscorejni",
"cscorejnicvstatic",
"apriltagjni");
return true;
} catch (IOException e) {
e.printStackTrace();
return false;
}
}
@@ -182,7 +213,20 @@ public class TestUtils {
private static Path getResourcesFolderPath(boolean testMode) {
System.out.println("CWD: " + Path.of("").toAbsolutePath().toString());
return Path.of("test-resources").toAbsolutePath();
// VSCode likes to make this path relative to the wrong root directory, so a fun hack to tell
// if it's wrong
Path ret = Path.of("test-resources").toAbsolutePath();
if (Path.of("test-resources")
.toAbsolutePath()
.toString()
.replace("/", "")
.replace("\\", "")
.toLowerCase()
.matches(".*photon-[a-z]*test-resources")) {
ret = Path.of("../test-resources").toAbsolutePath();
}
return ret;
}
public static Path getTestMode2019ImagePath() {

4
photon-core/src/main/java/org/photonvision/common/util/file/FileUtils.java
View File

@@ -78,7 +78,7 @@ public class FileUtils {
}
public static void setFilePerms(Path path) throws IOException {
if (!Platform.currentPlatform.isWindows()) {
if (!Platform.isWindows()) {
File thisFile = path.toFile();
Set<PosixFilePermission> perms =
Files.readAttributes(path, PosixFileAttributes.class).permissions();
@@ -96,7 +96,7 @@ public class FileUtils {
}
public static void setAllPerms(Path path) {
if (!Platform.currentPlatform.isWindows()) {
if (!Platform.isWindows()) {
String command = String.format("chmod 777 -R %s", path.toString());
try {
Process p = Runtime.getRuntime().exec(command);

4
photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
View File

@@ -177,7 +177,9 @@ public class MathUtils {
// CameraToTarget _should_ be in opencv-land EDN
var nwu =
CoordinateSystem.convert(
new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
new Pose3d().transformBy(cameraToTarget3d),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
return new Pose3d(nwu.getTranslation(), WPILIB_BASE_ROTATION.rotateBy(nwu.getRotation()));
}

8
photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagDetector.java
View File

@@ -30,6 +30,8 @@ import org.opencv.core.Mat;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import edu.wpi.first.apriltag.jni.AprilTagJNI;
import edu.wpi.first.apriltag.jni.DetectionResult;
public class AprilTagDetector {
private static final Logger logger = new Logger(AprilTagDetector.class, LogGroup.VisionModule);
@@ -43,13 +45,13 @@ public class AprilTagDetector {
private void updateDetector() {
if (m_detectorPtr != 0) {
// TODO: in JNI
AprilTagJNI.AprilTag_Destroy(m_detectorPtr);
AprilTagJNI.aprilTagDestroy(m_detectorPtr);
m_detectorPtr = 0;
}
logger.debug("Creating detector with params " + m_detectorParams);
m_detectorPtr =
AprilTagJNI.AprilTag_Create(
AprilTagJNI.aprilTagCreate(
m_detectorParams.tagFamily.getNativeName(),
m_detectorParams.decimate,
m_detectorParams.blur,
@@ -95,7 +97,7 @@ public class AprilTagDetector {
}
}
return AprilTagJNI.AprilTag_Detect(
return AprilTagJNI.aprilTagDetect(
m_detectorPtr, grayscaleImg, doPoseEst, tagWidthMeters, fx, fy, cx, cy, numIterations);
}
}

191
photon-core/src/main/java/org/photonvision/vision/apriltag/AprilTagJNI.java
View File

@@ -1,191 +0,0 @@
/*
Copyright (c) 2022 Photon Vision. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of FIRST, WPILib, nor the names of other WPILib
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.photonvision.vision.apriltag;
import edu.wpi.first.util.RuntimeDetector;
import edu.wpi.first.util.RuntimeLoader;
import java.io.File;
import java.io.IOException;
import java.io.InputStream;
import java.net.URL;
import java.nio.file.Files;
import java.nio.file.Path;
import org.opencv.core.Mat;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
public class AprilTagJNI {
static final boolean USE_DEBUG =
false; // Development flag - should be false on release, but flip to True to read in a debug
// version of the library
static final String NATIVE_DEBUG_LIBRARY_NAME = "apriltagd";
static final String NATIVE_RELEASE_LIBRARY_NAME = "apriltag";
static boolean s_libraryLoaded = false;
static RuntimeLoader<AprilTagJNI> s_loader = null;
private static Logger logger = new Logger(AprilTagJNI.class, LogGroup.VisionModule);
public static synchronized void forceLoad() throws IOException {
if (s_libraryLoaded) return;
try {
// Ensure the lib directory has been created to receive the unpacked shared object
File libDirectory = Path.of("lib/").toFile();
if (!libDirectory.exists()) {
Files.createDirectory(libDirectory.toPath()).toFile();
}
// Pick the proper library based on development flags
String libBaseName = USE_DEBUG ? NATIVE_DEBUG_LIBRARY_NAME : NATIVE_RELEASE_LIBRARY_NAME;
String libFileName = System.mapLibraryName(libBaseName);
File libFile = Path.of("lib/" + libFileName).toFile();
// Always extract the library fresh
// Yes, technically, a hashing strategy should speed this up, but it's only a
// one-time, at-startup time hit. And not very big.
URL resourceURL;
String subfolder;
// TODO 64-bit Pi support
if (RuntimeDetector.isAthena()) {
subfolder = "athena";
} else if (RuntimeDetector.isAarch64()) {
subfolder = "aarch64";
} else if (RuntimeDetector.isRaspbian()) {
subfolder = "raspbian";
} else if (RuntimeDetector.isWindows()) {
subfolder = "win64";
} else if (RuntimeDetector.isLinux()) {
subfolder = "linux64";
} else if (RuntimeDetector.isMac()) {
subfolder = "mac";
} // NOT m1, afaict, lol
else {
logger.error("Could not determine platform! Cannot load Apriltag JNI");
return;
}
resourceURL =
AprilTagJNI.class.getResource(
"/nativelibraries/apriltag/" + subfolder + "/" + libFileName);
try (InputStream in = resourceURL.openStream()) {
// Remove the file if it already exists
if (libFile.exists()) Files.delete(libFile.toPath());
// Copy in a fresh resource
Files.copy(in, libFile.toPath());
}
// Actually load the library
System.load(libFile.getAbsolutePath());
s_libraryLoaded = true;
} catch (UnsatisfiedLinkError e) {
logger.error("Couldn't load apriltag shared object");
e.printStackTrace();
} catch (IOException ioe) {
logger.error("IO exception copying apriltag shared object");
ioe.printStackTrace();
}
if (!s_libraryLoaded) {
logger.error("Failed to load AprilTag Native Library!");
} else {
logger.info("AprilTag Native Library loaded successfully");
}
}
// Returns a pointer to a apriltag_detector_t
public static native long AprilTag_Create(
String fam, double decimate, double blur, int threads, boolean debug, boolean refine_edges);
// Destroy and free a previously created detector.
public static native long AprilTag_Destroy(long detector);
private static native Object[] AprilTag_Detect(
long detector,
long imgAddr,
int rows,
int cols,
boolean doPoseEstimation,
double tagWidth,
double fx,
double fy,
double cx,
double cy,
int nIters);
// Detect targets given a GRAY frame. Returns a pointer toa zarray
public static DetectionResult[] AprilTag_Detect(
long detector,
Mat img,
boolean doPoseEstimation,
double tagWidth,
double fx,
double fy,
double cx,
double cy,
int nIters) {
return (DetectionResult[])
AprilTag_Detect(
detector,
img.dataAddr(),
img.rows(),
img.cols(),
doPoseEstimation,
tagWidth,
fx,
fy,
cx,
cy,
nIters);
}
public static void main(String[] args) {
// System.loadLibrary("apriltag");
long detector = AprilTag_Create("tag36h11", 2, 2, 1, false, true);
// var buff = ByteBuffer.allocateDirect(1280 * 720);
// // try {
// // CameraServerCvJNI.forceLoad();
// // } catch (IOException e) {
// // // TODO Auto-generated catch block
// // e.printStackTrace();
// // }
// // PicamJNI.forceLoad();
// // TestUtils.loadLibraries();
// var img = Imgcodecs.imread("~/Downloads/TagFams.jpg");
// var ret = AprilTag_Detect(detector, 0, 720, 1280);
// System.out.println(detector);
// System.out.println(ret);
// System.out.println(List.of(ret));
}
}

190
photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java
View File

@@ -1,190 +0,0 @@
/*
Copyright (c) 2022 Photon Vision. All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of FIRST, WPILib, nor the names of other WPILib
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.photonvision.vision.apriltag;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.util.Arrays;
import org.photonvision.common.util.math.MathUtils;
public class DetectionResult {
public int getId() {
return id;
}
public int getHamming() {
return hamming;
}
public float getDecisionMargin() {
return decision_margin;
}
public void setDecisionMargin(float decision_margin) {
this.decision_margin = decision_margin;
}
public double[] getHomography() {
return homography;
}
public void setHomography(double[] homography) {
this.homography = homography;
}
public double getCenterX() {
return centerX;
}
public void setCenterX(double centerX) {
this.centerX = centerX;
}
public double getCenterY() {
return centerY;
}
public void setCenterY(double centerY) {
this.centerY = centerY;
}
public double[] getCorners() {
return corners;
}
public void setCorners(double[] corners) {
this.corners = corners;
}
public double getError1() {
return error1;
}
public double getError2() {
return error2;
}
public Transform3d getPoseResult1() {
return poseResult1;
}
public Transform3d getPoseResult2() {
return poseResult2;
}
int id;
int hamming;
float decision_margin;
double[] homography;
double centerX, centerY;
double[] corners;
Transform3d poseResult1;
double error1;
Transform3d poseResult2;
double error2;
public DetectionResult(
int id,
int hamming,
float decision_margin,
double[] homography,
double centerX,
double centerY,
double[] corners,
double[] pose1TransArr,
double[] pose1RotArr,
double err1,
double[] pose2TransArr,
double[] pose2RotArr,
double err2) {
this.id = id;
this.hamming = hamming;
this.decision_margin = decision_margin;
this.homography = homography;
this.centerX = centerX;
this.centerY = centerY;
this.corners = corners;
this.error1 = err1;
this.poseResult1 =
new Transform3d(
new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]),
new Rotation3d(MathUtils.orthogonalizeRotationMatrix(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr))));
this.error2 = err2;
this.poseResult2 =
new Transform3d(
new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]),
new Rotation3d(MathUtils.orthogonalizeRotationMatrix(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr))));
}
/**
* Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be
* ambiguous.
*/
public double getPoseAmbiguity() {
var min = Math.min(error1, error2);
var max = Math.max(error1, error2);
if (max > 0) {
return min / max;
} else {
return -1;
}
}
@Override
public String toString() {
return "DetectionResult [centerX="
+ centerX
+ ", centerY="
+ centerY
+ ", corners="
+ Arrays.toString(corners)
+ ", decision_margin="
+ decision_margin
+ ", error1="
+ error1
+ ", error2="
+ error2
+ ", hamming="
+ hamming
+ ", homography="
+ Arrays.toString(homography)
+ ", id="
+ id
+ ", poseResult1="
+ poseResult1
+ ", poseResult2="
+ poseResult2
+ "]";
}
}

1
photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java
View File

@@ -43,6 +43,7 @@ public class QuirkyCamera {
new QuirkyCamera(0x2000, 0x1415, CameraQuirk.Gain, CameraQuirk.FPSCap100), // PS3Eye
new QuirkyCamera(
-1, -1, "mmal service 16.1", CameraQuirk.PiCam), // PiCam (via V4L2, not zerocopy)
new QuirkyCamera(-1, -1, "unicam", CameraQuirk.PiCam), // PiCam (via V4L2, not zerocopy)
new QuirkyCamera(0x85B, 0x46D, CameraQuirk.AdjustableFocus) // Logitech C925-e
);

13
photon-core/src/main/java/org/photonvision/vision/frame/consumer/FileSaveFrameConsumer.java
View File

@@ -17,8 +17,8 @@
package org.photonvision.vision.frame.consumer;
import edu.wpi.first.networktables.BooleanEntry;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import java.io.File;
import java.text.DateFormat;
import java.text.SimpleDateFormat;
@@ -48,7 +48,7 @@ public class FileSaveFrameConsumer implements Consumer<Frame> {
private String camNickname;
private String fnamePrefix;
private final long CMD_RESET_TIME_MS = 500;
private final NetworkTableEntry entry;
private final BooleanEntry entry;
// Helps prevent race conditions between user set & auto-reset logic
private ReentrantLock lock;
@@ -58,15 +58,14 @@ public class FileSaveFrameConsumer implements Consumer<Frame> {
this.ntEntryName = streamPrefix + NT_SUFFIX;
this.rootTable = NetworkTablesManager.getInstance().kRootTable;
updateCameraNickname(camNickname);
entry = subTable.getEntry(ntEntryName);
entry.forceSetBoolean(false);
entry = subTable.getBooleanTopic(ntEntryName).getEntry(false);
this.logger = new Logger(FileSaveFrameConsumer.class, this.camNickname, LogGroup.General);
}
public void accept(Frame frame) {
if (frame != null && !frame.image.getMat().empty()) {
if (lock.tryLock()) {
boolean curCommand = entry.getBoolean(false);
boolean curCommand = entry.get(false);
if (curCommand && !prevCommand) {
Date now = new Date();
String savefile =
@@ -88,7 +87,7 @@ public class FileSaveFrameConsumer implements Consumer<Frame> {
} else if (!curCommand) {
// If the entry is currently false, set it again. This will make sure it shows up on the
// dashboard.
entry.forceSetBoolean(false);
entry.set(false);
}
prevCommand = curCommand;
@@ -106,7 +105,7 @@ public class FileSaveFrameConsumer implements Consumer<Frame> {
private void removeEntries() {
if (this.subTable != null) {
if (this.subTable.containsKey(ntEntryName)) {
this.subTable.delete(ntEntryName);
this.subTable.getEntry(ntEntryName).close();
}
}
}

2
photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagDetectionPipe.java
View File

@@ -17,10 +17,10 @@
package org.photonvision.vision.pipe.impl;
import edu.wpi.first.apriltag.jni.DetectionResult;
import java.util.List;
import org.opencv.core.Mat;
import org.photonvision.vision.apriltag.AprilTagDetector;
import org.photonvision.vision.apriltag.DetectionResult;
import org.photonvision.vision.pipe.CVPipe;
public class AprilTagDetectionPipe

2
photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View File

@@ -17,6 +17,7 @@
package org.photonvision.vision.pipeline;
import edu.wpi.first.apriltag.jni.DetectionResult;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.util.Units;
import java.util.ArrayList;
@@ -25,7 +26,6 @@ import org.opencv.core.Mat;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.raspi.PicamJNI;
import org.photonvision.vision.apriltag.AprilTagDetectorParams;
import org.photonvision.vision.apriltag.DetectionResult;
import org.photonvision.vision.camera.CameraQuirk;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.opencv.CVMat;

4
photon-core/src/main/java/org/photonvision/vision/processes/VisionModuleChangeSubscriber.java
View File

@@ -148,9 +148,13 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
curAdvSettings.offsetDualPointB = newPoint;
curAdvSettings.offsetDualPointBArea = latestTarget.getArea();
break;
default:
break;
}
}
break;
default:
break;
}
}
}

2
photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View File

@@ -16,6 +16,7 @@
*/
package org.photonvision.vision.target;
import edu.wpi.first.apriltag.jni.DetectionResult;
import edu.wpi.first.math.geometry.Transform3d;
import java.util.HashMap;
import java.util.List;
@@ -26,7 +27,6 @@ import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.RotatedRect;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.vision.apriltag.DetectionResult;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.*;

8
photon-core/src/test/java/org/photonvision/vision/frame/provider/FileFrameProviderTest.java
View File

@@ -19,8 +19,6 @@ package org.photonvision.vision.frame.provider;
import static org.junit.jupiter.api.Assertions.*;
import edu.wpi.first.cscore.CameraServerCvJNI;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Paths;
import org.junit.jupiter.api.BeforeAll;
@@ -31,11 +29,7 @@ import org.photonvision.vision.frame.Frame;
public class FileFrameProviderTest {
@BeforeAll
public static void initPath() {
try {
CameraServerCvJNI.forceLoad();
} catch (IOException e) {
e.printStackTrace();
}
TestUtils.loadLibraries();
}
@Test

107
photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java Normal file
View File

@@ -0,0 +1,107 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.vision.pipeline;
import edu.wpi.first.math.geometry.Translation3d;
import java.io.IOException;
import java.util.stream.Collectors;
import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
import org.photonvision.common.util.TestUtils;
import org.photonvision.vision.camera.QuirkyCamera;
import org.photonvision.vision.frame.provider.FileFrameProvider;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.target.TargetModel;
import org.photonvision.vision.target.TrackedTarget;
public class AprilTagTest {
@BeforeEach
public void Init() throws IOException {
TestUtils.loadLibraries();
}
@Test
public void testApriltagFacingCamera() {
var pipeline = new AprilTagPipeline();
pipeline.getSettings().inputShouldShow = true;
pipeline.getSettings().outputShouldDraw = true;
pipeline.getSettings().solvePNPEnabled = true;
pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
pipeline.getSettings().cornerDetectionUseConvexHulls = true;
pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;
var frameProvider =
new FileFrameProvider(
TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag1_640_480, false),
TestUtils.WPI2020Image.FOV,
TestUtils.get2020LifeCamCoeffs(false));
CVPipelineResult pipelineResult;
try {
pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
printTestResults(pipelineResult);
} catch (RuntimeException e) {
// For now, will throw coz rotation3d ctor
return;
}
// Draw on input
var outputPipe = new OutputStreamPipeline();
outputPipe.process(
pipelineResult.inputFrame,
pipelineResult.outputFrame,
pipeline.getSettings(),
pipelineResult.targets);
TestUtils.showImage(pipelineResult.inputFrame.image.getMat(), "Pipeline output", 999999);
// these numbers are not *accurate*, but they are known and expected
var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2);
Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2);
Assertions.assertEquals(0.0, pose.getTranslation().getY(), 0.2);
var objX = new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getY();
var objY = new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getZ();
var objZ = new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getX();
System.out.printf("Object x %.2f y %.2f z %.2f\n", objX, objY, objZ);
// We expect the object X to be forward, or -X in world space
Assertions.assertEquals(
-1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getX(), 0.1);
// We expect the object Y axis to be right, or negative-Y in world space
Assertions.assertEquals(
-1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getY(), 0.1);
// We expect the object Z axis to be up, or +Z in world space
Assertions.assertEquals(1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.1);
}
private static void printTestResults(CVPipelineResult pipelineResult) {
double fps = 1000 / pipelineResult.getLatencyMillis();
System.out.println(
"Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " " + "fps)");
System.out.println("Found " + pipelineResult.targets.size() + " valid targets");
System.out.println(
"Found targets at "
+ pipelineResult.targets.stream()
.map(TrackedTarget::getBestCameraToTarget3d)
.collect(Collectors.toList()));
}
}

6
photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
View File

@@ -27,7 +27,7 @@ import java.util.Arrays;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.lang3.tuple.Triple;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.Mat;
@@ -43,8 +43,8 @@ import org.photonvision.vision.pipe.impl.Calibrate3dPipe;
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
public class Calibrate3dPipeTest {
@BeforeEach
public void init() {
@BeforeAll
public static void init() {
TestUtils.loadLibraries();
}

4
photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java
View File

@@ -35,8 +35,8 @@ import org.photonvision.vision.frame.provider.FileFrameProvider;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
public class VisionModuleManagerTest {
@BeforeEach
public void init() {
@BeforeAll
public static void init() {
TestUtils.loadLibraries();
}

7
photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java
View File

@@ -22,18 +22,11 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.cscore.UsbCameraInfo;
import java.util.ArrayList;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.util.TestUtils;
public class VisionSourceManagerTest {
@BeforeEach
public void init() {
TestUtils.loadLibraries();
}
@Test
public void visionSourceTest() {
var inst = new VisionSourceManager();