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

Update Java Simulation Examples (#913)

Browse Source 
Removes apriltagExample and simposeest, replacing them with swervedriveposeestsim

---------

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
amquake
2023-10-05 05:57:38 -07:00
committed by GitHub
parent 65d5494ab3
commit ce0d28da93
65 changed files with 1859 additions and 2011 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
View File

@@ -66,6 +66,8 @@ public class VisionSystemSim {
private final Field2d dbgField;
private final Transform3d kEmptyTrf = new Transform3d();
/**
* A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot
* running PhotonVision, detecting targets placed on the field. {@link VisionTargetSim}s added to
@@ -337,7 +339,7 @@ public class VisionSystemSim {
* Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically
* determine if a new frame should be submitted.
*
* @param robotPoseMeters The current robot pose in meters
* @param robotPoseMeters The simulated robot pose in meters
*/
public void update(Pose2d robotPoseMeters) {
update(new Pose3d(robotPoseMeters));
@@ -347,7 +349,7 @@ public class VisionSystemSim {
* Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically
* determine if a new frame should be submitted.
*
* @param robotPoseMeters The current robot pose in meters
* @param robotPoseMeters The simulated robot pose in meters
*/
public void update(Pose3d robotPoseMeters) {
var targetTypes = targetSets.entrySet();
@@ -370,13 +372,15 @@ public class VisionSystemSim {
var allTargets = new ArrayList<VisionTargetSim>();
targetTypes.forEach((entry) -> allTargets.addAll(entry.getValue()));
var visibleTargets = new ArrayList<Pose3d>();
var cameraPose2ds = new ArrayList<Pose2d>();
var visTgtPoses2d = new ArrayList<Pose2d>();
var cameraPoses2d = new ArrayList<Pose2d>();
boolean processed = false;
// process each camera
for (var camSim : camSimMap.values()) {
// check if this camera is ready to process and get latency
var optTimestamp = camSim.consumeNextEntryTime();
if (optTimestamp.isEmpty()) continue;
else processed = true;
// when this result "was" read by NT
long timestampNT = optTimestamp.get();
// this result's processing latency in milliseconds
@@ -387,7 +391,7 @@ public class VisionSystemSim {
// use camera pose from the image capture timestamp
Pose3d lateRobotPose = getRobotPose(timestampCapture);
Pose3d lateCameraPose = lateRobotPose.plus(getRobotToCamera(camSim, timestampCapture).get());
cameraPose2ds.add(lateCameraPose.toPose2d());
cameraPoses2d.add(lateCameraPose.toPose2d());
// process a PhotonPipelineResult with visible targets
var camResult = camSim.process(latencyMillis, lateCameraPose, allTargets);
@@ -395,14 +399,12 @@ public class VisionSystemSim {
camSim.submitProcessedFrame(camResult, timestampNT);
// display debug results
for (var target : camResult.getTargets()) {
visibleTargets.add(lateCameraPose.transformBy(target.getBestCameraToTarget()));
var trf = target.getBestCameraToTarget();
if (trf.equals(kEmptyTrf)) continue;
visTgtPoses2d.add(lateCameraPose.transformBy(trf).toPose2d());
}
}
if (visibleTargets.size() != 0) {
dbgField
.getObject("visibleTargetPoses")
.setPoses(visibleTargets.stream().map(Pose3d::toPose2d).collect(Collectors.toList()));
}
if (cameraPose2ds.size() != 0) dbgField.getObject("cameras").setPoses(cameraPose2ds);
if (processed) dbgField.getObject("visibleTargetPoses").setPoses(visTgtPoses2d);
if (cameraPoses2d.size() != 0) dbgField.getObject("cameras").setPoses(cameraPoses2d);
}
}