From d61225eba3c83d5a3dc7d2a58ea23160b7822a49 Mon Sep 17 00:00:00 2001 From: amquake Date: Wed, 25 Oct 2023 18:35:06 -0700 Subject: [PATCH] [photon-lib] Simulate multitag result (#973) --- .gitignore | 3 + .../java/org/photonvision/PhotonCamera.java | 67 ++++++++++++------- .../simulation/PhotonCameraSim.java | 27 +++++++- 3 files changed, 71 insertions(+), 26 deletions(-) diff --git a/.gitignore b/.gitignore index 609770cdf5..766874b000 100644 --- a/.gitignore +++ b/.gitignore @@ -157,5 +157,8 @@ photonlib-java-examples/*/vendordeps/* photonlib-cpp-examples/*/vendordeps/* */networktables.json +*/networktables.json.bck +photonlib-cpp-examples/*/networktables.json.bck +photonlib-java-examples/*/networktables.json.bck *.sqlite photon-server/src/main/resources/web/index.html diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 9500629ca3..cd64965990 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -24,11 +24,9 @@ package org.photonvision; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.numbers.*; @@ -121,6 +119,9 @@ 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; @@ -331,27 +332,45 @@ public boolean isConnected() { return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC; } - /** - * Set the Apriltag Field Layout used by all PhotonVision coprocessors that are (or might later) - * connect to this robot. The topic is marked as persistant, 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); - return true; - } catch (JsonProcessingException e) { - MathSharedStore.reportError("Error setting ATFL in " + this.getName(), e.getStackTrace()); - } - return false; - } + // 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(); 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 1ad02b5b4d..b2c7317c32 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -24,6 +24,8 @@ package org.photonvision.simulation; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.CvSource; import edu.wpi.first.cscore.VideoMode.PixelFormat; @@ -36,6 +38,7 @@ import java.util.ArrayList; import java.util.List; import java.util.Optional; +import java.util.stream.Collectors; import org.opencv.core.Core; import org.opencv.core.CvType; import org.opencv.core.Mat; @@ -52,6 +55,8 @@ import org.photonvision.estimation.OpenCVHelp; 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.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -77,6 +82,8 @@ public class PhotonCameraSim implements AutoCloseable { private double minTargetAreaPercent; private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest; + private AprilTagFieldLayout tagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + // video stream simulation private final CvSource videoSimRaw; private final Mat videoSimFrameRaw = new Mat(); @@ -514,11 +521,27 @@ public PhotonPipelineResult process( videoSimProcessed.putFrame(videoSimFrameProcessed); } else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose); - // put this simulated data to NT + // calculate multitag results + var multitagResults = new MultiTargetPNPResults(); + // 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 = + VisionEstimation.estimateCamPosePNP( + prop.getIntrinsics(), prop.getDistCoeffs(), detectableTgts, tagLayout); + multitagResults = new MultiTargetPNPResults(pnpResults, usedIDs); + } + + // sort target order if (sortMode != null) { detectableTgts.sort(sortMode.getComparator()); } - return new PhotonPipelineResult(latencyMillis, detectableTgts); + + // put this simulated data to NT + return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResults); } /**