From 308fd801d43e6de61e88f9d3a610f575adc48b32 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 15 Nov 2023 18:28:26 -0500 Subject: [PATCH 01/13] [photon-targeting][photon-lib] Add tests to the targeting classes and cleanup photon-lib & photon-targeting (#1007) * Make MultiTagPNPResult and PNPResult singular * add java tests * Formatting fixes * bring in the rest of the little stuff * final things * Formatting fixes * add multisubscriber back * Formatting fixes * make comments better about x and y relationship --- .../vision/pipe/impl/MultiTargetPNPPipe.java | 14 +- .../vision/pipeline/AprilTagPipeline.java | 4 +- .../vision/pipeline/ArucoPipeline.java | 4 +- .../vision/pipeline/Calibrate3dPipeline.java | 4 +- .../pipeline/result/CVPipelineResult.java | 14 +- photon-lib/build.gradle | 2 +- photon-lib/publish.gradle | 2 +- .../java/org/photonvision/PhotonCamera.java | 68 +-- .../org/photonvision/PhotonPoseEstimator.java | 6 +- .../java/org/photonvision/PhotonUtils.java | 2 +- .../simulation/PhotonCameraSim.java | 17 +- .../simulation/SimCameraProperties.java | 16 +- .../simulation/SimPhotonCamera.java | 4 +- .../simulation/SimVisionSystem.java | 4 +- .../photonvision/simulation/VideoSimUtil.java | 20 +- .../simulation/VisionSystemSim.java | 5 +- .../java/org/photonvision/PacketTest.java | 8 +- .../photonvision/PhotonPoseEstimatorTest.java | 4 +- .../org/photonvision/PhotonVersionTest.java | 2 +- .../photonvision/estimation/OpenCVHelp.java | 27 +- .../photonvision/estimation/TargetModel.java | 2 +- .../estimation/VisionEstimation.java | 24 +- ...Results.java => MultiTargetPNPResult.java} | 22 +- .../{PNPResults.java => PNPResult.java} | 21 +- .../targeting/PhotonPipelineResult.java | 20 +- .../targeting/PhotonTrackedTarget.java | 6 +- .../targeting/MultiTargetPNPResultTest.java | 66 +++ .../photonvision/targeting/PNPResultTest.java | 77 ++++ .../targeting/PhotonPipelineResultTest.java | 399 ++++++++++++++++++ .../targeting/PhotonTrackedTargetTest.java | 119 ++++++ .../targeting/TargetCornerTest.java | 41 ++ 31 files changed, 833 insertions(+), 191 deletions(-) rename photon-targeting/src/main/java/org/photonvision/targeting/{MultiTargetPNPResults.java => MultiTargetPNPResult.java} (80%) rename photon-targeting/src/main/java/org/photonvision/targeting/{PNPResults.java => PNPResult.java} (92%) create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java index 313d8a19e0..4a656aeb43 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java @@ -24,7 +24,7 @@ import org.photonvision.common.logging.Logger; import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; -import org.photonvision.targeting.MultiTargetPNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.pipe.CVPipe; import org.photonvision.vision.target.TrackedTarget; @@ -32,13 +32,13 @@ /** Estimate the camera pose given multiple Apriltag observations */ public class MultiTargetPNPPipe extends CVPipe< - List, MultiTargetPNPResults, MultiTargetPNPPipe.MultiTargetPNPPipeParams> { + List, MultiTargetPNPResult, MultiTargetPNPPipe.MultiTargetPNPPipeParams> { private static final Logger logger = new Logger(MultiTargetPNPPipe.class, LogGroup.VisionModule); private boolean hasWarned = false; @Override - protected MultiTargetPNPResults process(List targetList) { + protected MultiTargetPNPResult process(List targetList) { if (params == null || params.cameraCoefficients == null || params.cameraCoefficients.getCameraIntrinsicsMat() == null @@ -48,13 +48,13 @@ protected MultiTargetPNPResults process(List targetList) { "Cannot perform solvePNP an uncalibrated camera! Please calibrate this resolution..."); hasWarned = true; } - return new MultiTargetPNPResults(); + return new MultiTargetPNPResult(); } return calculateCameraInField(targetList); } - private MultiTargetPNPResults calculateCameraInField(List targetList) { + private MultiTargetPNPResult calculateCameraInField(List targetList) { // Find tag IDs that exist in the tag layout var tagIDsUsed = new ArrayList(); for (var target : targetList) { @@ -64,7 +64,7 @@ private MultiTargetPNPResults calculateCameraInField(List targetL // Only run with multiple targets if (tagIDsUsed.size() < 2) { - return new MultiTargetPNPResults(); + return new MultiTargetPNPResult(); } var estimatedPose = @@ -75,7 +75,7 @@ private MultiTargetPNPResults calculateCameraInField(List targetL params.atfl, params.targetModel); - return new MultiTargetPNPResults(estimatedPose, tagIDsUsed); + return new MultiTargetPNPResult(estimatedPose, tagIDsUsed); } public static class MultiTargetPNPPipeParams { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java index 1eb040616a..5d2abe1a07 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java @@ -31,7 +31,7 @@ import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.util.math.MathUtils; import org.photonvision.estimation.TargetModel; -import org.photonvision.targeting.MultiTargetPNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.vision.apriltag.AprilTagFamily; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -149,7 +149,7 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting } // Do multi-tag pose estimation - MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults(); + MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult(); if (settings.solvePNPEnabled && settings.doMultiTarget) { var multiTagOutput = multiTagPNPPipe.run(targetList); sumPipeNanosElapsed += multiTagOutput.nanosElapsed; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java index c463cb14e4..727eee69ff 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java @@ -47,7 +47,7 @@ import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.util.math.MathUtils; import org.photonvision.estimation.TargetModel; -import org.photonvision.targeting.MultiTargetPNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.vision.aruco.ArucoDetectionResult; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -170,7 +170,7 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings) } // Do multi-tag pose estimation - MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults(); + MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult(); if (settings.solvePNPEnabled && settings.doMultiTarget) { var multiTagOutput = multiTagPNPPipe.run(targetList); sumPipeNanosElapsed += multiTagOutput.nanosElapsed; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java index 95008e77f9..e1a3bc8aae 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java @@ -34,7 +34,7 @@ import org.photonvision.common.logging.Logger; import org.photonvision.common.util.SerializationUtils; import org.photonvision.common.util.file.FileUtils; -import org.photonvision.targeting.MultiTargetPNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -147,7 +147,7 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se sumPipeNanosElapsed, fps, // Unused but here in case Collections.emptyList(), - new MultiTargetPNPResults(), + new MultiTargetPNPResult(), new Frame( new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties)); } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java index 5993cc9531..4186fd91e5 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java @@ -20,7 +20,7 @@ import java.util.Collections; import java.util.List; import org.photonvision.common.util.math.MathUtils; -import org.photonvision.targeting.MultiTargetPNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.opencv.Releasable; import org.photonvision.vision.target.TrackedTarget; @@ -31,23 +31,23 @@ public class CVPipelineResult implements Releasable { public final double fps; public final List targets; public final Frame inputAndOutputFrame; - public MultiTargetPNPResults multiTagResult; + public MultiTargetPNPResult multiTagResult; public CVPipelineResult( double processingNanos, double fps, List targets, Frame inputFrame) { - this(processingNanos, fps, targets, new MultiTargetPNPResults(), inputFrame); + this(processingNanos, fps, targets, new MultiTargetPNPResult(), inputFrame); } public CVPipelineResult( double processingNanos, double fps, List targets, - MultiTargetPNPResults multiTagResults, + MultiTargetPNPResult multiTagResult, Frame inputFrame) { this.processingNanos = processingNanos; this.fps = fps; this.targets = targets != null ? targets : Collections.emptyList(); - this.multiTagResult = multiTagResults; + this.multiTagResult = multiTagResult; this.inputAndOutputFrame = inputFrame; } @@ -56,8 +56,8 @@ public CVPipelineResult( double processingNanos, double fps, List targets, - MultiTargetPNPResults multiTagResults) { - this(processingNanos, fps, targets, multiTagResults, null); + MultiTargetPNPResult multiTagResult) { + this(processingNanos, fps, targets, multiTagResult, null); } public boolean hasTargets() { diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 02cb0fd064..2ff63fe4b7 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -46,7 +46,7 @@ dependencies { implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get() implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() - implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); + implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get() // Junit testImplementation wpilibTools.deps.wpilibJava("cscore") diff --git a/photon-lib/publish.gradle b/photon-lib/publish.gradle index ca67c43b0e..bc825b81aa 100644 --- a/photon-lib/publish.gradle +++ b/photon-lib/publish.gradle @@ -152,7 +152,7 @@ publishing { tasks.withType(PublishToMavenRepository) { doFirst { - println("Publishing to " + repository.url); + println("Publishing to " + repository.url) } } diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index cd64965990..8b46a2c372 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -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; @@ -43,7 +41,6 @@ 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; @@ -73,9 +70,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() { @@ -99,7 +95,6 @@ public void close() { pipelineIndexRequest.close(); cameraIntrinsicsSubscriber.close(); cameraDistortionSubscriber.close(); - atflPublisher.close(); } private final String path; @@ -111,7 +106,7 @@ 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; @@ -119,12 +114,6 @@ public static void setVersionCheckEnabled(boolean 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. * @@ -159,11 +148,8 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { 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 = + // Existing is enough to make this multisubscriber do its thing + MultiSubscriber m_topicNameSubscriber = new MultiSubscriber( instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true)); } @@ -329,49 +315,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> getCameraMatrix() { var cameraMatrix = cameraIntrinsicsSubscriber.get(); if (cameraMatrix != null && cameraMatrix.length == 9) { @@ -428,7 +374,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( diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 32b4c7ba1f..dbf36292c2 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -443,15 +443,15 @@ private Optional multiTagOnRioStrategy( return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); } - var pnpResults = + var pnpResult = VisionEstimation.estimateCamPosePNP( cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel); // try fallback strategy if solvePNP fails for some reason - if (!pnpResults.isPresent) + if (!pnpResult.isPresent) return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); var best = new Pose3d() - .plus(pnpResults.best) // field-to-camera + .plus(pnpResult.best) // field-to-camera .plus(robotToCamera.inverse()); // field-to-robot return Optional.of( diff --git a/photon-lib/src/main/java/org/photonvision/PhotonUtils.java b/photon-lib/src/main/java/org/photonvision/PhotonUtils.java index 4d35c5ac29..90c407f555 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonUtils.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonUtils.java @@ -201,7 +201,7 @@ public static Rotation2d getYawToPose(Pose2d robotPose, Pose2d targetPose) { * * @param robotPose Pose of the robot * @param targetPose Pose of the target - * @return + * @return distance to the pose */ public static double getDistanceToPose(Pose2d robotPose, Pose2d targetPose) { return robotPose.getTranslation().getDistance(targetPose.getTranslation()); diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index f725837e13..b602b0194d 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -54,8 +54,8 @@ import org.photonvision.estimation.RotTrlTransform3d; import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; -import org.photonvision.targeting.MultiTargetPNPResults; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.targeting.PNPResult; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -80,7 +80,8 @@ public class PhotonCameraSim implements AutoCloseable { private double minTargetAreaPercent; private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest; - private AprilTagFieldLayout tagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + private final AprilTagFieldLayout tagLayout = + AprilTagFields.kDefaultField.loadAprilTagLayoutField(); // video stream simulation private final CvSource videoSimRaw; @@ -419,7 +420,7 @@ public PhotonPipelineResult process( // projected target can't be detected, skip to next if (!(canSeeCorners(noisyTargetCorners) && areaPercent >= minTargetAreaPercent)) continue; - var pnpSim = new PNPResults(); + var pnpSim = new PNPResult(); if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP pnpSim = OpenCVHelp.solvePNP_SQUARE( @@ -516,21 +517,21 @@ public PhotonPipelineResult process( } else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose); // calculate multitag results - var multitagResults = new MultiTargetPNPResults(); + var multitagResult = new MultiTargetPNPResult(); // TODO: Implement ATFL subscribing in backend // var tagLayout = cam.getAprilTagFieldLayout(); var visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(detectableTgts, tagLayout); if (visibleLayoutTags.size() > 1) { List usedIDs = visibleLayoutTags.stream().map(t -> t.ID).sorted().collect(Collectors.toList()); - var pnpResults = + var pnpResult = VisionEstimation.estimateCamPosePNP( prop.getIntrinsics(), prop.getDistCoeffs(), detectableTgts, tagLayout, TargetModel.kAprilTag16h5); - multitagResults = new MultiTargetPNPResults(pnpResults, usedIDs); + multitagResult = new MultiTargetPNPResult(pnpResult, usedIDs); } // sort target order @@ -539,7 +540,7 @@ public PhotonPipelineResult process( } // put this simulated data to NT - return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResults); + return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResult); } /** diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java index 35b0ad190f..6d157fb252 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -79,7 +79,7 @@ public class SimCameraProperties { private double avgLatencyMs = 0; private double latencyStdDevMs = 0; // util - private List viewplanes = new ArrayList<>(); + private final List viewplanes = new ArrayList<>(); /** Default constructor which is the same as {@link #PERFECT_90DEG} */ public SimCameraProperties() { @@ -215,8 +215,9 @@ public void setCalibration( 0, getPixelPitch(resHeight).plus(new Rotation2d(-Math.PI / 2)).getRadians(), 0)) }; viewplanes.clear(); - for (int i = 0; i < p.length; i++) { - viewplanes.add(new DMatrix3(p[i].getX(), p[i].getY(), p[i].getZ())); + for (Translation3d translation3d : p) { + viewplanes.add( + new DMatrix3(translation3d.getX(), translation3d.getY(), translation3d.getZ())); } } @@ -457,8 +458,7 @@ public Pair getVisibleLine( // check if the ends of the line segment are visible boolean aVisible = true; boolean bVisible = true; - for (int i = 0; i < viewplanes.size(); i++) { - var normal = viewplanes.get(i); + for (DMatrix3 normal : viewplanes) { double aVisibility = CommonOps_DDF3.dot(av, normal); if (aVisibility < 0) aVisible = false; double bVisibility = CommonOps_DDF3.dot(bv, normal); @@ -467,7 +467,7 @@ public Pair getVisibleLine( if (aVisibility <= 0 && bVisibility <= 0) return new Pair<>(null, null); } // both ends are inside frustum - if (aVisible && bVisible) return new Pair<>(Double.valueOf(0), Double.valueOf(1)); + if (aVisible && bVisible) return new Pair<>((double) 0, 1.0); // parametrized (t=0 at a, t=1 at b) intersections with viewplanes double[] intersections = {Double.NaN, Double.NaN, Double.NaN, Double.NaN}; @@ -546,8 +546,8 @@ public Pair getVisibleLine( } // one viewplane intersection else if (!Double.isNaN(inter1)) { - if (aVisible) return new Pair<>(Double.valueOf(0), inter1); - if (bVisible) return new Pair<>(inter1, Double.valueOf(1)); + if (aVisible) return new Pair<>((double) 0, inter1); + if (bVisible) return new Pair<>(inter1, 1.0); return new Pair<>(inter1, null); } // no intersections diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java index 58e3e2fcf7..5eb73b35eb 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java @@ -35,7 +35,7 @@ import org.photonvision.PhotonTargetSortMode; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.networktables.NTTopicSet; -import org.photonvision.targeting.MultiTargetPNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -142,7 +142,7 @@ public void submitProcessedFrame( } PhotonPipelineResult newResult = - new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResults()); + new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResult()); var newPacket = new Packet(newResult.getPacketSize()); newResult.populatePacket(newPacket); ts.rawBytesEntry.set(newPacket.getData()); diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java index 52756f4727..df16412de5 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java @@ -120,9 +120,7 @@ public SimVisionSystem( */ public void addSimVisionTarget(SimVisionTarget target) { tgtList.add(target); - dbgField - .getObject("Target " + Integer.toString(target.targetID)) - .setPose(target.targetPose.toPose2d()); + dbgField.getObject("Target " + target.targetID).setPose(target.targetPose.toPose2d()); } /** diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java index 1bbe921006..8b5c787b78 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java @@ -374,8 +374,8 @@ public static void drawTagDetection(int id, Point[] dstPoints, Mat destination) /** * Set the field dimensions that are used for drawing the field wireframe. * - * @param fieldLengthMeters - * @param fieldWidthMeters + * @param fieldLengthMeters field length in meters (x direction) + * @param fieldWidthMeters field width in meters (y direction) */ public static void setFieldDimensionsMeters(double fieldLengthMeters, double fieldWidthMeters) { fieldLength = fieldLengthMeters; @@ -501,8 +501,8 @@ public static List polyFrom3dLines( if (inter.getSecond() == null) continue; // cull line to the inside of the camera fulcrum - double inter1 = inter.getFirst().doubleValue(); - double inter2 = inter.getSecond().doubleValue(); + double inter1 = inter.getFirst(); + double inter2 = inter.getSecond(); var baseDelta = ptb.minus(pta); var old_pta = pta; if (inter1 > 0) pta = old_pta.plus(baseDelta.times(inter1)); @@ -510,11 +510,11 @@ public static List polyFrom3dLines( baseDelta = ptb.minus(pta); // project points into 2d - var poly = new ArrayList(); - poly.addAll( - Arrays.asList( - OpenCVHelp.projectPoints( - prop.getIntrinsics(), prop.getDistCoeffs(), camRt, List.of(pta, ptb)))); + var poly = + new ArrayList<>( + Arrays.asList( + OpenCVHelp.projectPoints( + prop.getIntrinsics(), prop.getDistCoeffs(), camRt, List.of(pta, ptb)))); var pxa = poly.get(0); var pxb = poly.get(1); @@ -526,7 +526,7 @@ public static List polyFrom3dLines( for (int j = 0; j < subdivisions; j++) { subPts.add(pta.plus(subDelta.times(j + 1))); } - if (subPts.size() > 0) { + if (!subPts.isEmpty()) { poly.addAll( 1, Arrays.asList( diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java index 573ad77e91..eb4720a21a 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java @@ -176,8 +176,7 @@ public Optional getCameraPose(PhotonCameraSim cameraSim) { */ public Optional getCameraPose(PhotonCameraSim cameraSim, double timeSeconds) { var robotToCamera = getRobotToCamera(cameraSim, timeSeconds); - if (robotToCamera.isEmpty()) return Optional.empty(); - return Optional.of(getRobotPose(timeSeconds).plus(robotToCamera.get())); + return robotToCamera.map(transform3d -> getRobotPose(timeSeconds).plus(transform3d)); } /** @@ -405,6 +404,6 @@ public void update(Pose3d robotPoseMeters) { } } if (processed) dbgField.getObject("visibleTargetPoses").setPoses(visTgtPoses2d); - if (cameraPoses2d.size() != 0) dbgField.getObject("cameras").setPoses(cameraPoses2d); + if (!cameraPoses2d.isEmpty()) dbgField.getObject("cameras").setPoses(cameraPoses2d); } } diff --git a/photon-lib/src/test/java/org/photonvision/PacketTest.java b/photon-lib/src/test/java/org/photonvision/PacketTest.java index c29a629b5b..a2f98a27c2 100644 --- a/photon-lib/src/test/java/org/photonvision/PacketTest.java +++ b/photon-lib/src/test/java/org/photonvision/PacketTest.java @@ -30,8 +30,8 @@ import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.Test; import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.targeting.MultiTargetPNPResults; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.targeting.PNPResult; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; @@ -174,8 +174,8 @@ public void testMultiTargetSerde() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8)))), - new MultiTargetPNPResults( - new PNPResults( + new MultiTargetPNPResult( + new PNPResult( new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), List.of(1, 2, 3))); diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 1b386cc747..4eaffa0fea 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -69,7 +69,7 @@ public static void init() { e.printStackTrace(); } - List tagList = new ArrayList(2); + List tagList = new ArrayList<>(2); tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d()))); tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d()))); double fieldLength = Units.feetToMeters(54.0); @@ -619,7 +619,7 @@ void averageBestPoses() { assertEquals(2.15, pose.getZ(), .01); } - private class PhotonCameraInjector extends PhotonCamera { + private static class PhotonCameraInjector extends PhotonCamera { public PhotonCameraInjector() { super("Test"); } diff --git a/photon-lib/src/test/java/org/photonvision/PhotonVersionTest.java b/photon-lib/src/test/java/org/photonvision/PhotonVersionTest.java index 6dc5a5854b..3009de49e9 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonVersionTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonVersionTest.java @@ -30,7 +30,7 @@ import org.junit.jupiter.api.Test; public class PhotonVersionTest { - public static final boolean versionMatches(String versionString, String other) { + public static boolean versionMatches(String versionString, String other) { String c = versionString; Pattern p = Pattern.compile("v[0-9]+.[0-9]+.[0-9]+"); Matcher m = p.matcher(c); diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 84e5d8d7cc..ed53a17b80 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -45,12 +45,12 @@ import org.opencv.core.Rect; import org.opencv.core.RotatedRect; import org.opencv.imgproc.Imgproc; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.PNPResult; import org.photonvision.targeting.TargetCorner; public final class OpenCVHelp { - private static Rotation3d NWU_TO_EDN; - private static Rotation3d EDN_TO_NWU; + private static final Rotation3d NWU_TO_EDN; + private static final Rotation3d EDN_TO_NWU; // Creating a cscore object is sufficient to load opencv, per // https://www.chiefdelphi.com/t/unsatisfied-link-error-when-simulating-java-robot-code-using-opencv/426731/4 @@ -170,8 +170,8 @@ public static Point[] cornersToPoints(TargetCorner... corners) { public static List pointsToCorners(Point... points) { var corners = new ArrayList(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; } @@ -199,7 +199,7 @@ public static List pointsToCorners(MatOfPoint2f matInput) { *

({1,2,3}, true, 1) == {3,2,1} * * @param Element type - * @param elements + * @param elements list elements * @param backwards If indexing should happen in reverse (0, size-1, size-2, ...) * @param shiftStart How much the inital index should be shifted (instead of starting at index 0, * start at shiftStart, negated if backwards) @@ -401,7 +401,7 @@ public static Point[] getConvexHull(Point[] points) { * @return The resulting transformation that maps the camera pose to the target pose and the * ambiguity if an alternate solution is available. */ - public static PNPResults solvePNP_SQUARE( + public static PNPResult solvePNP_SQUARE( Matrix cameraMatrix, Matrix distCoeffs, List modelTrls, @@ -466,15 +466,14 @@ public static PNPResults solvePNP_SQUARE( // check if solvePnP failed with NaN results and retrying failed if (Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result"); - if (alt != null) - return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]); - else return new PNPResults(best, errors[0]); + if (alt != null) return new PNPResult(best, alt, errors[0] / errors[1], errors[0], errors[1]); + else return new PNPResult(best, errors[0]); } // solvePnP failed catch (Exception e) { System.err.println("SolvePNP_SQUARE failed!"); e.printStackTrace(); - return new PNPResults(); + return new PNPResult(); } finally { // release our Mats from native memory objectMat.release(); @@ -509,7 +508,7 @@ public static PNPResults solvePNP_SQUARE( * model points are supplied relative to the origin, this transformation brings the camera to * the origin. */ - public static PNPResults solvePNP_SQPNP( + public static PNPResult solvePNP_SQPNP( Matrix cameraMatrix, Matrix distCoeffs, List objectTrls, @@ -558,11 +557,11 @@ public static PNPResults solvePNP_SQPNP( // check if solvePnP failed with NaN results if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result"); - return new PNPResults(best, error[0]); + return new PNPResult(best, error[0]); } catch (Exception e) { System.err.println("SolvePNP_SQPNP failed!"); e.printStackTrace(); - return new PNPResults(); + return new PNPResult(); } } } diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java index f945d2c806..aa77a1e5d9 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java @@ -151,7 +151,7 @@ public TargetModel(List vertices) { */ public List 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()); } /** diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java index 9ad6e0fc4e..e1e270fc3c 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -29,7 +29,7 @@ import java.util.Objects; import java.util.stream.Collectors; import org.opencv.core.Point; -import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.PNPResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; @@ -64,9 +64,9 @@ public static List getVisibleLayoutTags( * @param visTags The visible tags reported by PV. Non-tag targets are automatically excluded. * @param tagLayout The known tag layout on the field * @return The transformation that maps the field origin to the camera pose. Ensure the {@link - * PNPResults} are present before utilizing them. + * PNPResult} are present before utilizing them. */ - public static PNPResults estimateCamPosePNP( + public static PNPResult estimateCamPosePNP( Matrix cameraMatrix, Matrix distCoeffs, List visTags, @@ -74,9 +74,9 @@ public static PNPResults estimateCamPosePNP( TargetModel tagModel) { if (tagLayout == null || visTags == null - || tagLayout.getTags().size() == 0 - || visTags.size() == 0) { - return new PNPResults(); + || tagLayout.getTags().isEmpty() + || visTags.isEmpty()) { + return new PNPResult(); } var corners = new ArrayList(); @@ -92,8 +92,8 @@ public static PNPResults estimateCamPosePNP( corners.addAll(tgt.getDetectedCorners()); }); } - if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { - return new PNPResults(); + if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) { + return new PNPResult(); } Point[] points = OpenCVHelp.cornersToPoints(corners); @@ -101,14 +101,14 @@ public static PNPResults estimateCamPosePNP( if (knownTags.size() == 1) { var camToTag = OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, tagModel.vertices, points); - if (!camToTag.isPresent) return new PNPResults(); + if (!camToTag.isPresent) return new PNPResult(); var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse()); var altPose = new Pose3d(); if (camToTag.ambiguity != 0) altPose = knownTags.get(0).pose.transformBy(camToTag.alt.inverse()); var o = new Pose3d(); - return new PNPResults( + return new PNPResult( new Transform3d(o, bestPose), new Transform3d(o, altPose), camToTag.ambiguity, @@ -120,8 +120,8 @@ public static PNPResults estimateCamPosePNP( var objectTrls = new ArrayList(); for (var tag : knownTags) objectTrls.addAll(tagModel.getFieldVertices(tag.pose)); var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points); - if (!camToOrigin.isPresent) return new PNPResults(); - return new PNPResults( + if (!camToOrigin.isPresent) return new PNPResult(); + return new PNPResult( camToOrigin.best.inverse(), camToOrigin.alt.inverse(), camToOrigin.ambiguity, diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java similarity index 80% rename from photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java rename to photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java index 453c0a57e5..a3ddaaa6f0 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java @@ -21,30 +21,30 @@ import java.util.List; import org.photonvision.common.dataflow.structures.Packet; -public class MultiTargetPNPResults { +public class MultiTargetPNPResult { // Seeing 32 apriltags at once seems like a sane limit private static final int MAX_IDS = 32; // pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally) - public static final int PACK_SIZE_BYTES = PNPResults.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS); + public static final int PACK_SIZE_BYTES = PNPResult.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS); - public PNPResults estimatedPose = new PNPResults(); + public PNPResult estimatedPose = new PNPResult(); public List fiducialIDsUsed = List.of(); - public MultiTargetPNPResults() {} + public MultiTargetPNPResult() {} - public MultiTargetPNPResults(PNPResults results, List ids) { + public MultiTargetPNPResult(PNPResult results, List ids) { estimatedPose = results; fiducialIDsUsed = ids; } - public static MultiTargetPNPResults createFromPacket(Packet packet) { - var results = PNPResults.createFromPacket(packet); + public static MultiTargetPNPResult createFromPacket(Packet packet) { + var results = PNPResult.createFromPacket(packet); var ids = new ArrayList(MAX_IDS); for (int i = 0; i < MAX_IDS; i++) { - int targetId = (int) packet.decodeShort(); + int targetId = packet.decodeShort(); if (targetId > -1) ids.add(targetId); } - return new MultiTargetPNPResults(results, ids); + return new MultiTargetPNPResult(results, ids); } public void populatePacket(Packet packet) { @@ -72,7 +72,7 @@ public boolean equals(Object obj) { if (this == obj) return true; if (obj == null) return false; if (getClass() != obj.getClass()) return false; - MultiTargetPNPResults other = (MultiTargetPNPResults) obj; + MultiTargetPNPResult other = (MultiTargetPNPResult) obj; if (estimatedPose == null) { if (other.estimatedPose != null) return false; } else if (!estimatedPose.equals(other.estimatedPose)) return false; @@ -84,7 +84,7 @@ public boolean equals(Object obj) { @Override public String toString() { - return "MultiTargetPNPResults [estimatedPose=" + return "MultiTargetPNPResult [estimatedPose=" + estimatedPose + ", fiducialIDsUsed=" + fiducialIDsUsed diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java similarity index 92% rename from photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java rename to photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java index 11a77547fc..ab7aa5f46f 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java @@ -30,7 +30,7 @@ *

Note that the coordinate frame of these transforms depends on the implementing solvePnP * method. */ -public class PNPResults { +public class PNPResult { /** * If this result is valid. A false value indicates there was an error in estimation, and this * result should not be used. @@ -59,7 +59,7 @@ public class PNPResults { public final double ambiguity; /** An empty (invalid) result. */ - public PNPResults() { + public PNPResult() { this.isPresent = false; this.best = new Transform3d(); this.alt = new Transform3d(); @@ -68,11 +68,11 @@ public PNPResults() { this.altReprojErr = 0; } - public PNPResults(Transform3d best, double bestReprojErr) { + public PNPResult(Transform3d best, double bestReprojErr) { this(best, best, 0, bestReprojErr, bestReprojErr); } - public PNPResults( + public PNPResult( Transform3d best, Transform3d alt, double ambiguity, @@ -88,7 +88,7 @@ public PNPResults( public static final int PACK_SIZE_BYTES = 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3); - public static PNPResults createFromPacket(Packet packet) { + public static PNPResult createFromPacket(Packet packet) { var present = packet.decodeBoolean(); var best = PacketUtils.decodeTransform(packet); var alt = PacketUtils.decodeTransform(packet); @@ -96,20 +96,19 @@ public static PNPResults createFromPacket(Packet packet) { var altEr = packet.decodeDouble(); var ambiguity = packet.decodeDouble(); if (present) { - return new PNPResults(best, alt, ambiguity, bestEr, altEr); + return new PNPResult(best, alt, ambiguity, bestEr, altEr); } else { - return new PNPResults(); + return new PNPResult(); } } - public Packet populatePacket(Packet packet) { + public void populatePacket(Packet packet) { packet.encode(isPresent); PacketUtils.encodeTransform(packet, best); PacketUtils.encodeTransform(packet, alt); packet.encode(bestReprojErr); packet.encode(altReprojErr); packet.encode(ambiguity); - return packet; } @Override @@ -134,7 +133,7 @@ public boolean equals(Object obj) { if (this == obj) return true; if (obj == null) return false; if (getClass() != obj.getClass()) return false; - PNPResults other = (PNPResults) obj; + PNPResult other = (PNPResult) obj; if (isPresent != other.isPresent) return false; if (best == null) { if (other.best != null) return false; @@ -153,7 +152,7 @@ public boolean equals(Object obj) { @Override public String toString() { - return "PNPResults [isPresent=" + return "PNPResult [isPresent=" + isPresent + ", best=" + best diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 4472078f8a..9a8ce65db9 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -35,7 +35,7 @@ public class PhotonPipelineResult { private double timestampSeconds = -1; // Multi-tag result - private MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults(); + private MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult(); /** Constructs an empty pipeline result. */ public PhotonPipelineResult() {} @@ -59,7 +59,7 @@ public PhotonPipelineResult(double latencyMillis, List targ * @param result Result from multi-target PNP. */ public PhotonPipelineResult( - double latencyMillis, List targets, MultiTargetPNPResults result) { + double latencyMillis, List targets, MultiTargetPNPResult result) { this.latencyMillis = latencyMillis; this.targets.addAll(targets); this.multiTagResult = result; @@ -73,7 +73,7 @@ public PhotonPipelineResult( public int getPacketSize() { return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES + 8 // latency - + MultiTargetPNPResults.PACK_SIZE_BYTES + + MultiTargetPNPResult.PACK_SIZE_BYTES + 1; // target count } @@ -130,7 +130,7 @@ public void setTimestampSeconds(double timestampSeconds) { * @return Whether the pipeline has targets. */ public boolean hasTargets() { - return targets.size() > 0; + return !targets.isEmpty(); } /** @@ -143,10 +143,10 @@ public List getTargets() { } /** - * Return the latest mulit-target result. Be sure to check + * Return the latest multi-target result. Be sure to check * getMultiTagResult().estimatedPose.isPresent before using the pose estimate! */ - public MultiTargetPNPResults getMultiTagResult() { + public MultiTargetPNPResult getMultiTagResult() { return multiTagResult; } @@ -159,7 +159,7 @@ public MultiTargetPNPResults getMultiTagResult() { public Packet createFromPacket(Packet packet) { // Decode latency, existence of targets, and number of targets. latencyMillis = packet.decodeDouble(); - this.multiTagResult = MultiTargetPNPResults.createFromPacket(packet); + this.multiTagResult = MultiTargetPNPResult.createFromPacket(packet); byte targetCount = packet.decodeByte(); targets.clear(); @@ -197,7 +197,7 @@ public Packet populatePacket(Packet packet) { public int hashCode() { final int prime = 31; int result = 1; - result = prime * result + ((targets == null) ? 0 : targets.hashCode()); + result = prime * result + targets.hashCode(); long temp; temp = Double.doubleToLongBits(latencyMillis); result = prime * result + (int) (temp ^ (temp >>> 32)); @@ -213,9 +213,7 @@ public boolean equals(Object obj) { if (obj == null) return false; if (getClass() != obj.getClass()) return false; PhotonPipelineResult other = (PhotonPipelineResult) obj; - if (targets == null) { - if (other.targets != null) return false; - } else if (!targets.equals(other.targets)) return false; + if (!targets.equals(other.targets)) return false; if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis)) return false; if (Double.doubleToLongBits(timestampSeconds) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 40263f5cc5..dc847ec5fc 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -198,9 +198,9 @@ public boolean equals(Object obj) { private static void encodeList(Packet packet, List list) { packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE)); - for (int i = 0; i < list.size(); i++) { - packet.encode(list.get(i).x); - packet.encode(list.get(i).y); + for (TargetCorner targetCorner : list) { + packet.encode(targetCorner.x); + packet.encode(targetCorner.y); } } diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java new file mode 100644 index 0000000000..4e28237ac5 --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java @@ -0,0 +1,66 @@ +/* + * 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 . + */ + +package org.photonvision.targeting; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class MultiTargetPNPResultTest { + @Test + public void equalityTest() { + var a = new MultiTargetPNPResult(); + var b = new MultiTargetPNPResult(); + assertEquals(a, b); + + a = + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3)); + + b = + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3)); + + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 8, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(3, 4, 7)); + var b = + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3)); + + assertNotEquals(a, b); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java new file mode 100644 index 0000000000..9f86473eaa --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java @@ -0,0 +1,77 @@ +/* + * 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 . + */ + +package org.photonvision.targeting; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import org.junit.jupiter.api.Test; + +public class PNPResultTest { + @Test + public void equalityTest() { + var a = new PNPResult(); + var b = new PNPResult(); + assertEquals(a, b); + + a = new PNPResult(new Transform3d(0, 1, 2, new Rotation3d()), 0.0); + b = new PNPResult(new Transform3d(0, 1, 2, new Rotation3d()), 0.0); + assertEquals(a, b); + + a = + new PNPResult( + new Transform3d(0, 1, 2, new Rotation3d()), + new Transform3d(3, 4, 5, new Rotation3d()), + 0.5, + 0.1, + 0.1); + b = + new PNPResult( + new Transform3d(0, 1, 2, new Rotation3d()), + new Transform3d(3, 4, 5, new Rotation3d()), + 0.5, + 0.1, + 0.1); + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = new PNPResult(new Transform3d(0, 1, 2, new Rotation3d()), 0.0); + var b = new PNPResult(new Transform3d(3, 4, 5, new Rotation3d()), 0.1); + assertNotEquals(a, b); + + a = + new PNPResult( + new Transform3d(3, 4, 5, new Rotation3d()), + new Transform3d(0, 1, 2, new Rotation3d()), + 0.5, + 0.1, + 0.1); + b = + new PNPResult( + new Transform3d(3, 4, 5, new Rotation3d()), + new Transform3d(0, 1, 2, new Rotation3d()), + 0.5, + 0.1, + 0.2); + assertNotEquals(a, b); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java new file mode 100644 index 0000000000..72dacda168 --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java @@ -0,0 +1,399 @@ +/* + * 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 . + */ + +package org.photonvision.targeting; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class PhotonPipelineResultTest { + @Test + public void equalityTest() { + var a = new PhotonPipelineResult(); + var b = new PhotonPipelineResult(); + assertEquals(a, b); + + a = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + b = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + assertEquals(a, b); + + a = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3))); + b = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3))); + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 7.0, + 2.0, + 1.0, + -9.0, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + var b = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 7.0, + 2.0, + 1.0, + -9.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + assertNotEquals(a, b); + + a = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 8, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(3, 4, 7))); + b = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3))); + assertNotEquals(a, b); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java new file mode 100644 index 0000000000..5d42a2b68f --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java @@ -0,0 +1,119 @@ +/* + * 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 . + */ + +package org.photonvision.targeting; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class PhotonTrackedTargetTest { + @Test + public void equalityTest() { + var a = + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + var b = + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + var b = + new PhotonTrackedTarget( + 7.0, + 2.0, + 1.0, + -9.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + assertNotEquals(a, b); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java new file mode 100644 index 0000000000..b813fb7afb --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java @@ -0,0 +1,41 @@ +/* + * 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 . + */ + +package org.photonvision.targeting; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import org.junit.jupiter.api.Test; + +public class TargetCornerTest { + @Test + public void equalityTest() { + var a = new TargetCorner(0, 1); + var b = new TargetCorner(0, 1); + + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = new TargetCorner(0, 1); + var b = new TargetCorner(2, 4); + + assertNotEquals(a, b); + } +} From 994ea1e76bfd883455b9ed94db2306c7ff3c90e7 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 19 Nov 2023 15:16:22 -0500 Subject: [PATCH 02/13] [photon-targeting] Move C++ targeting classes to photon-targetting (#1009) * add classes to targeting and update gradle * rename me * Finish cleanup * Formatting fixes * just use common.gradle * Update build.gradle * Update config.gradle * remove typo * simplify * Add Packet Headers * move simulation classes into simulation folder * draw in dependency * fix * Everything working minus tests cause im lazy * formatting fixes REMEMBER TO REMOVE UNUSED IMPORTS, IM JUST TOO LAZY TO CHECK RN * move packet test to targeting * Formatting fixes * remove TargetCorner from c++ im not 100% sure but just doing std::pair is sufficient and shouldnt conflict with protobuf * think i added packet * fix namespace issue * organize imports in photon-targeting * Formatting fixes * remove ctors * fix typo * Add PNP and Multitag packet tests * revert TargetCorner class * Add Test placeholders * remove annoying print * Reorganize build and publish process channeling inner Thad * add targeting as flag * Update config.gradle * fix issue with platform binaries not building * Update photonlib.json.in casing still needs to be checked * add minimum level back * add back UTF-8 encoding of javadoc * simplify publish model for photon-lib * fix windows symbol generation * formatting fixes * move task from main gradle to config * Update config.gradle --- photon-lib/build.gradle | 147 +++++-------- photon-lib/publish.gradle | 206 ------------------ photon-lib/src/generate/photonlib.json.in | 23 +- .../{photonlib => photon}/PhotonCamera.cpp | 10 +- .../PhotonPoseEstimator.cpp | 26 +-- .../cpp/photonlib/MultiTargetPNPResult.cpp | 113 ---------- .../cpp/photonlib/PhotonPipelineResult.cpp | 71 ------ .../{photonlib => photon}/PhotonCamera.h | 10 +- .../PhotonPoseEstimator.h | 12 +- .../PhotonTargetSortMode.h | 10 +- .../{photonlib => photon}/PhotonUtils.h | 10 +- .../simulation}/SimPhotonCamera.h | 13 +- .../simulation}/SimVisionSystem.h | 9 +- .../simulation}/SimVisionTarget.h | 10 +- .../include/photonlib/MultiTargetPNPResult.h | 61 ------ .../native/cpp/PhotonPoseEstimatorTest.cpp | 106 ++++----- .../src/test/native/cpp/PhotonUtilsTest.cpp | 2 +- .../test/native/cpp/SimVisionSystemTest.cpp | 78 ++++--- photon-targeting/build.gradle | 64 +++--- photon-targeting/publish.gradle | 32 --- photon-targeting/settings.gradle | 7 +- .../photon/targeting/MultiTargetPNPResult.cpp | 56 +++++ .../native/cpp/photon/targeting/PNPResult.cpp | 77 +++++++ .../photon/targeting/PhotonPipelineResult.cpp | 67 ++++++ .../photon/targeting}/PhotonTrackedTarget.cpp | 40 ++-- .../photon/dataflow/structures}/Packet.h | 34 ++- .../photon/targeting/MultiTargetPNPResult.h | 37 ++++ .../include/photon/targeting/PNPResult.h | 45 ++++ .../photon/targeting}/PhotonPipelineResult.h | 65 +++--- .../photon/targeting}/PhotonTrackedTarget.h | 47 ++-- .../src/test/native/cpp/PacketTest.cpp | 88 ++++---- photon-targeting/src/test/native/cpp/main.cpp | 23 ++ .../targeting/MultiTargetPNPResultTest.cpp | 25 +++ .../native/cpp/targeting/PNPResultTest.cpp | 25 +++ .../targeting/PhotonPipelineResultTest.cpp | 25 +++ .../cpp/targeting/PhotonTrackedTargetTest.cpp | 25 +++ .../aimandrange/src/main/cpp/Robot.cpp | 4 +- .../aimandrange/src/main/include/Robot.h | 4 +- .../aimattarget/src/main/cpp/Robot.cpp | 4 +- .../aimattarget/src/main/include/Robot.h | 4 +- .../src/main/include/PhotonCameraWrapper.h | 12 +- .../apriltagExample/src/main/include/Robot.h | 2 +- .../getinrange/src/main/cpp/Robot.cpp | 6 +- .../getinrange/src/main/include/Robot.h | 4 +- shared/PhotonVersion.h.in | 2 +- shared/common.gradle | 3 +- shared/config.gradle | 16 ++ shared/javacommon.gradle | 115 ++++++++++ shared/publish.gradle | 81 +++++++ shared/setupBuild.gradle | 76 +++++++ 50 files changed, 1127 insertions(+), 905 deletions(-) delete mode 100644 photon-lib/publish.gradle rename photon-lib/src/main/native/cpp/{photonlib => photon}/PhotonCamera.cpp (97%) rename photon-lib/src/main/native/cpp/{photonlib => photon}/PhotonPoseEstimator.cpp (96%) delete mode 100644 photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp delete mode 100644 photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp rename photon-lib/src/main/native/include/{photonlib => photon}/PhotonCamera.h (95%) rename photon-lib/src/main/native/include/{photonlib => photon}/PhotonPoseEstimator.h (96%) rename photon-lib/src/main/native/include/{photonlib => photon}/PhotonTargetSortMode.h (90%) rename photon-lib/src/main/native/include/{photonlib => photon}/PhotonUtils.h (96%) rename photon-lib/src/main/native/include/{photonlib => photon/simulation}/SimPhotonCamera.h (93%) rename photon-lib/src/main/native/include/{photonlib => photon/simulation}/SimVisionSystem.h (97%) rename photon-lib/src/main/native/include/{photonlib => photon/simulation}/SimVisionTarget.h (88%) delete mode 100644 photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h delete mode 100644 photon-targeting/publish.gradle create mode 100644 photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp create mode 100644 photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp create mode 100644 photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp rename {photon-lib/src/main/native/cpp/photonlib => photon-targeting/src/main/native/cpp/photon/targeting}/PhotonTrackedTarget.cpp (76%) rename {photon-lib/src/main/native/include/photonlib => photon-targeting/src/main/native/include/photon/dataflow/structures}/Packet.h (69%) create mode 100644 photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h create mode 100644 photon-targeting/src/main/native/include/photon/targeting/PNPResult.h rename {photon-lib/src/main/native/include/photonlib => photon-targeting/src/main/native/include/photon/targeting}/PhotonPipelineResult.h (60%) rename {photon-lib/src/main/native/include/photonlib => photon-targeting/src/main/native/include/photon/targeting}/PhotonTrackedTarget.h (70%) rename {photon-lib => photon-targeting}/src/test/native/cpp/PacketTest.cpp (50%) create mode 100644 photon-targeting/src/test/native/cpp/main.cpp create mode 100644 photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp create mode 100644 photon-targeting/src/test/native/cpp/targeting/PNPResultTest.cpp create mode 100644 photon-targeting/src/test/native/cpp/targeting/PhotonPipelineResultTest.cpp create mode 100644 photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp create mode 100644 shared/javacommon.gradle create mode 100644 shared/publish.gradle create mode 100644 shared/setupBuild.gradle diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 2ff63fe4b7..e5d880fb4a 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -1,111 +1,73 @@ plugins { - id "java" - id 'cpp' - id "google-test-test-suite" id 'edu.wpi.first.WpilibTools' version '1.3.0' } +apply plugin: "edu.wpi.first.NativeUtils" + import java.nio.file.Path -apply plugin: "edu.wpi.first.NativeUtils" +ext { + nativeName = "photonlib" + includePhotonTargeting = true +} -apply from: "${rootDir}/shared/config.gradle" +apply from: "${rootDir}/shared/setupBuild.gradle" apply from: "${rootDir}/versioningHelper.gradle" -test { - useJUnitPlatform() -} -java { - sourceCompatibility = JavaVersion.VERSION_11 - targetCompatibility = JavaVersion.VERSION_11 +wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get() + +def nativeConfigName = 'wpilibNatives' +def nativeConfig = configurations.create(nativeConfigName) + +def nativeTasks = wpilibTools.createExtractionTasks { + configurationName = nativeConfigName } -wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get() -println("Building for WPILib ${wpilibTools.deps.wpilibVersion}") +nativeTasks.addToSourceSetResources(sourceSets.main) + +nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal") +nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) -// Apply Java configuration dependencies { implementation project(":photon-targeting") - // WPILib deps implementation wpilibTools.deps.wpilibJava("wpiutil") implementation wpilibTools.deps.wpilibJava("cameraserver") implementation wpilibTools.deps.wpilibJava("cscore") - implementation wpilibTools.deps.wpilibJava("wpimath") implementation wpilibTools.deps.wpilibJava("wpinet") - implementation wpilibTools.deps.wpilibJava("hal") + implementation wpilibTools.deps.wpilibJava("wpimath") implementation wpilibTools.deps.wpilibJava("ntcore") + implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) - // Jackson implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: wpi.versions.jacksonVersion.get() implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get() implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() - implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get() - - // Junit - testImplementation wpilibTools.deps.wpilibJava("cscore") - testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2") - testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2") - testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2") + implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); } -// Set up exports properly -nativeUtils { - exportsConfigs { - // Main library is just default empty. This will export everything - Photon { - } - } -} - -model { - components { - Photon(NativeLibrarySpec) { - sources { - cpp { - source { - srcDirs "src/main/native/cpp" - include "**/*.cpp" - } - exportedHeaders { - srcDirs "src/main/native/include" - srcDirs "src/generate/native/include" - } - } - } - nativeUtils.useRequiredLibrary(it, "wpilib_shared") - nativeUtils.useRequiredLibrary(it, "apriltag_shared") - nativeUtils.useRequiredLibrary(it, "opencv_shared") - } - } - testSuites { - cppTest(GoogleTestTestSuiteSpec) { - testing $.components.Photon - - sources.cpp { - source { - srcDir "src/test/native/cpp" - include "**/*.cpp" - } - } - - nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") - nativeUtils.useRequiredLibrary(it, "apriltag_shared") - nativeUtils.useRequiredLibrary(it, "googletest_static") - nativeUtils.useRequiredLibrary(it, "opencv_shared") - } +cppHeadersZip { + from('src/generate/native/include') { + into '/' } } def photonlibFileInput = file("src/generate/photonlib.json.in") ext.photonlibFileOutput = file("$buildDir/generated/vendordeps/photonlib.json") +def vendorJson = artifacts.add('archives', file("$photonlibFileOutput")) + task generateVendorJson() { description = "Generates the vendor JSON file" group = "PhotonVision" @@ -141,29 +103,28 @@ task writeCurrentVersion { build.mustRunAfter writeCurrentVersion -tasks.withType(Javadoc) { - options.encoding = 'UTF-8' -} - -apply from: "publish.gradle" - -def testNativeConfigName = 'wpilibTestNatives' -def testNativeConfig = configurations.create(testNativeConfigName) +model { + components { + all { + it.sources.each { + it.exportedHeaders { + srcDirs "src/main/native/include" + srcDirs "src/generate/native/include" + } + } + } + } -def folder = project.layout.buildDirectory.dir('NativeTest') + // Publish the vendordep json + publishing { + publications { + vendorjson(MavenPublication) { + artifact vendorJson -def testNativeTasks = wpilibTools.createExtractionTasks { - taskPostfix = "Test" - configurationName = testNativeConfigName - rootTaskFolder.set(folder) + artifactId = "${nativeName}-json" + groupId = "org.photonvision" + version "1.0" + } + } + } } - -testNativeTasks.addToSourceSetResources(sourceSets.test) - -testNativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore") -testNativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore") -testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") -testNativeConfig.dependencies.add wpilibTools.deps.wpilib("hal") -testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil") -testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath") -testNativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) diff --git a/photon-lib/publish.gradle b/photon-lib/publish.gradle deleted file mode 100644 index bc825b81aa..0000000000 --- a/photon-lib/publish.gradle +++ /dev/null @@ -1,206 +0,0 @@ -apply plugin: 'maven-publish' - -ext.licenseFile = files("$rootDir/LICENSE") -ext.photonVersionFile = files("$projectDir/src/generate/native/include") - -def outputsFolder = file("$buildDir/outputs") -def allOutputsFolder = file("$buildDir/allOutputs") - -def versionFile = file("$allOutputsFolder/version.txt") - -task outputVersions() { - description = 'Prints the versions of wpilib to a file for use by the downstream packaging project' - group = 'Build' - outputs.files(versionFile) - - doFirst { - buildDir.mkdir() - outputsFolder.mkdir() - allOutputsFolder.mkdir() - } - - doLast { - versionFile.write pubVersion - } -} - -task libraryBuild() {} - -build.dependsOn outputVersions - -task copyAllOutputs(type: Copy) { - destinationDir allOutputsFolder -} - -build.dependsOn copyAllOutputs -copyAllOutputs.dependsOn outputVersions - -ext.addTaskToCopyAllOutputs = { task -> - copyAllOutputs.dependsOn task - copyAllOutputs.inputs.file task.archiveFile - copyAllOutputs.from task.archiveFile -} - -def artifactGroupId = 'org.photonvision' -def baseArtifactId = 'PhotonLib' -def zipBaseName = "_GROUP_org_photonvision_photonlib_ID_${baseArtifactId}-cpp_CLS" -def javaBaseName = "_GROUP_org_photonvision_photonlib_ID_${baseArtifactId}-java_CLS" - -task cppHeadersZip(type: Zip) { - destinationDirectory = outputsFolder - archiveBaseName = zipBaseName - archiveClassifier = "headers" - - from(licenseFile) { - into '/' - } - - from(photonVersionFile) { - into '/' - } - - from('src/main/native/include/') { - into '/' - } -} - -task cppSourcesZip(type: Zip) { - destinationDirectory = outputsFolder - archiveBaseName = zipBaseName - archiveClassifier = "sources" - - from(licenseFile) { - into '/' - } - - from('src/main/native/cpp') { - into '/' - } -} - -build.dependsOn cppHeadersZip -addTaskToCopyAllOutputs(cppHeadersZip) -build.dependsOn cppSourcesZip -addTaskToCopyAllOutputs(cppSourcesZip) - -task sourcesJar(type: Jar, dependsOn: classes) { - archiveClassifier = 'sources' - from sourceSets.main.allSource -} - -task javadocJar(type: Jar, dependsOn: javadoc) { - archiveClassifier = 'javadoc' - from javadoc.destinationDir -} - -task outputJar(type: Jar, dependsOn: classes) { - archiveBaseName = javaBaseName - destinationDirectory = outputsFolder - from sourceSets.main.output -} - -task outputSourcesJar(type: Jar, dependsOn: classes) { - archiveBaseName = javaBaseName - destinationDirectory = outputsFolder - archiveClassifier = 'sources' - from sourceSets.main.allSource -} - -task outputJavadocJar(type: Jar, dependsOn: javadoc) { - archiveBaseName = javaBaseName - destinationDirectory = outputsFolder - archiveClassifier = 'javadoc' - from javadoc.destinationDir -} - -def vendorJson = artifacts.add('archives', file("$photonlibFileOutput")) - -artifacts { - archives sourcesJar - archives javadocJar - archives outputJar - archives outputSourcesJar - archives outputJavadocJar -} - -addTaskToCopyAllOutputs(outputSourcesJar) -addTaskToCopyAllOutputs(outputJavadocJar) -addTaskToCopyAllOutputs(outputJar) - -build.dependsOn outputSourcesJar -build.dependsOn outputJavadocJar -build.dependsOn outputJar - -libraryBuild.dependsOn build - -def releasesRepoUrl = "$buildDir/repos/releases" - -publishing { - repositories { - maven { - url = releasesRepoUrl - } - maven { - url ('https://maven.photonvision.org/repository/' + (isDev ? 'snapshots' : 'internal')) - credentials { - username 'ghactions' - password System.getenv("ARTIFACTORY_API_KEY") - } - } - } -} - -tasks.withType(PublishToMavenRepository) { - doFirst { - println("Publishing to " + repository.url) - } -} - -task cleanReleaseRepo(type: Delete) { - delete releasesRepoUrl -} - -tasks.matching {it != cleanReleaseRepo}.all {it.dependsOn cleanReleaseRepo} - -model { - publishing { - def taskList = createComponentZipTasks($.components, ['Photon'], zipBaseName, Zip, project, includeStandardZipFormat) - - publications { - cpp(MavenPublication) { - taskList.each { - artifact it - } - artifact cppHeadersZip - artifact cppSourcesZip - - artifactId = "${baseArtifactId}-cpp" - groupId artifactGroupId - version pubVersion - } - java(MavenPublication) { - artifact jar - artifact sourcesJar - artifact javadocJar - - artifactId = "${baseArtifactId}-java" - groupId artifactGroupId - version pubVersion - } - vendorjson(MavenPublication) { - artifact vendorJson - - artifactId = "${baseArtifactId}-json" - groupId = artifactGroupId - version "1.0" - } - } - } -} - -tasks.withType(PublishToMavenRepository) { - it.mustRunAfter generateVendorJson -} - -publishToMavenLocal.dependsOn libraryBuild -publish.dependsOn libraryBuild diff --git a/photon-lib/src/generate/photonlib.json.in b/photon-lib/src/generate/photonlib.json.in index 9b8f63aaa4..9645b6e9d3 100644 --- a/photon-lib/src/generate/photonlib.json.in +++ b/photon-lib/src/generate/photonlib.json.in @@ -13,9 +13,24 @@ "cppDependencies": [ { "groupId": "org.photonvision", - "artifactId": "PhotonLib-cpp", + "artifactId": "photonlib-cpp", "version": "${photon_version}", - "libName": "Photon", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "${photon_version}", + "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -30,12 +45,12 @@ "javaDependencies": [ { "groupId": "org.photonvision", - "artifactId": "PhotonLib-java", + "artifactId": "photonlib-java", "version": "${photon_version}" }, { "groupId": "org.photonvision", - "artifactId": "PhotonTargeting-java", + "artifactId": "photontargeting-java", "version": "${photon_version}" } ] diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp similarity index 97% rename from photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp rename to photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index d0901b10bd..95eecb444c 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -22,16 +22,16 @@ * SOFTWARE. */ -#include "photonlib/PhotonCamera.h" +#include "photon/PhotonCamera.h" #include #include #include #include "PhotonVersion.h" -#include "photonlib/Packet.h" +#include "photon/dataflow/structures/Packet.h" -namespace photonlib { +namespace photon { constexpr const units::second_t VERSION_CHECK_INTERVAL = 5_s; static const std::vector PHOTON_PREFIX = {"/photonvision/"}; @@ -91,7 +91,7 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { const auto value = rawBytesEntry.Get(); if (!value.size()) return result; - photonlib::Packet packet{value}; + photon::Packet packet{value}; packet >> result; @@ -192,4 +192,4 @@ void PhotonCamera::VerifyVersion() { } } -} // namespace photonlib +} // namespace photon diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp similarity index 96% rename from photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp rename to photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index 080243749f..f2ff30f7ea 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -22,7 +22,7 @@ * SOFTWARE. */ -#include "photonlib/PhotonPoseEstimator.h" +#include "photon/PhotonPoseEstimator.h" #include #include @@ -44,11 +44,11 @@ #include #include -#include "photonlib/PhotonCamera.h" -#include "photonlib/PhotonPipelineResult.h" -#include "photonlib/PhotonTrackedTarget.h" +#include "photon/PhotonCamera.h" +#include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" -namespace photonlib { +namespace photon { namespace detail { cv::Point3d ToPoint3d(const frc::Translation3d& translation); @@ -360,14 +360,14 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) { std::optional PhotonPoseEstimator::MultiTagOnCoprocStrategy( PhotonPipelineResult result, std::optional camMat, std::optional distCoeffs) { - if (result.MultiTagResult().result.isValid) { + if (result.MultiTagResult().result.isPresent) { const auto field2camera = result.MultiTagResult().result.best; const auto fieldToRobot = frc::Pose3d() + field2camera + m_robotToCamera.Inverse(); - return photonlib::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(), - result.GetTargets(), - MULTI_TAG_PNP_ON_COPROCESSOR); + return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(), + result.GetTargets(), + MULTI_TAG_PNP_ON_COPROCESSOR); } return Update(result, std::nullopt, std::nullopt, @@ -425,9 +425,9 @@ std::optional PhotonPoseEstimator::MultiTagOnRioStrategy( const Pose3d pose = detail::ToPose3d(tvec, rvec); - return photonlib::EstimatedRobotPose( - pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(), - result.GetTargets(), MULTI_TAG_PNP_ON_RIO); + return photon::EstimatedRobotPose(pose.TransformBy(m_robotToCamera.Inverse()), + result.GetTimestamp(), result.GetTargets(), + MULTI_TAG_PNP_ON_RIO); } std::optional @@ -476,4 +476,4 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) { result.GetTimestamp(), result.GetTargets(), AVERAGE_BEST_TARGETS}; } -} // namespace photonlib +} // namespace photon diff --git a/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp b/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp deleted file mode 100644 index 91d6ffd6c3..0000000000 --- a/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "photonlib/MultiTargetPNPResult.h" - -namespace photonlib { - -Packet& operator<<(Packet& packet, const MultiTargetPnpResult& target) { - packet << target.result; - - size_t i; - for (i = 0; i < target.fiducialIdsUsed.capacity(); i++) { - if (i < target.fiducialIdsUsed.size()) { - packet << static_cast(target.fiducialIdsUsed[i]); - } else { - packet << static_cast(-1); - } - } - - return packet; -} - -Packet& operator>>(Packet& packet, MultiTargetPnpResult& target) { - packet >> target.result; - - target.fiducialIdsUsed.clear(); - for (size_t i = 0; i < target.fiducialIdsUsed.capacity(); i++) { - int16_t id = 0; - packet >> id; - - if (id > -1) { - target.fiducialIdsUsed.push_back(id); - } - } - - return packet; -} - -// Encode a transform3d -Packet& operator<<(Packet& packet, const frc::Transform3d& transform) { - packet << transform.Translation().X().value() - << transform.Translation().Y().value() - << transform.Translation().Z().value() - << transform.Rotation().GetQuaternion().W() - << transform.Rotation().GetQuaternion().X() - << transform.Rotation().GetQuaternion().Y() - << transform.Rotation().GetQuaternion().Z(); - - return packet; -} - -// Decode a transform3d -Packet& operator>>(Packet& packet, frc::Transform3d& transform) { - frc::Transform3d ret; - - // We use these for best and alt transforms below - double x = 0; - double y = 0; - double z = 0; - double w = 0; - - // decode and unitify translation - packet >> x >> y >> z; - const auto translation = frc::Translation3d( - units::meter_t(x), units::meter_t(y), units::meter_t(z)); - - // decode and add units to rotation - packet >> w >> x >> y >> z; - const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z)); - - transform = frc::Transform3d(translation, rotation); - - return packet; -} - -Packet& operator<<(Packet& packet, PNPResults const& result) { - packet << result.isValid << result.best << result.alt - << result.bestReprojectionErr << result.altReprojectionErr - << result.ambiguity; - - return packet; -} - -Packet& operator>>(Packet& packet, PNPResults& result) { - packet >> result.isValid >> result.best >> result.alt >> - result.bestReprojectionErr >> result.altReprojectionErr >> - result.ambiguity; - - return packet; -} - -} // namespace photonlib diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp deleted file mode 100644 index 462dd25a90..0000000000 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "photonlib/PhotonPipelineResult.h" - -namespace photonlib { -PhotonPipelineResult::PhotonPipelineResult( - units::second_t latency, std::span targets) - : latency(latency), - targets(targets.data(), targets.data() + targets.size()) {} - -bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const { - return latency == other.latency && targets == other.targets; -} - -bool PhotonPipelineResult::operator!=(const PhotonPipelineResult& other) const { - return !operator==(other); -} - -Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { - // Encode latency and number of targets. - packet << result.latency.value() * 1000 << result.m_pnpResults - << static_cast(result.targets.size()); - - // Encode the information of each target. - for (auto& target : result.targets) packet << target; - - // Return the packet - return packet; -} - -Packet& operator>>(Packet& packet, PhotonPipelineResult& result) { - // Decode latency, existence of targets, and number of targets. - double latencyMillis = 0; - int8_t targetCount = 0; - packet >> latencyMillis >> result.m_pnpResults >> targetCount; - result.latency = units::second_t(latencyMillis / 1000.0); - - result.targets.clear(); - - // Decode the information of each target. - for (int i = 0; i < targetCount; ++i) { - PhotonTrackedTarget target; - packet >> target; - result.targets.push_back(target); - } - return packet; -} - -} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h similarity index 95% rename from photon-lib/src/main/native/include/photonlib/PhotonCamera.h rename to photon-lib/src/main/native/include/photon/PhotonCamera.h index 2f2af08a26..2a1418f1b5 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -39,13 +39,17 @@ #include #include -#include "photonlib/PhotonPipelineResult.h" +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" namespace cv { class Mat; } // namespace cv -namespace photonlib { +namespace photon { enum LEDMode : int { kDefault = -1, kOff = 0, kOn = 1, kBlink = 2 }; @@ -207,4 +211,4 @@ class PhotonCamera { void VerifyVersion(); }; -} // namespace photonlib +} // namespace photon diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h similarity index 96% rename from photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h rename to photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index 7c93ac9ce9..cdb60f6cee 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -30,14 +30,18 @@ #include #include -#include "photonlib/PhotonCamera.h" -#include "photonlib/PhotonPipelineResult.h" +#include "photon/PhotonCamera.h" +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" namespace cv { class Mat; } // namespace cv -namespace photonlib { +namespace photon { enum PoseStrategy { LOWEST_AMBIGUITY = 0, CLOSEST_TO_CAMERA_HEIGHT, @@ -296,4 +300,4 @@ class PhotonPoseEstimator { PhotonPipelineResult result); }; -} // namespace photonlib +} // namespace photon diff --git a/photon-lib/src/main/native/include/photonlib/PhotonTargetSortMode.h b/photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h similarity index 90% rename from photon-lib/src/main/native/include/photonlib/PhotonTargetSortMode.h rename to photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h index 6bd596a231..c8ad53c39f 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonTargetSortMode.h +++ b/photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h @@ -26,9 +26,13 @@ #include -#include "photonlib/PhotonTrackedTarget.h" +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" -namespace photonlib { +namespace photon { namespace PhotonTargetSortMode { @@ -82,4 +86,4 @@ struct CenterMost { } }; } // namespace PhotonTargetSortMode -} // namespace photonlib +} // namespace photon diff --git a/photon-lib/src/main/native/include/photonlib/PhotonUtils.h b/photon-lib/src/main/native/include/photon/PhotonUtils.h similarity index 96% rename from photon-lib/src/main/native/include/photonlib/PhotonUtils.h rename to photon-lib/src/main/native/include/photon/PhotonUtils.h index 535b41cc30..14076a5cd3 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonUtils.h +++ b/photon-lib/src/main/native/include/photon/PhotonUtils.h @@ -32,7 +32,13 @@ #include #include -namespace photonlib { +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" + +namespace photon { class PhotonUtils { public: /** @@ -177,4 +183,4 @@ class PhotonUtils { return fieldToTarget.TransformBy(targetToCamera); } }; -} // namespace photonlib +} // namespace photon diff --git a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h b/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h similarity index 93% rename from photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h rename to photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h index 44ab0917a2..365efebd59 100644 --- a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h @@ -32,10 +32,15 @@ #include -#include "photonlib/PhotonCamera.h" -#include "photonlib/PhotonTargetSortMode.h" +#include "photon/PhotonCamera.h" +#include "photon/PhotonTargetSortMode.h" +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" -namespace photonlib { +namespace photon { class SimPhotonCamera : public PhotonCamera { public: SimPhotonCamera(nt::NetworkTableInstance instance, @@ -129,4 +134,4 @@ class SimPhotonCamera : public PhotonCamera { nt::NetworkTableEntry versionEntry; nt::RawPublisher rawBytesPublisher; }; -} // namespace photonlib +} // namespace photon diff --git a/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h b/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h similarity index 97% rename from photon-lib/src/main/native/include/photonlib/SimVisionSystem.h rename to photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h index 68de17c5b2..759efb3adc 100644 --- a/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h @@ -34,8 +34,13 @@ #include "SimPhotonCamera.h" #include "SimVisionTarget.h" +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" -namespace photonlib { +namespace photon { class SimVisionSystem { public: SimPhotonCamera cam; @@ -218,4 +223,4 @@ class SimVisionSystem { return (inRange && inHorizAngle && inVertAngle && targetBigEnough); } }; -} // namespace photonlib +} // namespace photon diff --git a/photon-lib/src/main/native/include/photonlib/SimVisionTarget.h b/photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h similarity index 88% rename from photon-lib/src/main/native/include/photonlib/SimVisionTarget.h rename to photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h index d5a19164bf..33a9adfee4 100644 --- a/photon-lib/src/main/native/include/photonlib/SimVisionTarget.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h @@ -27,7 +27,13 @@ #include #include -namespace photonlib { +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" + +namespace photon { class SimVisionTarget { public: SimVisionTarget() = default; @@ -56,4 +62,4 @@ class SimVisionTarget { units::square_meter_t targetArea; int targetId; }; -} // namespace photonlib +} // namespace photon diff --git a/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h b/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h deleted file mode 100644 index 96681a0307..0000000000 --- a/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#pragma once - -#include -#include - -#include "photonlib/Packet.h" - -namespace photonlib { - -class PNPResults { - public: - // This could be wrapped in an std::optional, but chose to do it this way to - // mirror Java - bool isValid; - - frc::Transform3d best; - double bestReprojectionErr; - - frc::Transform3d alt; - double altReprojectionErr; - - double ambiguity; - - friend Packet& operator<<(Packet& packet, const PNPResults& result); - friend Packet& operator>>(Packet& packet, PNPResults& result); -}; - -class MultiTargetPnpResult { - public: - PNPResults result; - wpi::SmallVector fiducialIdsUsed; - - friend Packet& operator<<(Packet& packet, const MultiTargetPnpResult& result); - friend Packet& operator>>(Packet& packet, MultiTargetPnpResult& result); -}; - -} // namespace photonlib diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index aa4150f593..aed60ff2fc 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -35,10 +35,13 @@ #include #include "gtest/gtest.h" -#include "photonlib/PhotonCamera.h" -#include "photonlib/PhotonPipelineResult.h" -#include "photonlib/PhotonPoseEstimator.h" -#include "photonlib/PhotonTrackedTarget.h" +#include "photon/PhotonCamera.h" +#include "photon/PhotonPoseEstimator.h" +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" static std::vector tags = { {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3), @@ -54,24 +57,24 @@ static std::vector> detectedCorners{ std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}; TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { - photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test"); + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ + wpi::SmallVector targets{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), frc::Rotation3d(1_rad, 2_rad, 3_rad)), frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), frc::Rotation3d(1_rad, 2_rad, 3_rad)), 0.7, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), frc::Rotation3d(1_rad, 2_rad, 3_rad)), @@ -83,8 +86,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.testResult = {2_ms, targets}; cameraOne.testResult.SetTimestamp(units::second_t(11)); - photonlib::PhotonPoseEstimator estimator( - aprilTags, photonlib::LOWEST_AMBIGUITY, std::move(cameraOne), {}); + photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, + std::move(cameraOne), {}); auto estimatedPose = estimator.Update(); frc::Pose3d pose = estimatedPose.value().estimatedPose; @@ -104,29 +107,29 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { }; auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft); - std::vector> cameras; + std::vector> cameras; - photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test"); + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); // ID 0 at 3,3,3 // ID 1 at 5,5,5 - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ + wpi::SmallVector targets{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), @@ -138,8 +141,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.testResult = {2_ms, targets}; cameraOne.testResult.SetTimestamp(17_s); - photonlib::PhotonPoseEstimator estimator( - aprilTags, photonlib::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne), + photon::PhotonPoseEstimator estimator( + aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne), {{0_m, 0_m, 4_m}, {}}); auto estimatedPose = estimator.Update(); frc::Pose3d pose = estimatedPose.value().estimatedPose; @@ -152,24 +155,24 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { } TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { - photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test"); + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ + wpi::SmallVector targets{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), @@ -181,9 +184,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.testResult = {2_ms, targets}; cameraOne.testResult.SetTimestamp(units::second_t(17)); - photonlib::PhotonPoseEstimator estimator(aprilTags, - photonlib::CLOSEST_TO_REFERENCE_POSE, - std::move(cameraOne), {}); + photon::PhotonPoseEstimator estimator( + aprilTags, photon::CLOSEST_TO_REFERENCE_POSE, std::move(cameraOne), {}); estimator.SetReferencePose( frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad))); auto estimatedPose = estimator.Update(); @@ -197,24 +199,24 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { } TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { - photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test"); + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ + wpi::SmallVector targets{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), @@ -226,30 +228,30 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.testResult = {2_ms, targets}; cameraOne.testResult.SetTimestamp(units::second_t(17)); - photonlib::PhotonPoseEstimator estimator( - aprilTags, photonlib::CLOSEST_TO_LAST_POSE, std::move(cameraOne), {}); + photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, + std::move(cameraOne), {}); estimator.SetLastPose( frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad))); auto estimatedPose = estimator.Update(); ASSERT_TRUE(estimatedPose); frc::Pose3d pose = estimatedPose.value().estimatedPose; - wpi::SmallVector targetsThree{ - photonlib::PhotonTrackedTarget{ + wpi::SmallVector targetsThree{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 1, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 0, frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), @@ -272,24 +274,24 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { } TEST(PhotonPoseEstimatorTest, AverageBestPoses) { - photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test"); + photon::PhotonCamera cameraOne = photon::PhotonCamera("test"); - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ + wpi::SmallVector targets{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), @@ -301,8 +303,8 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.testResult = {2_ms, targets}; cameraOne.testResult.SetTimestamp(units::second_t(15)); - photonlib::PhotonPoseEstimator estimator( - aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {}); + photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, + std::move(cameraOne), {}); auto estimatedPose = estimator.Update(); frc::Pose3d pose = estimatedPose.value().estimatedPose; @@ -314,24 +316,24 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { } TEST(PhotonPoseEstimatorTest, PoseCache) { - photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test2"); + photon::PhotonCamera cameraOne = photon::PhotonCamera("test2"); - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ + wpi::SmallVector targets{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, 4.0, 0, frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.7, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, 6.7, 1, frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), 0.3, corners, detectedCorners}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 9.0, -2.0, 19.0, 3.0, 0, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m), frc::Rotation3d(0_rad, 0_rad, 0_rad)), @@ -341,8 +343,8 @@ TEST(PhotonPoseEstimatorTest, PoseCache) { cameraOne.test = true; - photonlib::PhotonPoseEstimator estimator( - aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {}); + photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, + std::move(cameraOne), {}); // empty input, expect empty out estimator.GetCamera()->testResult = {2_ms, {}}; diff --git a/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp b/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp index c1be0ca91b..95a14e259e 100644 --- a/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp @@ -23,6 +23,6 @@ */ #include "gtest/gtest.h" -#include "photonlib/PhotonUtils.h" +#include "photon/PhotonUtils.h" TEST(PhotonUtilsTest, Include) {} diff --git a/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp b/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp index f26682f4ba..2c985c6e56 100644 --- a/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp +++ b/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp @@ -23,13 +23,13 @@ */ #include "gtest/gtest.h" -#include "photonlib/PhotonUtils.h" -#include "photonlib/SimVisionSystem.h" +#include "photon/PhotonUtils.h" +#include "photon/simulation/SimVisionSystem.h" class SimVisionSystemTest : public ::testing::Test { void SetUp() override { nt::NetworkTableInstance::GetDefault().StartServer(); - photonlib::PhotonCamera::SetVersionCheckEnabled(false); + photon::PhotonCamera::SetVersionCheckEnabled(false); } void TearDown() override {} @@ -44,7 +44,7 @@ class SimVisionSystemTestDistanceParamsTest std::tuple> {}; TEST_F(SimVisionSystemTest, TestEmpty) { - photonlib::SimVisionSystem sys{ + photon::SimVisionSystem sys{ "Test", 80.0_deg, frc::Transform3d{}, 99999_m, 320, 240, 0}; SUCCEED(); } @@ -53,9 +53,9 @@ TEST_F(SimVisionSystemTest, TestVisibilityCupidShuffle) { const frc::Pose3d targetPose{ {15.98_m, 0_m, 2_m}, frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}}; - photonlib::SimVisionSystem sys{ + photon::SimVisionSystem sys{ "Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0}; - sys.AddSimVisionTarget(photonlib::SimVisionTarget{targetPose, 1_m, 3_m, 3}); + sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 1_m, 3_m, 3}); // To the right, to the right frc::Pose2d robotPose{{5_m, 0_m}, frc::Rotation2d{-70_deg}}; @@ -104,9 +104,9 @@ TEST_F(SimVisionSystemTest, TestNotVisibleVertOne) { const frc::Pose3d targetPose{ {15.98_m, 0_m, 1_m}, frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}}; - photonlib::SimVisionSystem sys{ + photon::SimVisionSystem sys{ "Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0}; - sys.AddSimVisionTarget(photonlib::SimVisionTarget{targetPose, 1_m, 3_m, 3}); + sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 1_m, 3_m, 3}); frc::Pose2d robotPose{{5_m, 0_m}, frc::Rotation2d{5_deg}}; sys.ProcessFrame(robotPose); @@ -128,10 +128,10 @@ TEST_F(SimVisionSystemTest, TestNotVisibleVertOne) { // frc::Translation3d{0_m, 0_m, 1_m}, // frc::Rotation3d{0_deg, (units::constants::detail::PI_VAL / 4) * 1_rad, // 0_deg}}; -// photonlib::SimVisionSystem sys{ +// photon::SimVisionSystem sys{ // "Test", 80.0_deg, robotToCamera.Inverse(), 99999_m, 1234, 1234, 0}; // sys.AddSimVisionTarget( -// photonlib::SimVisionTarget{targetPose, 3_m, 0.5_m, 1736}); +// photon::SimVisionTarget{targetPose, 3_m, 0.5_m, 1736}); // frc::Pose2d robotPose{{14.98_m, 0_m}, frc::Rotation2d{5_deg}}; // sys.ProcessFrame(robotPose); @@ -146,10 +146,10 @@ TEST_F(SimVisionSystemTest, TestNotVisibleTargetSize) { const frc::Pose3d targetPose{ {15.98_m, 0_m, 1_m}, frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}}; - photonlib::SimVisionSystem sys{ + photon::SimVisionSystem sys{ "Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 20.0}; sys.AddSimVisionTarget( - photonlib::SimVisionTarget{targetPose, 0.1_m, 0.025_m, 24}); + photon::SimVisionTarget{targetPose, 0.1_m, 0.025_m, 24}); frc::Pose2d robotPose{{12_m, 0_m}, frc::Rotation2d{5_deg}}; sys.ProcessFrame(robotPose); @@ -164,10 +164,9 @@ TEST_F(SimVisionSystemTest, TestNotVisibleTooFarForLEDs) { const frc::Pose3d targetPose{ {15.98_m, 0_m, 1_m}, frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}}; - photonlib::SimVisionSystem sys{ - "Test", 80.0_deg, frc::Transform3d{}, 10_m, 640, 480, 1.0}; - sys.AddSimVisionTarget( - photonlib::SimVisionTarget{targetPose, 1_m, 0.25_m, 78}); + photon::SimVisionSystem sys{"Test", 80.0_deg, frc::Transform3d{}, 10_m, 640, + 480, 1.0}; + sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 1_m, 0.25_m, 78}); frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{5_deg}}; sys.ProcessFrame(robotPose); @@ -184,10 +183,9 @@ TEST_P(SimVisionSystemTestWithParamsTest, YawAngles) { frc::Rotation3d{0_deg, 0_deg, (3 * units::constants::detail::PI_VAL / 4) * 1_rad}}; frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; - photonlib::SimVisionSystem sys{ + photon::SimVisionSystem sys{ "Test", 120.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0}; - sys.AddSimVisionTarget( - photonlib::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23}); + sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23}); sys.ProcessFrame(robotPose); @@ -195,7 +193,7 @@ TEST_P(SimVisionSystemTestWithParamsTest, YawAngles) { ASSERT_TRUE(results.HasTargets()); - photonlib::PhotonTrackedTarget target = results.GetBestTarget(); + photon::PhotonTrackedTarget target = results.GetBestTarget(); ASSERT_NEAR(GetParam().to(), target.GetYaw(), 0.0001); } @@ -206,10 +204,9 @@ TEST_P(SimVisionSystemTestWithParamsTest, PitchAngles) { frc::Rotation3d{0_deg, 0_deg, (3 * units::constants::detail::PI_VAL / 4) * 1_rad}}; frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{0_deg}}; - photonlib::SimVisionSystem sys{ + photon::SimVisionSystem sys{ "Test", 120.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0}; - sys.AddSimVisionTarget( - photonlib::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23}); + sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23}); sys.MoveCamera(frc::Transform3d{frc::Translation3d{}, frc::Rotation3d{0_deg, GetParam(), 0_deg}}); @@ -219,7 +216,7 @@ TEST_P(SimVisionSystemTestWithParamsTest, PitchAngles) { ASSERT_TRUE(results.HasTargets()); - photonlib::PhotonTrackedTarget target = results.GetBestTarget(); + photon::PhotonTrackedTarget target = results.GetBestTarget(); ASSERT_NEAR(GetParam().to(), target.GetPitch(), 0.0001); } @@ -242,7 +239,7 @@ TEST_P(SimVisionSystemTestDistanceParamsTest, DistanceCalc) { frc::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m}, frc::Rotation3d{}}; frc::Transform3d robotToCamera{frc::Translation3d{0_m, 0_m, heightParam}, frc::Rotation3d{0_deg, pitchParam, 0_deg}}; - photonlib::SimVisionSystem sys{ + photon::SimVisionSystem sys{ "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho" "wsyourdaygoingihopegoodhaveagreatrestofyourlife", 160.0_deg, @@ -251,19 +248,18 @@ TEST_P(SimVisionSystemTestDistanceParamsTest, DistanceCalc) { 640, 480, 0}; - sys.AddSimVisionTarget( - photonlib::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 0}); + sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 0}); sys.ProcessFrame(robotPose); auto results = sys.cam.GetLatestResult(); ASSERT_TRUE(results.HasTargets()); - photonlib::PhotonTrackedTarget target = results.GetBestTarget(); + photon::PhotonTrackedTarget target = results.GetBestTarget(); ASSERT_NEAR(target.GetYaw(), 0.0, 0.0001); - units::meter_t dist = photonlib::PhotonUtils::CalculateDistanceToTarget( + units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget( robotToCamera.Z(), targetPose.Z(), pitchParam, units::degree_t{target.GetPitch()}); ASSERT_NEAR(dist.to(), @@ -300,51 +296,51 @@ TEST_F(SimVisionSystemTest, TestMultipleTargets) { const frc::Pose3d targetPoseR{ {15.98_m, -2_m, 0_m}, frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}}; - photonlib::SimVisionSystem sys{ + photon::SimVisionSystem sys{ "Test", 160.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 20.0}; - sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + sys.AddSimVisionTarget(photon::SimVisionTarget{ targetPoseL.TransformBy(frc::Transform3d{ frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), 0.25_m, 0.25_m, 1}); - sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + sys.AddSimVisionTarget(photon::SimVisionTarget{ targetPoseC.TransformBy(frc::Transform3d{ frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), 0.25_m, 0.25_m, 2}); - sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + sys.AddSimVisionTarget(photon::SimVisionTarget{ targetPoseR.TransformBy(frc::Transform3d{ frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), 0.25_m, 0.25_m, 3}); - sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + sys.AddSimVisionTarget(photon::SimVisionTarget{ targetPoseL.TransformBy(frc::Transform3d{ frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), 0.25_m, 0.25_m, 4}); - sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + sys.AddSimVisionTarget(photon::SimVisionTarget{ targetPoseC.TransformBy(frc::Transform3d{ frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), 0.25_m, 0.25_m, 5}); - sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + sys.AddSimVisionTarget(photon::SimVisionTarget{ targetPoseR.TransformBy(frc::Transform3d{ frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), 0.25_m, 0.25_m, 6}); - sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + sys.AddSimVisionTarget(photon::SimVisionTarget{ targetPoseL.TransformBy(frc::Transform3d{ frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), 0.25_m, 0.25_m, 7}); - sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + sys.AddSimVisionTarget(photon::SimVisionTarget{ targetPoseC.TransformBy(frc::Transform3d{ frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), 0.25_m, 0.25_m, 8}); - sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + sys.AddSimVisionTarget(photon::SimVisionTarget{ targetPoseL.TransformBy(frc::Transform3d{ frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), 0.25_m, 0.25_m, 9}); - sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + sys.AddSimVisionTarget(photon::SimVisionTarget{ targetPoseR.TransformBy(frc::Transform3d{ frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), 0.25_m, 0.25_m, 10}); - sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + sys.AddSimVisionTarget(photon::SimVisionTarget{ targetPoseL.TransformBy(frc::Transform3d{ frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}), 0.25_m, 0.25_m, 11}); diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 7d3bcddaa7..6f91d82dc6 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -1,38 +1,50 @@ plugins { - id "java" id 'edu.wpi.first.WpilibTools' version '1.3.0' } -apply from: "${rootDir}/shared/common.gradle" +ext { + nativeName = "photontargeting" +} -dependencies { - implementation wpilibTools.deps.wpilibJava("wpimath") - implementation wpilibTools.deps.wpilibJava("apriltag") - implementation wpilibTools.deps.wpilibJava("cscore") - implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) +apply from: "${rootDir}/shared/setupBuild.gradle" - implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() - // Junit - testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2") - testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2") - testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2") -} +wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get() -tasks.withType(JavaCompile) { - options.encoding = 'UTF-8' -} -tasks.withType(Javadoc) { - options.encoding = 'UTF-8' -} +def nativeConfigName = 'wpilibNatives' +def nativeConfig = configurations.create(nativeConfigName) -java { - withJavadocJar() - withSourcesJar() +def nativeTasks = wpilibTools.createExtractionTasks { + configurationName = nativeConfigName } -test { - useJUnitPlatform() -} +nativeTasks.addToSourceSetResources(sourceSets.main) -apply from: "publish.gradle" +nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal") +nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) + +dependencies { + implementation wpilibTools.deps.wpilibJava("wpiutil") + implementation wpilibTools.deps.wpilibJava("cameraserver") + implementation wpilibTools.deps.wpilibJava("cscore") + implementation wpilibTools.deps.wpilibJava("wpinet") + implementation wpilibTools.deps.wpilibJava("wpimath") + implementation wpilibTools.deps.wpilibJava("ntcore") + implementation wpilibTools.deps.wpilibJava("hal") + implementation wpilibTools.deps.wpilibJava("wpilibj") + implementation wpilibTools.deps.wpilibJava("apriltag") + implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) + + implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() + implementation group: "com.fasterxml.jackson.core", name: "jackson-core", version: wpi.versions.jacksonVersion.get() + implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get() + + implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() + implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); +} diff --git a/photon-targeting/publish.gradle b/photon-targeting/publish.gradle deleted file mode 100644 index 7978fd4d3d..0000000000 --- a/photon-targeting/publish.gradle +++ /dev/null @@ -1,32 +0,0 @@ -apply plugin: 'maven-publish' - -def artifactGroupId = 'org.photonvision' -def baseArtifactId = 'PhotonTargeting' - -publishing { - repositories { - maven { - url ('https://maven.photonvision.org/repository/' + (isDev ? 'snapshots' : 'internal')) - credentials { - username 'ghactions' - password System.getenv("ARTIFACTORY_API_KEY") - } - } - } - - publications { - mavenJava(MavenPublication) { - groupId = artifactGroupId - artifactId = "${baseArtifactId}-java" - version = pubVersion - - from components.java - } - } -} - -tasks.withType(PublishToMavenRepository) { - doFirst { - println("Publishing PhotonTargeting to " + repository.url) - } -} diff --git a/photon-targeting/settings.gradle b/photon-targeting/settings.gradle index 68f1512598..3b582363c9 100644 --- a/photon-targeting/settings.gradle +++ b/photon-targeting/settings.gradle @@ -1 +1,6 @@ -rootProject.name = 'photon-targeting' +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + } +} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp new file mode 100644 index 0000000000..3cfdbfe510 --- /dev/null +++ b/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp @@ -0,0 +1,56 @@ +/* + * 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 . + */ + +#include "photon/targeting/MultiTargetPNPResult.h" + +namespace photon { + +bool MultiTargetPNPResult::operator==(const MultiTargetPNPResult& other) const { + return other.result == result && other.fiducialIdsUsed == fiducialIdsUsed; +} + +Packet& operator<<(Packet& packet, const MultiTargetPNPResult& result) { + packet << result.result; + + size_t i; + for (i = 0; i < result.fiducialIdsUsed.capacity(); i++) { + if (i < result.fiducialIdsUsed.size()) { + packet << static_cast(result.fiducialIdsUsed[i]); + } else { + packet << static_cast(-1); + } + } + + return packet; +} + +Packet& operator>>(Packet& packet, MultiTargetPNPResult& result) { + packet >> result.result; + + result.fiducialIdsUsed.clear(); + for (size_t i = 0; i < result.fiducialIdsUsed.capacity(); i++) { + int16_t id = 0; + packet >> id; + + if (id > -1) { + result.fiducialIdsUsed.push_back(id); + } + } + + return packet; +} +} // namespace photon diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp new file mode 100644 index 0000000000..b43e43481d --- /dev/null +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp @@ -0,0 +1,77 @@ +/* + * 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 . + */ + +#include "photon/targeting/PNPResult.h" + +namespace photon { +bool PNPResult::operator==(const PNPResult& other) const { + return other.isPresent == isPresent && other.best == best && + other.bestReprojErr == bestReprojErr && other.alt == alt && + other.altReprojErr == altReprojErr && other.ambiguity == ambiguity; +} + +// Encode a transform3d +Packet& operator<<(Packet& packet, const frc::Transform3d& transform) { + packet << transform.Translation().X().value() + << transform.Translation().Y().value() + << transform.Translation().Z().value() + << transform.Rotation().GetQuaternion().W() + << transform.Rotation().GetQuaternion().X() + << transform.Rotation().GetQuaternion().Y() + << transform.Rotation().GetQuaternion().Z(); + + return packet; +} + +// Decode a transform3d +Packet& operator>>(Packet& packet, frc::Transform3d& transform) { + frc::Transform3d ret; + + // We use these for best and alt transforms below + double x = 0; + double y = 0; + double z = 0; + double w = 0; + + // decode and unitify translation + packet >> x >> y >> z; + const auto translation = frc::Translation3d( + units::meter_t(x), units::meter_t(y), units::meter_t(z)); + + // decode and add units to rotation + packet >> w >> x >> y >> z; + const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z)); + + transform = frc::Transform3d(translation, rotation); + + return packet; +} + +Packet& operator<<(Packet& packet, PNPResult const& result) { + packet << result.isPresent << result.best << result.alt + << result.bestReprojErr << result.altReprojErr << result.ambiguity; + + return packet; +} + +Packet& operator>>(Packet& packet, PNPResult& result) { + packet >> result.isPresent >> result.best >> result.alt >> + result.bestReprojErr >> result.altReprojErr >> result.ambiguity; + + return packet; +} +} // namespace photon diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp new file mode 100644 index 0000000000..c901ad65b4 --- /dev/null +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp @@ -0,0 +1,67 @@ +/* + * 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 . + */ + +#include "photon/targeting/PhotonPipelineResult.h" + +namespace photon { +PhotonPipelineResult::PhotonPipelineResult( + units::second_t latency, std::span targets) + : latency(latency), + targets(targets.data(), targets.data() + targets.size()) {} + +PhotonPipelineResult::PhotonPipelineResult( + units::second_t latency, std::span targets, + MultiTargetPNPResult multitagResult) + : latency(latency), + targets(targets.data(), targets.data() + targets.size()), + multitagResult(multitagResult) {} + +bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const { + return latency == other.latency && targets == other.targets && + multitagResult == other.multitagResult; +} + +Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { + // Encode latency and number of targets. + packet << result.latency.value() * 1000 << result.multitagResult + << static_cast(result.targets.size()); + + // Encode the information of each target. + for (auto& target : result.targets) packet << target; + + // Return the packet + return packet; +} + +Packet& operator>>(Packet& packet, PhotonPipelineResult& result) { + // Decode latency, existence of targets, and number of targets. + double latencyMillis = 0; + int8_t targetCount = 0; + packet >> latencyMillis >> result.multitagResult >> targetCount; + result.latency = units::second_t(latencyMillis / 1000.0); + + result.targets.clear(); + + // Decode the information of each target. + for (int i = 0; i < targetCount; ++i) { + PhotonTrackedTarget target; + packet >> target; + result.targets.push_back(target); + } + return packet; +} +} // namespace photon diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp similarity index 76% rename from photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp rename to photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp index 84a5846fcd..3252528efd 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp @@ -1,28 +1,21 @@ /* - * MIT License + * Copyright (C) Photon Vision. * - * Copyright (c) PhotonVision + * 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. * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: + * 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. * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . */ -#include "photonlib/PhotonTrackedTarget.h" +#include "photon/targeting/PhotonTrackedTarget.h" #include #include @@ -32,7 +25,7 @@ static constexpr const uint8_t MAX_CORNERS = 8; -namespace photonlib { +namespace photon { PhotonTrackedTarget::PhotonTrackedTarget( double yaw, double pitch, double area, double skew, int id, @@ -57,10 +50,6 @@ bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const { other.minAreaRectCorners == minAreaRectCorners; } -bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const { - return !operator==(other); -} - Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) { packet << target.yaw << target.pitch << target.area << target.skew << target.fiducialId @@ -143,5 +132,4 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) { return packet; } - -} // namespace photonlib +} // namespace photon diff --git a/photon-lib/src/main/native/include/photonlib/Packet.h b/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h similarity index 69% rename from photon-lib/src/main/native/include/photonlib/Packet.h rename to photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h index 59ae40b462..c2b16c7821 100644 --- a/photon-lib/src/main/native/include/photonlib/Packet.h +++ b/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h @@ -1,25 +1,18 @@ /* - * MIT License + * Copyright (C) Photon Vision. * - * Copyright (c) PhotonVision + * 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. * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: + * 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. * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . */ #pragma once @@ -30,7 +23,7 @@ #include -namespace photonlib { +namespace photon { /** * A packet that holds byte-packed data to be sent over NetworkTables. @@ -126,5 +119,4 @@ class Packet { size_t readPos = 0; size_t writePos = 0; }; - -} // namespace photonlib +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h new file mode 100644 index 0000000000..76a323e39a --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h @@ -0,0 +1,37 @@ +/* + * 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 . + */ + +#pragma once + +#include +#include + +#include "PNPResult.h" +#include "photon/dataflow/structures/Packet.h" + +namespace photon { +class MultiTargetPNPResult { + public: + PNPResult result; + wpi::SmallVector fiducialIdsUsed; + + bool operator==(const MultiTargetPNPResult& other) const; + + friend Packet& operator<<(Packet& packet, const MultiTargetPNPResult& result); + friend Packet& operator>>(Packet& packet, MultiTargetPNPResult& result); +}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h new file mode 100644 index 0000000000..e97f6c8d71 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h @@ -0,0 +1,45 @@ +/* + * 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 . + */ + +#pragma once + +#include + +#include "photon/dataflow/structures/Packet.h" + +namespace photon { + +class PNPResult { + public: + // This could be wrapped in an std::optional, but chose to do it this way to + // mirror Java + bool isPresent; + + frc::Transform3d best; + double bestReprojErr; + + frc::Transform3d alt; + double altReprojErr; + + double ambiguity; + + bool operator==(const PNPResult& other) const; + + friend Packet& operator<<(Packet& packet, const PNPResult& target); + friend Packet& operator>>(Packet& packet, PNPResult& target); +}; +} // namespace photon diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h similarity index 60% rename from photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h rename to photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h index 8370702b3b..16aa6d5f6e 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -1,25 +1,18 @@ /* - * MIT License + * Copyright (C) Photon Vision. * - * Copyright (c) PhotonVision + * 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. * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: + * 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. * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . */ #pragma once @@ -31,18 +24,18 @@ #include #include -#include "photonlib/MultiTargetPNPResult.h" -#include "photonlib/Packet.h" -#include "photonlib/PhotonTrackedTarget.h" +#include "MultiTargetPNPResult.h" +#include "PhotonTrackedTarget.h" +#include "photon/dataflow/structures/Packet.h" -namespace photonlib { +namespace photon { /** * Represents a pipeline result from a PhotonCamera. */ class PhotonPipelineResult { public: /** - * Constructs an empty pipeline result. + * Constructs an empty pipeline result */ PhotonPipelineResult() = default; @@ -54,10 +47,20 @@ class PhotonPipelineResult { PhotonPipelineResult(units::second_t latency, std::span targets); + /** + * Constructs a pipeline result. + * @param latency The latency in the pipeline. + * @param targets The list of targets identified by the pipeline. + * @param multitagResult The multitarget result + */ + PhotonPipelineResult(units::second_t latency, + std::span targets, + MultiTargetPNPResult multitagResult); + /** * Returns the best target in this pipeline result. If there are no targets, - * this method will return an empty target with all values set to zero. The - * best target is determined by the target sort mode in the PhotonVision UI. + * this method will return null. The best target is determined by the target + * sort mode in the PhotonVision UI. * * @return The best target of the pipeline result. */ @@ -90,10 +93,10 @@ class PhotonPipelineResult { /** * Return the latest mulit-target result, as calculated on your coprocessor. - * Be sure to check getMultiTagResult().estimatedPose.isValid before using the - * pose estimate! + * Be sure to check getMultiTagResult().estimatedPose.isPresent before using + * the pose estimate! */ - const MultiTargetPnpResult& MultiTagResult() const { return m_pnpResults; } + const MultiTargetPNPResult& MultiTagResult() const { return multitagResult; } /** * Sets the timestamp in seconds @@ -118,16 +121,14 @@ class PhotonPipelineResult { } bool operator==(const PhotonPipelineResult& other) const; - bool operator!=(const PhotonPipelineResult& other) const; friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result); friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result); - private: units::second_t latency = 0_s; units::second_t timestamp = -1_s; wpi::SmallVector targets; - MultiTargetPnpResult m_pnpResults; + MultiTargetPNPResult multitagResult; inline static bool HAS_WARNED = false; }; -} // namespace photonlib +} // namespace photon diff --git a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h similarity index 70% rename from photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h rename to photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h index 2674d680a7..7eb18d125f 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h @@ -1,25 +1,18 @@ /* - * MIT License + * Copyright (C) Photon Vision. * - * Copyright (c) PhotonVision + * 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. * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: + * 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. * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . */ #pragma once @@ -32,9 +25,9 @@ #include #include -#include "photonlib/Packet.h" +#include "photon/dataflow/structures/Packet.h" -namespace photonlib { +namespace photon { /** * Represents a tracked target within a pipeline. */ @@ -53,13 +46,14 @@ class PhotonTrackedTarget { * @param skew The skew of the target. * @param pose The camera-relative pose of the target. * @param alternatePose The alternate camera-relative pose of the target. - * @Param corners The corners of the bounding rectangle. + * @param minAreaRectCorners The corners of the bounding rectangle. + * @param detectedCorners All detected corners */ PhotonTrackedTarget( double yaw, double pitch, double area, double skew, int fiducialID, const frc::Transform3d& pose, const frc::Transform3d& alternatePose, double ambiguity, - const wpi::SmallVector, 4> corners, + const wpi::SmallVector, 4> minAreaRectCorners, const std::vector> detectedCorners); /** @@ -97,7 +91,8 @@ class PhotonTrackedTarget { * down), in no particular order, of the minimum area bounding rectangle of * this target */ - wpi::SmallVector, 4> GetMinAreaRectCorners() const { + const wpi::SmallVector, 4>& GetMinAreaRectCorners() + const { return minAreaRectCorners; } @@ -112,7 +107,7 @@ class PhotonTrackedTarget { * V + Y | | * 0 ----- 1 */ - std::vector> GetDetectedCorners() { + const std::vector>& GetDetectedCorners() const { return detectedCorners; } @@ -141,12 +136,10 @@ class PhotonTrackedTarget { } bool operator==(const PhotonTrackedTarget& other) const; - bool operator!=(const PhotonTrackedTarget& other) const; friend Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target); friend Packet& operator>>(Packet& packet, PhotonTrackedTarget& target); - private: double yaw = 0; double pitch = 0; double area = 0; @@ -158,4 +151,4 @@ class PhotonTrackedTarget { wpi::SmallVector, 4> minAreaRectCorners; std::vector> detectedCorners; }; -} // namespace photonlib +} // namespace photon diff --git a/photon-lib/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp similarity index 50% rename from photon-lib/src/test/native/cpp/PacketTest.cpp rename to photon-targeting/src/test/native/cpp/PacketTest.cpp index fe53cd85c1..1094a3783f 100644 --- a/photon-lib/src/test/native/cpp/PacketTest.cpp +++ b/photon-targeting/src/test/native/cpp/PacketTest.cpp @@ -1,37 +1,53 @@ /* - * MIT License + * Copyright (C) Photon Vision. * - * Copyright (c) PhotonVision + * 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. * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: + * 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. * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . */ -#include - #include #include "gtest/gtest.h" -#include "photonlib/PhotonPipelineResult.h" -#include "photonlib/PhotonTrackedTarget.h" +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" + +TEST(PacketTest, PNPResult) { + photon::PNPResult result; + photon::Packet p; + p << result; + + photon::PNPResult b; + p >> b; + + EXPECT_EQ(result, b); +} + +TEST(PacketTest, MultiTargetPNPResult) { + photon::MultiTargetPNPResult result; + photon::Packet p; + p << result; + + photon::MultiTargetPNPResult b; + p >> b; + + EXPECT_EQ(result, b); +} TEST(PacketTest, PhotonTrackedTarget) { - photonlib::PhotonTrackedTarget target{ + photon::PhotonTrackedTarget target{ 3.0, 4.0, 9.0, @@ -45,31 +61,27 @@ TEST(PacketTest, PhotonTrackedTarget) { {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}; - photonlib::Packet p; + photon::Packet p; p << target; - photonlib::PhotonTrackedTarget b; + photon::PhotonTrackedTarget b; p >> b; - for (auto& c : p.GetData()) { - std::cout << static_cast(c) << ","; - } - EXPECT_EQ(target, b); } TEST(PacketTest, PhotonPipelineResult) { - photonlib::PhotonPipelineResult result{1_s, {}}; - photonlib::Packet p; + photon::PhotonPipelineResult result{1_s, {}}; + photon::Packet p; p << result; - photonlib::PhotonPipelineResult b; + photon::PhotonPipelineResult b; p >> b; EXPECT_EQ(result, b); - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ + wpi::SmallVector targets{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.0, @@ -82,7 +94,7 @@ TEST(PacketTest, PhotonPipelineResult) { -1, {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}, - photonlib::PhotonTrackedTarget{ + photon::PhotonTrackedTarget{ 3.0, -4.0, 9.1, @@ -97,11 +109,11 @@ TEST(PacketTest, PhotonPipelineResult) { {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}}; - photonlib::PhotonPipelineResult result2{2_s, targets}; - photonlib::Packet p2; + photon::PhotonPipelineResult result2{2_s, targets}; + photon::Packet p2; p2 << result2; - photonlib::PhotonPipelineResult b2; + photon::PhotonPipelineResult b2; p2 >> b2; EXPECT_EQ(result2, b2); diff --git a/photon-targeting/src/test/native/cpp/main.cpp b/photon-targeting/src/test/native/cpp/main.cpp new file mode 100644 index 0000000000..7aab1315f7 --- /dev/null +++ b/photon-targeting/src/test/native/cpp/main.cpp @@ -0,0 +1,23 @@ +/* + * 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 . + */ + +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp b/photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp new file mode 100644 index 0000000000..1add81e9f6 --- /dev/null +++ b/photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp @@ -0,0 +1,25 @@ +/* + * 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 . + */ + +#include "gtest/gtest.h" +#include "photon/targeting/MultiTargetPNPResult.h" + +// TODO +TEST(MultiTargetPNPResultTest, Equality) {} + +// TODO +TEST(MultiTargetPNPResultTest, Inequality) {} diff --git a/photon-targeting/src/test/native/cpp/targeting/PNPResultTest.cpp b/photon-targeting/src/test/native/cpp/targeting/PNPResultTest.cpp new file mode 100644 index 0000000000..ac2c2adf11 --- /dev/null +++ b/photon-targeting/src/test/native/cpp/targeting/PNPResultTest.cpp @@ -0,0 +1,25 @@ +/* + * 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 . + */ + +#include "gtest/gtest.h" +#include "photon/targeting/PNPResult.h" + +// TODO +TEST(PNPResultTest, Equality) {} + +// TODO +TEST(PNPResultTest, Inequality) {} diff --git a/photon-targeting/src/test/native/cpp/targeting/PhotonPipelineResultTest.cpp b/photon-targeting/src/test/native/cpp/targeting/PhotonPipelineResultTest.cpp new file mode 100644 index 0000000000..716334dcb3 --- /dev/null +++ b/photon-targeting/src/test/native/cpp/targeting/PhotonPipelineResultTest.cpp @@ -0,0 +1,25 @@ +/* + * 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 . + */ + +#include "gtest/gtest.h" +#include "photon/targeting/PhotonPipelineResult.h" + +// TODO +TEST(PhotonPipelineResultTest, Equality) {} + +// TODO +TEST(PhotonPipelineResultTest, Inequality) {} diff --git a/photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp b/photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp new file mode 100644 index 0000000000..243c56ad6f --- /dev/null +++ b/photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp @@ -0,0 +1,25 @@ +/* + * 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 . + */ + +#include "gtest/gtest.h" +#include "photon/targeting/PhotonTrackedTarget.h" + +// TODO +TEST(PhotonTrackedTargetTest, Equality) {} + +// TODO +TEST(PhotonTrackedTargetTest, Inequality) {} diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp index fc1c824eb9..ac98dcd496 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp @@ -24,7 +24,7 @@ #include "Robot.h" -#include +#include void Robot::TeleopPeriodic() { double forwardSpeed; @@ -37,7 +37,7 @@ void Robot::TeleopPeriodic() { if (result.HasTargets()) { // First calculate range - units::meter_t range = photonlib::PhotonUtils::CalculateDistanceToTarget( + units::meter_t range = photon::PhotonUtils::CalculateDistanceToTarget( CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH, units::degree_t{result.GetBestTarget().GetPitch()}); diff --git a/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h b/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h index 26518470c9..c7483a4177 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h +++ b/photonlib-cpp-examples/aimandrange/src/main/include/Robot.h @@ -24,7 +24,7 @@ #pragma once -#include +#include #include #include @@ -60,7 +60,7 @@ class Robot : public frc::TimedRobot { frc::PIDController turnController{ANGULAR_P, 0.0, ANGULAR_D}; // Change this to match the name of your camera - photonlib::PhotonCamera camera{"photonvision"}; + photon::PhotonCamera camera{"photonvision"}; frc::XboxController xboxController{0}; diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp index 3c7a70c591..e2004f163b 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp @@ -24,7 +24,7 @@ #include "Robot.h" -#include +#include void Robot::TeleopPeriodic() { double forwardSpeed = -xboxController.GetRightY(); @@ -33,7 +33,7 @@ void Robot::TeleopPeriodic() { if (xboxController.GetAButton()) { // Vision-alignment mode // Query the latest result from PhotonVision - photonlib::PhotonPipelineResult result = camera.GetLatestResult(); + photon::PhotonPipelineResult result = camera.GetLatestResult(); if (result.HasTargets()) { // Rotation speed is the output of the PID controller diff --git a/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h b/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h index a639c34008..6954a0426f 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h +++ b/photonlib-cpp-examples/aimattarget/src/main/include/Robot.h @@ -24,7 +24,7 @@ #pragma once -#include +#include #include #include @@ -40,7 +40,7 @@ class Robot : public frc::TimedRobot { private: // Change this to match the name of your camera - photonlib::PhotonCamera camera{"photonvision"}; + photon::PhotonCamera camera{"photonvision"}; // PID constants should be tuned per robot frc::PIDController controller{.1, 0, 0}; diff --git a/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h b/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h index c96ff8078d..4cb11e2e0c 100644 --- a/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h +++ b/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h @@ -24,8 +24,8 @@ #pragma once -#include -#include +#include +#include #include @@ -34,12 +34,12 @@ class PhotonCameraWrapper { public: - photonlib::PhotonPoseEstimator m_poseEstimator{ + photon::PhotonPoseEstimator m_poseEstimator{ frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp), - photonlib::MULTI_TAG_PNP_ON_RIO, - std::move(photonlib::PhotonCamera{"WPI2023"}), frc::Transform3d{}}; + photon::MULTI_TAG_PNP_ON_RIO, std::move(photon::PhotonCamera{"WPI2023"}), + frc::Transform3d{}}; - inline std::optional Update( + inline std::optional Update( frc::Pose2d estimatedPose) { m_poseEstimator.SetReferencePose(frc::Pose3d(estimatedPose)); return m_poseEstimator.Update(); diff --git a/photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h b/photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h index 71b962cae4..40bac7a42f 100644 --- a/photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h +++ b/photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h @@ -24,7 +24,7 @@ #pragma once -#include +#include #include #include diff --git a/photonlib-cpp-examples/getinrange/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/getinrange/src/main/cpp/Robot.cpp index 40a205ff18..7ac4cd93bd 100644 --- a/photonlib-cpp-examples/getinrange/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/getinrange/src/main/cpp/Robot.cpp @@ -24,7 +24,7 @@ #include "Robot.h" -#include +#include void Robot::TeleopPeriodic() { double forwardSpeed; @@ -33,11 +33,11 @@ void Robot::TeleopPeriodic() { if (xboxController.GetAButton()) { // Vision-alignment mode // Query the latest result from PhotonVision - photonlib::PhotonPipelineResult result = camera.GetLatestResult(); + photon::PhotonPipelineResult result = camera.GetLatestResult(); if (result.HasTargets()) { // First calculate range - units::meter_t range = photonlib::PhotonUtils::CalculateDistanceToTarget( + units::meter_t range = photon::PhotonUtils::CalculateDistanceToTarget( CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH, units::degree_t{result.GetBestTarget().GetPitch()}); diff --git a/photonlib-cpp-examples/getinrange/src/main/include/Robot.h b/photonlib-cpp-examples/getinrange/src/main/include/Robot.h index 08dec61134..e8905e4e0f 100644 --- a/photonlib-cpp-examples/getinrange/src/main/include/Robot.h +++ b/photonlib-cpp-examples/getinrange/src/main/include/Robot.h @@ -24,7 +24,7 @@ #pragma once -#include +#include #include #include @@ -56,7 +56,7 @@ class Robot : public frc::TimedRobot { frc::PIDController controller{P_GAIN, 0.0, D_GAIN}; // Change this to match the name of your camera - photonlib::PhotonCamera camera{"photonvision"}; + photon::PhotonCamera camera{"photonvision"}; frc::XboxController xboxController{0}; diff --git a/shared/PhotonVersion.h.in b/shared/PhotonVersion.h.in index c7dbb1419e..647421dcda 100644 --- a/shared/PhotonVersion.h.in +++ b/shared/PhotonVersion.h.in @@ -23,7 +23,7 @@ * regenerated any time the publish task is run, or when this file is deleted. */ -namespace photonlib { +namespace photon { namespace PhotonVersion { const std::string versionString = "${version}"; const std::string buildDate = "${date}"; diff --git a/shared/common.gradle b/shared/common.gradle index 55f58760a9..dc3331ff16 100644 --- a/shared/common.gradle +++ b/shared/common.gradle @@ -37,6 +37,7 @@ dependencies { implementation wpilibTools.deps.wpilibJava("ntcore") implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") + implementation wpilibTools.deps.wpilibJava("apriltag") implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) // Jackson @@ -84,7 +85,7 @@ tasks.register('generateJavaDocs', Javadoc) { } jacoco { - toolVersion = "0.8.9" + toolVersion = "0.8.10" reportsDirectory = layout.buildDirectory.dir('customJacocoReportDir') } diff --git a/shared/config.gradle b/shared/config.gradle index ca2df48acb..43300e26cc 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -20,6 +20,8 @@ nativeUtils.wpi.configureDependencies { nativeUtils.wpi.addWarnings() nativeUtils.wpi.addWarningsAsErrors() +nativeUtils.setSinglePrintPerPlatform() + // Enable builds for all platforms. model { components { @@ -34,6 +36,18 @@ model { } } + +task copyAllOutputs(type: Copy) { + def outputsFolder = file("$project.buildDir/outputs") + destinationDir outputsFolder +} + +ext.addTaskToCopyAllOutputs = { task -> + copyAllOutputs.dependsOn task + copyAllOutputs.inputs.file task.archiveFile + copyAllOutputs.from task.archiveFile +} + // Add debug path to binaries. ext.appendDebugPathToBinaries = { binaries -> binaries.withType(StaticLibraryBinarySpec) { @@ -69,6 +83,8 @@ ext.appendDebugPathToBinaries = { binaries -> } } +def licenseFile = file("$rootDir/LICENCE") + // Create ZIP tasks for each component. ext.createComponentZipTasks = { components, names, base, type, project, func -> def stringNames = names.collect { it.toString() } diff --git a/shared/javacommon.gradle b/shared/javacommon.gradle new file mode 100644 index 0000000000..6792f61c73 --- /dev/null +++ b/shared/javacommon.gradle @@ -0,0 +1,115 @@ +apply plugin: 'maven-publish' +apply plugin: 'java-library' +apply plugin: 'jacoco' + +java { + sourceCompatibility = JavaVersion.VERSION_11 + targetCompatibility = JavaVersion.VERSION_11 +} + +def baseArtifactId = nativeName +def artifactGroupId = 'org.photonvision' +def javaBaseName = "_GROUP_org_photonvision_${baseArtifactId}_ID_${baseArtifactId}-java_CLS" + +def outputsFolder = file("$buildDir/outputs") + +javadoc { + options.encoding = 'UTF-8' +} + +task sourcesJar(type: Jar, dependsOn: classes) { + archiveClassifier = 'sources' + from sourceSets.main.allSource +} + +task javadocJar(type: Jar, dependsOn: javadoc) { + archiveClassifier = 'javadoc' + from javadoc.destinationDir +} + +task outputJar(type: Jar, dependsOn: classes) { + archiveBaseName = javaBaseName + destinationDirectory = outputsFolder + from sourceSets.main.output +} + +task outputSourcesJar(type: Jar, dependsOn: classes) { + archiveBaseName = javaBaseName + destinationDirectory = outputsFolder + archiveClassifier = 'sources' + from sourceSets.main.allSource +} + +task outputJavadocJar(type: Jar, dependsOn: javadoc) { + archiveBaseName = javaBaseName + destinationDirectory = outputsFolder + archiveClassifier = 'javadoc' + from javadoc.destinationDir +} + +artifacts { + archives sourcesJar + archives javadocJar + archives outputJar + archives outputSourcesJar + archives outputJavadocJar +} + +addTaskToCopyAllOutputs(outputSourcesJar) +addTaskToCopyAllOutputs(outputJavadocJar) +addTaskToCopyAllOutputs(outputJar) + +build.dependsOn outputSourcesJar +build.dependsOn outputJavadocJar +build.dependsOn outputJar + +publishing { + publications { + java(MavenPublication) { + artifact jar + artifact sourcesJar + artifact javadocJar + + artifactId = "${baseArtifactId}-java" + groupId artifactGroupId + version pubVersion + } + } + + repositories { + maven { + url ('https://maven.photonvision.org/repository/' + (isDev ? 'snapshots' : 'internal')) + credentials { + username 'ghactions' + password System.getenv("ARTIFACTORY_API_KEY") + } + } + } +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' + testLogging { + events "failed" + exceptionFormat "full" + } + finalizedBy jacocoTestReport +} + +dependencies { + testImplementation 'org.junit.jupiter:junit-jupiter-api:5.10.0' + testImplementation 'org.junit.jupiter:junit-jupiter-params:5.10.0' + testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.10.0' +} + +jacoco { + toolVersion = "0.8.10" +} + +jacocoTestReport { + reports { + xml.required = true + html.required = true + } +} diff --git a/shared/publish.gradle b/shared/publish.gradle new file mode 100644 index 0000000000..22389e2342 --- /dev/null +++ b/shared/publish.gradle @@ -0,0 +1,81 @@ +apply plugin: 'maven-publish' + +def outputsFolder = file("$buildDir/outputs") + +def baseArtifactId = nativeName +def artifactGroupId = 'org.photonvision' +def zipBaseName = "_GROUP_org_photonvision_${baseArtifactId}_ID_${baseArtifactId}-cpp_CLS" + +def licenseFile = file("$rootDir/LICENCE") + +task cppSourcesZip(type: Zip) { + destinationDirectory = outputsFolder + archiveBaseName = zipBaseName + archiveClassifier = "sources" + + from(licenseFile) { + into '/' + } + + from('src/main/native/cpp') { + into '/' + } +} + +task cppHeadersZip(type: Zip) { + destinationDirectory = outputsFolder + archiveBaseName = zipBaseName + archiveClassifier = "headers" + + from(licenseFile) { + into '/' + } + + ext.includeDirs = [ + project.file('src/main/native/include') + ] + + ext.includeDirs.each { + from(it) { + into '/' + } + } +} + +artifacts { + archives cppHeadersZip + archives cppSourcesZip +} + +addTaskToCopyAllOutputs(cppSourcesZip) +addTaskToCopyAllOutputs(cppHeadersZip) + +model { + publishing { + def taskList = createComponentZipTasks($.components, [nativeName], zipBaseName, Zip, project, includeStandardZipFormat) + + publications { + cpp(MavenPublication) { + taskList.each { + artifact it + } + artifact cppHeadersZip + artifact cppSourcesZip + + artifactId = "${baseArtifactId}-cpp" + groupId artifactGroupId + version pubVersion + } + } + + repositories { + maven { + url ('https://maven.photonvision.org/repository/' + (isDev ? 'snapshots' : 'internal')) + credentials { + username 'ghactions' + password System.getenv("ARTIFACTORY_API_KEY") + } + } + } + } +} diff --git a/shared/setupBuild.gradle b/shared/setupBuild.gradle new file mode 100644 index 0000000000..b90f87e154 --- /dev/null +++ b/shared/setupBuild.gradle @@ -0,0 +1,76 @@ +apply plugin: 'cpp' +apply plugin: 'google-test-test-suite' +apply plugin: 'edu.wpi.first.NativeUtils' + +apply from: "${rootDir}/shared/config.gradle" +apply from: "${rootDir}/shared/javacommon.gradle" + +// Windows specific functionality to export all symbols from a binary automatically +nativeUtils { + exportsConfigs { + "${nativeName}" {} + } +} + +model { + components { + "${nativeName}"(NativeLibrarySpec) { + sources { + cpp { + source { + srcDirs 'src/main/native/cpp' + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/main/native/include' + include "**/*.h" + } + } + } + + if(project.hasProperty('includePhotonTargeting')) { + binaries.all { + lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared' + } + } + + nativeUtils.useRequiredLibrary(it, "wpilib_shared") + nativeUtils.useRequiredLibrary(it, "apriltag_shared") + nativeUtils.useRequiredLibrary(it, "opencv_shared") + } + } + testSuites { + "${nativeName}Test"(GoogleTestTestSuiteSpec) { + for(NativeComponentSpec c : $.components) { + if (c.name == nativeName) { + testing c + break + } + } + sources { + cpp { + source { + srcDirs 'src/test/native/cpp' + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/test/native/include', 'src/main/native/cpp' + } + } + } + + if(project.hasProperty('includePhotonTargeting')) { + binaries.all { + lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared' + } + } + + nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") + nativeUtils.useRequiredLibrary(it, "googletest_static") + nativeUtils.useRequiredLibrary(it, "apriltag_shared") + nativeUtils.useRequiredLibrary(it, "opencv_shared") + } + } +} + +apply from: "${rootDir}/shared/publish.gradle" From 586adebb614225d4ffc0363170ded07f1b39238a Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 20 Nov 2023 09:11:57 -0500 Subject: [PATCH 03/13] bump version (#1014) --- .github/workflows/build.yml | 2 +- build.gradle | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 852627cf5c..7a0365769c 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -119,7 +119,7 @@ jobs: path: build/html build-photonlib-host: env: - MACOSX_DEPLOYMENT_TARGET: 11 + MACOSX_DEPLOYMENT_TARGET: 12 strategy: fail-fast: false matrix: diff --git a/build.gradle b/build.gradle index fdf2fa70cb..c0792d03f2 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "com.diffplug.spotless" version "6.22.0" - id "edu.wpi.first.NativeUtils" version "2024.2.0" apply false + id "edu.wpi.first.NativeUtils" version "2024.3.2" apply false id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2" id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" id 'edu.wpi.first.WpilibTools' version '1.3.0' From 9b183ebb85be398f2e21474e3d18c0640027ea0f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 20 Nov 2023 21:24:49 -0500 Subject: [PATCH 04/13] Add option to view streams fullscreen (#1006) closes #1005 --- .../components/app/photon-camera-stream.vue | 32 +++++++++++++------ .../src/components/cameras/CamerasView.vue | 14 ++++++-- 2 files changed, 35 insertions(+), 11 deletions(-) diff --git a/photon-client/src/components/app/photon-camera-stream.vue b/photon-client/src/components/app/photon-camera-stream.vue index 13f3e75776..5485655ff5 100644 --- a/photon-client/src/components/app/photon-camera-stream.vue +++ b/photon-client/src/components/app/photon-camera-stream.vue @@ -8,7 +8,7 @@ import PvIcon from "@/components/common/pv-icon.vue"; const props = defineProps<{ streamType: "Raw" | "Processed"; - id?: string; + id: string; }>(); const streamSrc = computed(() => { @@ -25,8 +25,6 @@ const streamDesc = computed(() => `${props.streamType} Stream View`); const streamStyle = computed(() => { if (useStateStore().colorPickingMode) { return { width: "100%", cursor: "crosshair" }; - } else if (streamSrc.value !== loadingImage) { - return { width: "100%", cursor: "pointer" }; } return { width: "100%" }; @@ -40,11 +38,6 @@ const overlayStyle = computed(() => { } }); -const handleStreamClick = () => { - if (!useStateStore().colorPickingMode && streamSrc.value !== loadingImage) { - window.open(streamSrc.value); - } -}; const handleCaptureClick = () => { if (props.streamType === "Raw") { useCameraSettingsStore().saveInputSnapshot(); @@ -52,6 +45,14 @@ const handleCaptureClick = () => { useCameraSettingsStore().saveOutputSnapshot(); } }; +const handlePopoutClick = () => { + window.open(streamSrc.value); +}; +const handleFullscreenRequest = () => { + const stream = document.getElementById(props.id); + if (!stream) return; + stream.requestFullscreen(); +}; @@ -81,6 +94,7 @@ const handleCaptureClick = () => { } .stream-overlay { + display: flex; opacity: 0; transition: 0.1s ease; position: absolute; diff --git a/photon-client/src/components/cameras/CamerasView.vue b/photon-client/src/components/cameras/CamerasView.vue index f7c110cf62..1d52bfcf05 100644 --- a/photon-client/src/components/cameras/CamerasView.vue +++ b/photon-client/src/components/cameras/CamerasView.vue @@ -78,10 +78,20 @@ const fpsTooLow = computed(() => {

- +
- +
From 5a9cf418d460fc41cbe72845103df5802b20f47f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 21 Nov 2023 22:15:08 -0500 Subject: [PATCH 05/13] Fix bug with always opening stream fullscreen onclick (#1018) * Update photon-camera-stream.vue * formatting fixes --- .../src/components/app/photon-camera-stream.vue | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/photon-client/src/components/app/photon-camera-stream.vue b/photon-client/src/components/app/photon-camera-stream.vue index 5485655ff5..a836e4b362 100644 --- a/photon-client/src/components/app/photon-camera-stream.vue +++ b/photon-client/src/components/app/photon-camera-stream.vue @@ -57,14 +57,7 @@ const handleFullscreenRequest = () => { @@ -73,7 +73,7 @@ const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled " > - {{ target.ambiguity >= 0 ? target.ambiguity?.toFixed(2) + "%" : "(In Multi-Target)" }} + {{ target.ambiguity >= 0 ? target.ambiguity?.toFixed(2) : "(In Multi-Target)" }} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index dc847ec5fc..392a429006 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -97,8 +97,9 @@ public int getFiducialId() { } /** - * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be - * ambiguous. -1 if invalid. + * Get the ratio of best:alternate pose reprojection errors, called ambiguity. This is betweeen 0 + * and 1 (0 being no ambiguity, and 1 meaning both have the same reprojection error). Numbers + * above 0.2 are likely to be ambiguous. -1 if invalid. */ public double getPoseAmbiguity() { return poseAmbiguity; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h index 7eb18d125f..feba565910 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h @@ -112,8 +112,10 @@ class PhotonTrackedTarget { } /** - * Get the ratio of pose reprojection errors, called ambiguity. Numbers above - * 0.2 are likely to be ambiguous. -1 if invalid. + * Get the ratio of best:alternate pose reprojection errors, called ambiguity. + * This is betweeen 0 and 1 (0 being no ambiguity, and 1 meaning both have the + * same reprojection error). Numbers above 0.2 are likely to be ambiguous. -1 + * if invalid. */ double GetPoseAmbiguity() const { return poseAmbiguity; } From 7b49570e9d6caa64a5cf3ad33fd6ef587446acf8 Mon Sep 17 00:00:00 2001 From: Joseph Farkas <16584585+MrRedness@users.noreply.github.com> Date: Mon, 27 Nov 2023 23:43:57 -0500 Subject: [PATCH 10/13] Radians to Degrees for multi-tag pose (#1027) * Radians to Degrees for multi-tag pose * fix optional check --------- Co-authored-by: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> --- .../src/components/dashboard/tabs/TargetsTab.vue | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/photon-client/src/components/dashboard/tabs/TargetsTab.vue b/photon-client/src/components/dashboard/tabs/TargetsTab.vue index 45975da6d6..ac6aa632c4 100644 --- a/photon-client/src/components/dashboard/tabs/TargetsTab.vue +++ b/photon-client/src/components/dashboard/tabs/TargetsTab.vue @@ -73,7 +73,7 @@ const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings useCameraSettingsStore().currentPipelineSettings.solvePNPEnabled " > - {{ target.ambiguity >= 0 ? target.ambiguity?.toFixed(2) : "(In Multi-Target)" }} + {{ target.ambiguity >= 0 ? target.ambiguity.toFixed(2) : "(In Multi-Target)" }} @@ -102,7 +102,14 @@ const currentPipelineSettings = useCameraSettingsStore().currentPipelineSettings {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.x.toFixed(2) }} m {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.y.toFixed(2) }} m - {{ useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z.toFixed(2) }}° + + {{ + ( + useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z * + (180.0 / Math.PI) + ).toFixed(2) + }}° + {{ useStateStore().currentPipelineResults?.multitagResult?.fiducialIDsUsed }} From f597d111b320958a7b44d8eedd7fef61b5d35a7b Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 29 Nov 2023 18:32:04 -0500 Subject: [PATCH 11/13] Update bug issue template with UI specific comment (#1033) * Update bug_report.md * Update bug_report.md --- .github/ISSUE_TEMPLATE/bug_report.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md index d5d59cc6f1..5cdc15f846 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -18,7 +18,7 @@ Steps to reproduce the behavior: 4. See error **Screenshots / Videos** -If applicable, add screenshots to help explain your problem. Additionally, provide journalctl logs and settings zip export. +If applicable, add screenshots to help explain your problem. Additionally, provide journalctl logs and settings zip export. If your issue is regarding the web dashboard, please provide screenshots and the output of the browser console. **Platform:** - Hardware Platform (ex. Raspberry Pi 4, Windows x64): From 469bc0eeaeee19433c70782b84e452d1fce34d21 Mon Sep 17 00:00:00 2001 From: Programmers3539 Date: Fri, 1 Dec 2023 07:58:38 -0500 Subject: [PATCH 12/13] Fix Camera Index Assignment. (#1031) Fixes a bug where multiple cameras would receive identical stream indexes and would cause at least one camera to not function at all and the other to not display a stream. One of the reasons the that usbcamerasource equals method used to incorrectly determine equality was because the quirkycamera object didn't have a basename. When a camera that had a quirk was found it would set equal to a predefined quirkycamera without changing the name first. Add unit test that will better test the equality of two usbcamerasources. This required a few changes to allow creating a fake usbcamerasource. --- .../vision/camera/QuirkyCamera.java | 16 +++++- .../vision/camera/USBCameraSource.java | 54 ++++++++++++++++--- .../vision/processes/VisionModule.java | 2 +- .../vision/processes/VisionSourceManager.java | 2 +- .../processes/VisionSourceSettables.java | 2 +- .../processes/VisionModuleManagerTest.java | 18 ++++++- 6 files changed, 81 insertions(+), 13 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java b/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java index 53d89167ec..05737ed293 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java @@ -17,6 +17,8 @@ package org.photonvision.vision.camera; +import java.util.ArrayList; +import java.util.Arrays; import java.util.HashMap; import java.util.List; import java.util.Objects; @@ -108,8 +110,20 @@ public static QuirkyCamera getQuirkyCamera(int usbVid, int usbPid, String baseNa for (var qc : quirkyCameras) { boolean hasBaseName = !qc.baseName.isEmpty(); boolean matchesBaseName = qc.baseName.equals(baseName) || !hasBaseName; + // If we have a quirkycamera we need to copy the quirks from our predefined object and create + // a quirkycamera object with the baseName. if (qc.usbVid == usbVid && qc.usbPid == usbPid && matchesBaseName) { - return qc; + List quirks = new ArrayList(); + for (var q : CameraQuirk.values()) { + if (qc.hasQuirk(q)) quirks.add(q); + } + QuirkyCamera c = + new QuirkyCamera( + usbVid, + usbPid, + baseName, + Arrays.copyOf(quirks.toArray(), quirks.size(), CameraQuirk[].class)); + return c; } } return new QuirkyCamera(usbVid, usbPid, baseName); diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java index 6717f4edd8..a2f2911431 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java @@ -28,7 +28,9 @@ import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.TestUtils; import org.photonvision.vision.frame.FrameProvider; +import org.photonvision.vision.frame.provider.FileFrameProvider; import org.photonvision.vision.frame.provider.USBFrameProvider; import org.photonvision.vision.processes.VisionSource; import org.photonvision.vision.processes.VisionSourceSettables; @@ -37,10 +39,10 @@ public class USBCameraSource extends VisionSource { private final Logger logger; private final UsbCamera camera; private final USBCameraSettables usbCameraSettables; - private final USBFrameProvider usbFrameProvider; + private FrameProvider usbFrameProvider; private final CvSink cvSink; - public final QuirkyCamera cameraQuirks; + private QuirkyCamera cameraQuirks; public USBCameraSource(CameraConfiguration config) { super(config); @@ -77,6 +79,22 @@ public USBCameraSource(CameraConfiguration config) { } } + /** + * Mostly just used for unit tests to better simulate a usb camera without a camera being present. + */ + public USBCameraSource(CameraConfiguration config, int pid, int vid, boolean unitTest) { + this(config); + + cameraQuirks = QuirkyCamera.getQuirkyCamera(pid, vid, config.baseName); + + if (unitTest) + usbFrameProvider = + new FileFrameProvider( + TestUtils.getWPIImagePath( + TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes, false), + TestUtils.WPI2019Image.FOV); + } + void disableAutoFocus() { if (cameraQuirks.hasQuirk(CameraQuirk.AdjustableFocus)) { try { @@ -88,6 +106,10 @@ void disableAutoFocus() { } } + public QuirkyCamera getCameraQuirks() { + return this.cameraQuirks; + } + @Override public FrameProvider getFrameProvider() { return usbFrameProvider; @@ -103,7 +125,7 @@ protected USBCameraSettables(CameraConfiguration configuration) { super(configuration); getAllVideoModes(); if (!cameraQuirks.hasQuirk(CameraQuirk.StickyFPS)) - setVideoMode(videoModes.get(0)); // fixes double FPS set + if (!videoModes.isEmpty()) setVideoMode(videoModes.get(0)); // fixes double FPS set } public void setAutoExposure(boolean cameraAutoExposure) { @@ -343,11 +365,27 @@ public boolean isVendorCamera() { } @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - USBCameraSource that = (USBCameraSource) o; - return cameraQuirks.equals(that.cameraQuirks); + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null) return false; + if (getClass() != obj.getClass()) return false; + USBCameraSource other = (USBCameraSource) obj; + if (camera == null) { + if (other.camera != null) return false; + } else if (!camera.equals(other.camera)) return false; + if (usbCameraSettables == null) { + if (other.usbCameraSettables != null) return false; + } else if (!usbCameraSettables.equals(other.usbCameraSettables)) return false; + if (usbFrameProvider == null) { + if (other.usbFrameProvider != null) return false; + } else if (!usbFrameProvider.equals(other.usbFrameProvider)) return false; + if (cvSink == null) { + if (other.cvSink != null) return false; + } else if (!cvSink.equals(other.cvSink)) return false; + if (cameraQuirks == null) { + if (other.cameraQuirks != null) return false; + } else if (!cameraQuirks.equals(other.cameraQuirks)) return false; + return true; } @Override diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index 976053396b..483f6a7c0c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -95,7 +95,7 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, // Find quirks for the current camera if (visionSource instanceof USBCameraSource) { - cameraQuirks = ((USBCameraSource) visionSource).cameraQuirks; + cameraQuirks = ((USBCameraSource) visionSource).getCameraQuirks(); } else if (visionSource instanceof LibcameraGpuSource) { cameraQuirks = QuirkyCamera.ZeroCopyPiCamera; } else { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java index 4b31ba0691..c16e91e025 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java @@ -342,7 +342,7 @@ private static List loadVisionSourcesFromCamConfigs( cameraSources.add(piCamSrc); } else { var newCam = new USBCameraSource(configuration); - if (!newCam.cameraQuirks.hasQuirk(CameraQuirk.CompletelyBroken) + if (!newCam.getCameraQuirks().hasQuirk(CameraQuirk.CompletelyBroken) && !newCam.getSettables().videoModes.isEmpty()) { cameraSources.add(newCam); } diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java index cda7d08e6f..f2057a18db 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java @@ -59,7 +59,7 @@ public void setBlueGain(int blue) {} public abstract VideoMode getCurrentVideoMode(); public void setVideoModeInternal(int index) { - setVideoMode(getAllVideoModes().get(index)); + if (!getAllVideoModes().isEmpty()) setVideoMode(getAllVideoModes().get(index)); } public void setVideoMode(VideoMode mode) { diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java index e0a8376159..9b4ca528b6 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java @@ -31,6 +31,7 @@ import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.CVPipelineResultConsumer; import org.photonvision.common.util.TestUtils; +import org.photonvision.vision.camera.USBCameraSource; import org.photonvision.vision.frame.FrameProvider; import org.photonvision.vision.frame.FrameStaticProperties; import org.photonvision.vision.frame.provider.FileFrameProvider; @@ -165,7 +166,16 @@ public void testMultipleStreamIndex() { TestUtils.WPI2019Image.FOV); var testSource3 = new TestSource(ffp3, conf3); - var modules = vmm.addSources(List.of(testSource, testSource2, testSource3)); + // Arducam OV9281 UC844 raspberry pi test. + var conf4 = new CameraConfiguration("Left", "dev/video1"); + USBCameraSource usbSimulation = new USBCameraSource(conf4, 0x6366, 0x0c45, true); + + var conf5 = new CameraConfiguration("Right", "dev/video2"); + USBCameraSource usbSimulation2 = new USBCameraSource(conf5, 0x6366, 0x0c45, true); + + var modules = + vmm.addSources( + List.of(testSource, testSource2, testSource3, usbSimulation, usbSimulation2)); System.out.println( Arrays.toString( @@ -176,9 +186,15 @@ public void testMultipleStreamIndex() { modules.stream() .map(it -> it.visionSource.getCameraConfiguration().streamIndex) .collect(Collectors.toList()); + + assertTrue(usbSimulation.equals(usbSimulation)); + assertTrue(!usbSimulation.equals(usbSimulation2)); + assertTrue(idxs.contains(0)); assertTrue(idxs.contains(1)); assertTrue(idxs.contains(2)); + assertTrue(idxs.contains(3)); + assertTrue(idxs.contains(4)); } private static void printTestResults(CVPipelineResult pipelineResult) { From 6db5bc5e0c439a2e794fb1b36e0840fc67642fc6 Mon Sep 17 00:00:00 2001 From: Joseph Farkas <16584585+MrRedness@users.noreply.github.com> Date: Mon, 4 Dec 2023 16:55:16 +0000 Subject: [PATCH 13/13] Matching cameras by path ID (#1015) Allows multiple cameras of the same model to be used while ensuring they stay tied to the physical camera and not the port. Matching occurs when first connecting cameras so swapping the ports while PV is running will swap the virtual cameras until a restart. Currently only tested on Linux. --- .../configuration/SqlConfigProvider.java | 15 +- .../vision/processes/VisionSourceManager.java | 152 ++++++++++++++---- .../processes/VisionSourceManagerTest.java | 81 +++++++++- 3 files changed, 207 insertions(+), 41 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java index 8a0c755ad8..3f22e69522 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java @@ -51,6 +51,7 @@ static class TableKeys { static final String CAM_UNIQUE_NAME = "unique_name"; static final String CONFIG_JSON = "config_json"; static final String DRIVERMODE_JSON = "drivermode_json"; + static final String OTHERPATHS_JSON = "otherpaths_json"; static final String PIPELINE_JSONS = "pipeline_jsons"; static final String NETWORK_CONFIG = "networkConfig"; @@ -147,6 +148,7 @@ private void initDatabase() { + " unique_name TINYTEXT PRIMARY KEY,\n" + " config_json text NOT NULL,\n" + " drivermode_json text NOT NULL,\n" + + " otherpaths_json text NOT NULL,\n" + " pipeline_jsons mediumtext NOT NULL\n" + ");"; createCameraTableStatement.execute(sql); @@ -295,8 +297,8 @@ private void saveCameras(Connection conn) { try { // Replace this camera's row with the new settings var sqlString = - "REPLACE INTO cameras (unique_name, config_json, drivermode_json, pipeline_jsons) VALUES " - + "(?,?,?,?);"; + "REPLACE INTO cameras (unique_name, config_json, drivermode_json, otherpaths_json, pipeline_jsons) VALUES " + + "(?,?,?,?,?);"; for (var c : config.getCameraConfigurations().entrySet()) { PreparedStatement statement = conn.prepareStatement(sqlString); @@ -305,6 +307,7 @@ private void saveCameras(Connection conn) { statement.setString(1, c.getKey()); statement.setString(2, JacksonUtils.serializeToString(config)); statement.setString(3, JacksonUtils.serializeToString(config.driveModeSettings)); + statement.setString(4, JacksonUtils.serializeToString(config.otherPaths)); // Serializing a list of abstract classes sucks. Instead, make it into an array // of strings, which we can later unpack back into individual settings @@ -321,7 +324,7 @@ private void saveCameras(Connection conn) { }) .filter(Objects::nonNull) .collect(Collectors.toList()); - statement.setString(4, JacksonUtils.serializeToString(settings)); + statement.setString(5, JacksonUtils.serializeToString(settings)); statement.executeUpdate(); } @@ -455,10 +458,11 @@ private HashMap loadCameraConfigs(Connection conn) query = conn.prepareStatement( String.format( - "SELECT %s, %s, %s, %s FROM cameras", + "SELECT %s, %s, %s, %s, %s FROM cameras", TableKeys.CAM_UNIQUE_NAME, TableKeys.CONFIG_JSON, TableKeys.DRIVERMODE_JSON, + TableKeys.OTHERPATHS_JSON, TableKeys.PIPELINE_JSONS)); var result = query.executeQuery(); @@ -474,6 +478,8 @@ private HashMap loadCameraConfigs(Connection conn) var driverMode = JacksonUtils.deserialize( result.getString(TableKeys.DRIVERMODE_JSON), DriverModePipelineSettings.class); + var otherPaths = + JacksonUtils.deserialize(result.getString(TableKeys.OTHERPATHS_JSON), String[].class); List pipelineSettings = JacksonUtils.deserialize( result.getString(TableKeys.PIPELINE_JSONS), dummyList.getClass()); @@ -487,6 +493,7 @@ private HashMap loadCameraConfigs(Connection conn) config.pipelineSettings = loadedSettings; config.driveModeSettings = driverMode; + config.otherPaths = otherPaths; loadedConfigurations.put(uniqueName, config); } } catch (SQLException | IOException e) { diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java index c16e91e025..2e6ada0aa5 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceManager.java @@ -196,52 +196,112 @@ protected List tryMatchUSBCamImpl(boolean createSources) { * @param loadedUsbCamConfigs The USB {@link CameraConfiguration}s loaded from disk. * @return the matched configurations. */ - private List matchUSBCameras( + protected List matchUSBCameras( List detectedCamInfos, List loadedUsbCamConfigs) { var detectedCameraList = new ArrayList<>(detectedCamInfos); ArrayList cameraConfigurations = new ArrayList<>(); - // loop over all the configs loaded from disk + List unmatchedAfterByID = new ArrayList<>(loadedUsbCamConfigs); + + // loop over all the configs loaded from disk, attempting to match each camera + // to a config by path-by-id on linux for (CameraConfiguration config : loadedUsbCamConfigs) { UsbCameraInfo cameraInfo; - // attempt matching by path and basename - logger.debug( - "Trying to find a match for loaded camera " - + config.baseName - + " with path " - + config.path); - cameraInfo = - detectedCameraList.stream() - .filter( - usbCameraInfo -> - usbCameraInfo.path.equals(config.path) - && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) - .findFirst() - .orElse(null); - - // if path based fails, attempt basename only match - if (cameraInfo == null) { - logger.debug("Failed to match by path and name, falling back to name-only match"); + if (config.otherPaths.length == 0) { + logger.debug("No valid path-by-id found for config with name " + config.baseName); + } else { + // attempt matching by path and basename + logger.debug( + "Trying to find a match for loaded camera " + + config.baseName + + " with path-by-id " + + config.otherPaths[0]); + cameraInfo = + detectedCameraList.stream() + .filter( + usbCameraInfo -> + usbCameraInfo.otherPaths.length != 0 + && usbCameraInfo.otherPaths[0].equals(config.otherPaths[0]) + && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) + .findFirst() + .orElse(null); + + // If we actually matched a camera to a config, remove that camera from the list + // and add it to the output + if (cameraInfo != null) { + logger.debug("Matched the config for " + config.baseName + " to a physical camera!"); + detectedCameraList.remove(cameraInfo); + unmatchedAfterByID.remove(config); + cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); + } + } + } + + if (!unmatchedAfterByID.isEmpty() && !detectedCameraList.isEmpty()) { + logger.debug("Match by path-by-id failed, falling back to path-only matching"); + + List unmatchedAfterByPath = new ArrayList<>(loadedUsbCamConfigs); + + // now attempt to match the cameras and configs remaining by normal path + for (CameraConfiguration config : unmatchedAfterByID) { + UsbCameraInfo cameraInfo; + + // attempt matching by path and basename + logger.debug( + "Trying to find a match for loaded camera " + + config.baseName + + " with path " + + config.path); cameraInfo = detectedCameraList.stream() .filter( usbCameraInfo -> - cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) + usbCameraInfo.path.equals(config.path) + && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) .findFirst() .orElse(null); + + // If we actually matched a camera to a config, remove that camera from the list + // and add it to the output + if (cameraInfo != null) { + logger.debug("Matched the config for " + config.baseName + " to a physical camera!"); + detectedCameraList.remove(cameraInfo); + unmatchedAfterByPath.remove(config); + cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); + } } - // If we actually matched a camera to a config, remove that camera from the list and add it to - // the output - if (cameraInfo != null) { - logger.debug("Matched the config for " + config.baseName + " to a physical camera!"); - detectedCameraList.remove(cameraInfo); - cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); + if (!unmatchedAfterByPath.isEmpty() && !detectedCameraList.isEmpty()) { + logger.debug("Match by ID and path failed, falling back to name-only matching"); + + // if both path and ID based matching fails, attempt basename only match + for (CameraConfiguration config : unmatchedAfterByPath) { + UsbCameraInfo cameraInfo; + + logger.debug("Trying to find a match for loaded camera with name " + config.baseName); + + cameraInfo = + detectedCameraList.stream() + .filter( + usbCameraInfo -> + cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) + .findFirst() + .orElse(null); + + // If we actually matched a camera to a config, remove that camera from the list + // and add it to the output + if (cameraInfo != null) { + logger.debug("Matched the config for " + config.baseName + " to a physical camera!"); + detectedCameraList.remove(cameraInfo); + cameraConfigurations.add(mergeInfoIntoConfig(config, cameraInfo)); + } + } } } - // If we have any unmatched cameras left, create a new CameraConfiguration for them here. + // If we have any unmatched cameras left, create a new CameraConfiguration for + // them here. logger.debug( "After matching loaded configs " + detectedCameraList.size() + " cameras were unmatched."); for (UsbCameraInfo info : detectedCameraList) { @@ -250,7 +310,7 @@ && cameraNameToBaseName(usbCameraInfo.name).equals(config.baseName)) String uniqueName = baseNameToUniqueName(baseName); int suffix = 0; - while (containsName(cameraConfigurations, uniqueName)) { + while (containsName(cameraConfigurations, uniqueName) || containsName(uniqueName)) { suffix++; uniqueName = String.format("%s (%d)", uniqueName, suffix); } @@ -283,6 +343,27 @@ private CameraConfiguration mergeInfoIntoConfig(CameraConfiguration cfg, UsbCame cfg.path = info.path; } + if (cfg.otherPaths.length != info.otherPaths.length) { + logger.debug( + "Updating otherPath config from " + + Arrays.toString(cfg.otherPaths) + + " to " + + Arrays.toString(info.otherPaths)); + cfg.otherPaths = info.otherPaths.clone(); + } else { + for (int i = 0; i < info.otherPaths.length; i++) { + if (!cfg.otherPaths[i].equals(info.otherPaths[i])) { + logger.debug( + "Updating otherPath config from " + + Arrays.toString(cfg.otherPaths) + + " to " + + Arrays.toString(info.otherPaths)); + cfg.otherPaths = info.otherPaths.clone(); + break; + } + } + } + return cfg; } @@ -309,7 +390,7 @@ private List filterAllowedDevices(List allDevices) private boolean usbCamEquals(UsbCameraInfo a, UsbCameraInfo b) { return a.path.equals(b.path) - && a.dev == b.dev + // && a.dev == b.dev (dev is not constant in Windows) && a.name.equals(b.name) && a.productId == b.productId && a.vendorId == b.vendorId; @@ -363,4 +444,15 @@ private boolean containsName( return configList.stream() .anyMatch(configuration -> configuration.uniqueName.equals(uniqueName)); } + + /** + * Check if the current list of known cameras contains the given unique name. + * + * @param uniqueName The unique name. + * @return If the list of cameras contains the unique name. + */ + private boolean containsName(final String uniqueName) { + return VisionModuleManager.getInstance().getModules().stream() + .anyMatch(camera -> camera.visionSource.cameraConfiguration.uniqueName.equals(uniqueName)); + } } diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java index 2c64a9a622..e16213000e 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/VisionSourceManagerTest.java @@ -35,23 +35,90 @@ public void visionSourceTest() { ConfigManager.getInstance().load(); inst.tryMatchUSBCamImpl(); - var config = new CameraConfiguration("secondTestVideo", "dev/video1"); + var config3 = + new CameraConfiguration( + "secondTestVideo", + "secondTestVideo1", + "secondTestVideo1", + "dev/video1", + new String[] {"by-id/123"}); + var config4 = + new CameraConfiguration( + "secondTestVideo", + "secondTestVideo2", + "secondTestVideo2", + "dev/video2", + new String[] {"by-id/321"}); + UsbCameraInfo info1 = new UsbCameraInfo(0, "dev/video0", "testVideo", new String[0], 1, 2); + infoList.add(info1); - inst.registerLoadedConfigs(config); - var sources = inst.tryMatchUSBCamImpl(false); + inst.registerLoadedConfigs(config3, config4); + + inst.tryMatchUSBCamImpl(false); assertTrue(inst.knownUsbCameras.contains(info1)); - assertEquals(1, inst.unmatchedLoadedConfigs.size()); + assertEquals(2, inst.unmatchedLoadedConfigs.size()); + + UsbCameraInfo info2 = new UsbCameraInfo(0, "dev/video1", "testVideo", new String[0], 1, 2); - UsbCameraInfo info2 = - new UsbCameraInfo(0, "dev/video1", "secondTestVideo", new String[0], 2, 1); infoList.add(info2); + + var cams = inst.matchUSBCameras(infoList, inst.unmatchedLoadedConfigs); + + // assertEquals("testVideo (1)", cams.get(0).uniqueName); // Proper suffixing + + inst.tryMatchUSBCamImpl(false); + + assertTrue(inst.knownUsbCameras.contains(info2)); + assertEquals(2, inst.unmatchedLoadedConfigs.size()); + + UsbCameraInfo info3 = + new UsbCameraInfo(0, "dev/video2", "secondTestVideo", new String[] {"by-id/123"}, 2, 1); + + UsbCameraInfo info4 = + new UsbCameraInfo(0, "dev/video3", "secondTestVideo", new String[] {"by-id/321"}, 3, 1); + + infoList.add(info4); + + cams = inst.matchUSBCameras(infoList, inst.unmatchedLoadedConfigs); + + var cam4 = + cams.stream() + .filter( + cam -> cam.otherPaths.length > 0 && cam.otherPaths[0].equals(config4.otherPaths[0])) + .findFirst() + .orElse(null); + // If this is null, cam4 got matched to config3 instead of config4 + + assertEquals(cam4.nickname, config4.nickname); + + infoList.add(info3); + + cams = inst.matchUSBCameras(infoList, inst.unmatchedLoadedConfigs); + inst.tryMatchUSBCamImpl(false); assertTrue(inst.knownUsbCameras.contains(info2)); - assertEquals(2, inst.knownUsbCameras.size()); + assertTrue(inst.knownUsbCameras.contains(info3)); + + var cam3 = + cams.stream() + .filter( + cam -> cam.otherPaths.length > 0 && cam.otherPaths[0].equals(config3.otherPaths[0])) + .findFirst() + .orElse(null); + cam4 = + cams.stream() + .filter( + cam -> cam.otherPaths.length > 0 && cam.otherPaths[0].equals(config4.otherPaths[0])) + .findFirst() + .orElse(null); + + assertEquals(cam3.nickname, config3.nickname); + assertEquals(cam4.nickname, config4.nickname); + assertEquals(4, inst.knownUsbCameras.size()); assertEquals(0, inst.unmatchedLoadedConfigs.size()); } }