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

[photon-lib] Simulate multitag result (#973)

Browse Source
This commit is contained in:
amquake
2023-10-25 18:35:06 -07:00
committed by GitHub
parent 5b2be119e7
commit d61225eba3
3 changed files with 71 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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 class PhotonCamera implements AutoCloseable {
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 class PhotonCamera implements AutoCloseable {
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<Matrix<N3, N3>> getCameraMatrix() {
var cameraMatrix = cameraIntrinsicsSubscriber.get();

27
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

@@ -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 edu.wpi.first.util.WPIUtilJNI;
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.CameraTargetRelation;
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 class PhotonCameraSim implements AutoCloseable {
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<Integer> 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);
}
/**