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

Sim target sorting and "easy" NT entries (#262)

Browse Source 
* Typo fixes, add target sorting

* Add easy NT entries

* spotttttttttttttttttlessssssssssssss

* Run wpiformat

* Fix return on no targets

* formatting hell 2 electric boogaloo

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Banks T
2021-03-23 12:47:26 -04:00
committed by GitHub
parent f9fd7a0b45
commit 1e715ce4bb
5 changed files with 140 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java
View File

@@ -18,13 +18,24 @@
package org.photonvision;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.Arrays;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
@SuppressWarnings("unused")
public class SimPhotonCamera extends PhotonCamera {
private final NetworkTableEntry latencyMillisEntry;
private final NetworkTableEntry hasTargetEntry;
private final NetworkTableEntry targetPitchEntry;
private final NetworkTableEntry targetYawEntry;
private final NetworkTableEntry targetAreaEntry;
private final NetworkTableEntry targetSkewEntry;
private final NetworkTableEntry targetPoseEntry;
/**
* Constructs a Simulated PhotonCamera from a root table.
*
@@ -32,6 +43,14 @@ public class SimPhotonCamera extends PhotonCamera {
*/
public SimPhotonCamera(NetworkTable rootTable) {
super(rootTable);
latencyMillisEntry = rootTable.getEntry("latencyMillis");
hasTargetEntry = rootTable.getEntry("hasTargetEntry");
targetPitchEntry = rootTable.getEntry("targetPitchEntry");
targetYawEntry = rootTable.getEntry("targetYawEntry");
targetAreaEntry = rootTable.getEntry("targetAreaEntry");
targetSkewEntry = rootTable.getEntry("targetSkewEntry");
targetPoseEntry = rootTable.getEntry("targetPoseEntry");
}
/**
@@ -40,13 +59,13 @@ public class SimPhotonCamera extends PhotonCamera {
* @param cameraName The nickname of the camera (found in the PhotonVision UI).
*/
public SimPhotonCamera(String cameraName) {
super(cameraName);
this(NetworkTableInstance.getDefault().getTable("photonvision").getSubTable(cameraName));
}
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latencyMillis
* @param latencyMillis Latency of the provided frame
* @param targets Each target detected
*/
public void submitProcessedFrame(double latencyMillis, PhotonTrackedTarget... targets) {
@@ -56,15 +75,66 @@ public class SimPhotonCamera extends PhotonCamera {
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latencyMillis
* @param tgtList List of targets detected
* @param latencyMillis Latency of the provided frame
* @param sortMode Order in which to sort targets
* @param targets Each target detected
*/
public void submitProcessedFrame(double latencyMillis, List<PhotonTrackedTarget> tgtList) {
if (!getDriverMode()) {
PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, tgtList);
var newPacket = new Packet(newResult.getPacketSize());
newResult.populatePacket(newPacket);
rawBytesEntry.setRaw(newPacket.getData());
public void submitProcessedFrame(
double latencyMillis, PhotonTargetSortMode sortMode, PhotonTrackedTarget... targets) {
submitProcessedFrame(latencyMillis, sortMode, Arrays.asList(targets));
}
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latencyMillis Latency of the provided frame
* @param targetList List of targets detected
*/
public void submitProcessedFrame(double latencyMillis, List<PhotonTrackedTarget> targetList) {
submitProcessedFrame(latencyMillis, null, targetList);
}
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latencyMillis Latency of the provided frame
* @param sortMode Order in which to sort targets
* @param targetList List of targets detected
*/
public void submitProcessedFrame(
double latencyMillis, PhotonTargetSortMode sortMode, List<PhotonTrackedTarget> targetList) {
latencyMillisEntry.setDouble(latencyMillis);
if (sortMode != null) {
targetList.sort(sortMode.getComparator());
}
PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, targetList);
var newPacket = new Packet(newResult.getPacketSize());
newResult.populatePacket(newPacket);
rawBytesEntry.setRaw(newPacket.getData());
boolean hasTargets = newResult.hasTargets();
hasTargetEntry.setBoolean(hasTargets);
if (!hasTargets) {
targetPitchEntry.setDouble(0.0);
targetYawEntry.setDouble(0.0);
targetAreaEntry.setDouble(0.0);
targetPoseEntry.setDoubleArray(new double[] {0.0, 0.0, 0.0});
targetSkewEntry.setDouble(0.0);
} else {
var bestTarget = newResult.getBestTarget();
targetPitchEntry.setDouble(bestTarget.getPitch());
targetYawEntry.setDouble(bestTarget.getYaw());
targetAreaEntry.setDouble(bestTarget.getArea());
targetSkewEntry.setDouble(bestTarget.getSkew());
var transform = bestTarget.getCameraToTarget();
double[] poseData = {
transform.getX(), transform.getY(), transform.getRotation().getDegrees()
};
targetPoseEntry.setDoubleArray(poseData);
}
}
}