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

[photonlib] Simulation Visualization Update (#895)

Browse Source
This commit is contained in:
amquake
2023-09-19 16:10:04 -07:00
committed by GitHub
parent 9e371de1cb
commit 7f283640c4
11 changed files with 1140 additions and 335 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -130,7 +130,8 @@ public class VisionSystemSim {
* Get a simulated camera's position relative to the robot. If the requested camera is invalid, an
* empty optional is returned.
*
* @return The transform of this cameraSim, or an empty optional if it is invalid
* @param cameraSim The specific camera to get the robot-to-camera transform of
* @return The transform of this camera, or an empty optional if it is invalid
*/
public Optional<Transform3d> getRobotToCamera(PhotonCameraSim cameraSim) {
return getRobotToCamera(cameraSim, Timer.getFPGATimestamp());
@@ -140,9 +141,9 @@ public class VisionSystemSim {
* Get a simulated camera's position relative to the robot. If the requested camera is invalid, an
* empty optional is returned.
*
* @param cameraSim Specific camera to get the robot-to-camera transform of
* @param cameraSim The specific camera to get the robot-to-camera transform of
* @param timeSeconds Timestamp in seconds of when the transform should be observed
* @return The transform of this cameraSim, or an empty optional if it is invalid
* @return The transform of this camera, or an empty optional if it is invalid
*/
public Optional<Transform3d> getRobotToCamera(PhotonCameraSim cameraSim, double timeSeconds) {
var trfBuffer = camTrfMap.get(cameraSim);
@@ -152,6 +153,31 @@ public class VisionSystemSim {
return Optional.of(new Transform3d(new Pose3d(), sample.orElse(new Pose3d())));
}
/**
* Get a simulated camera's position on the field. If the requested camera is invalid, an empty
* optional is returned.
*
* @param cameraSim The specific camera to get the field pose of
* @return The pose of this camera, or an empty optional if it is invalid
*/
public Optional<Pose3d> getCameraPose(PhotonCameraSim cameraSim) {
return getCameraPose(cameraSim, Timer.getFPGATimestamp());
}
/**
* Get a simulated camera's position on the field. If the requested camera is invalid, an empty
* optional is returned.
*
* @param cameraSim The specific camera to get the field pose of
* @param timeSeconds Timestamp in seconds of when the pose should be observed
* @return The pose of this camera, or an empty optional if it is invalid
*/
public Optional<Pose3d> getCameraPose(PhotonCameraSim cameraSim, double timeSeconds) {
var robotToCamera = getRobotToCamera(cameraSim, timeSeconds);
if (robotToCamera.isEmpty()) return Optional.empty();
return Optional.of(getRobotPose(timeSeconds).plus(robotToCamera.get()));
}
/**
* Adjust a camera's position relative to the robot. Use this if your camera is on a gimbal or
* turret or some other mobile platform.
@@ -217,15 +243,16 @@ public class VisionSystemSim {
* PhotonCamera}s simulated from this system will report the location of the camera relative to
* the subset of these targets which are visible from the given camera position.
*
* <p>The AprilTags from this layout will be added as vision targets under the type "apriltags".
* The poses added preserve the tag layout's current alliance origin.
* <p>The AprilTags from this layout will be added as vision targets under the type "apriltag".
* The poses added preserve the tag layout's current alliance origin. If the tag layout's alliance
* origin is changed, these added tags will have to be cleared and re-added.
*
* @param tagLayout The field tag layout to get Apriltag poses and IDs from
*/
public void addVisionTargets(AprilTagFieldLayout tagLayout) {
public void addAprilTags(AprilTagFieldLayout tagLayout) {
for (AprilTag tag : tagLayout.getTags()) {
addVisionTargets(
"apriltags",
"apriltag",
new VisionTargetSim(
tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation
TargetModel.kTag16h5,
@@ -252,6 +279,10 @@ public class VisionSystemSim {
targetSets.clear();
}
public void clearAprilTags() {
removeVisionTargets("apriltag");
}
public Set<VisionTargetSim> removeVisionTargets(String type) {
return targetSets.remove(type);
}