Skip to content

Adding whether a given target observation should be considered stale #594

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Apr 2, 2025
Merged

Adding whether a given target observation should be considered stale #594

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7a1195f
Adding whether a given target observation should be considered stale …
rokadias Apr 1, 2025
87da258
Merge branch 'main' into saqib/pv-status
stephenjust Apr 2, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 10 additions & 6 deletions src/main/java/xbot/common/subsystems/vision/AprilTagVisionIO.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,15 +26,17 @@ public interface AprilTagVisionIO {
@AutoLog
class VisionIOInputs {
public boolean connected = false;
public TargetObservation latestTargetObservation =
new TargetObservation(0, new Rotation2d(), new Rotation2d(), new Transform3d(), 1);
public TargetObservation latestTargetObservation = new TargetObservation(0, 0, new Rotation2d(),
new Rotation2d(), new Transform3d(), 1, true);
public TargetObservation[] targetObservations = new TargetObservation[0];
public PoseObservation[] poseObservations = new PoseObservation[0];
public int[] tagIds = new int[0];
}

/** Represents the angle to a simple target, not used for pose estimation. */
record TargetObservation(int fiducialId, Rotation2d tx, Rotation2d ty, Transform3d cameraToTarget, double ambiguity) {}
record TargetObservation(double timestamp, int fiducialId, Rotation2d tx, Rotation2d ty, Transform3d cameraToTarget,
double ambiguity, boolean stale) {
}

/** Represents a robot pose sample used for pose estimation. */
record PoseObservation(
Expand All @@ -43,13 +45,15 @@ record PoseObservation(
double ambiguity,
int tagCount,
double averageTagDistance,
PoseObservationType type) {}
PoseObservationType type) {
}

enum PoseObservationType {
MEGATAG_1,
MEGATAG_2,
PHOTONVISION
}

default void updateInputs(VisionIOInputs inputs) {}
}
default void updateInputs(VisionIOInputs inputs) {
}
}
34 changes: 22 additions & 12 deletions src/main/java/xbot/common/subsystems/vision/AprilTagVisionIOPhotonVision.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.wpilibj.Timer;

import java.util.HashSet;
import java.util.LinkedList;
Expand All @@ -43,7 +44,11 @@ public abstract static class FactoryImpl implements AprilTagVisionIOFactory {
private static final int[] EMPTY_TAG_IDS_OBSERVATION = new int[0];
private static final PoseObservation[] EMPTY_POSE_OBSERVATION = new PoseObservation[0];
private static final TargetObservation[] EMPTY_TARGET_OBSERVATIONS = new TargetObservation[0];
private static final TargetObservation EMPTY_TARGET_OBSERVATION = new TargetObservation(0, new Rotation2d(), new Rotation2d(), new Transform3d(), 1);
private static final TargetObservation EMPTY_TARGET_OBSERVATION = new TargetObservation(0, 0, new Rotation2d(),
new Rotation2d(), new Transform3d(), 1, true);
// Using same heartbeat bounce as photonvision:
// https://github.com/PhotonVision/photonvision/blob/3c332db4bfe9083fc0311ae71cff92de588939ad/photon-lib/src/main/java/org/photonvision/PhotonCamera.java#L107
private static final double HEARTBEAT_DEBOUNCE_SEC = 0.5;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

maybe make this public, so the next time we need to reference it we don't need a copy-paste (I wish PV folks had made this a public constant)


protected final PhotonCamera camera;
protected final Transform3d robotToCamera;
Expand All @@ -57,7 +62,8 @@ public abstract static class FactoryImpl implements AprilTagVisionIOFactory {
* @param fieldLayout The April Tag field layout.
*/
@AssistedInject
public AprilTagVisionIOPhotonVision(@Assisted String name, @Assisted Transform3d robotToCamera, AprilTagFieldLayout fieldLayout) {
public AprilTagVisionIOPhotonVision(@Assisted String name, @Assisted Transform3d robotToCamera,
AprilTagFieldLayout fieldLayout) {
camera = new PhotonCamera(name);
this.robotToCamera = robotToCamera;
this.aprilTagFieldLayout = fieldLayout;
Expand All @@ -80,26 +86,30 @@ public void updateInputs(VisionIOInputs inputs) {
List<PoseObservation> poseObservations = new LinkedList<>();

for (var result : camera.getAllUnreadResults()) {
boolean stale = Timer.getFPGATimestamp() - result.getTimestampSeconds() > HEARTBEAT_DEBOUNCE_SEC;
// Update latest target observation
if (result.hasTargets()) {
var bestTarget = result.getBestTarget();
inputs.latestTargetObservation =
new TargetObservation(
bestTarget.getFiducialId(),
Rotation2d.fromDegrees(bestTarget.getYaw()),
Rotation2d.fromDegrees(bestTarget.getPitch()),
bestTarget.getBestCameraToTarget(),
bestTarget.getPoseAmbiguity());
inputs.latestTargetObservation = new TargetObservation(
result.getTimestampSeconds(),
bestTarget.getFiducialId(),
Rotation2d.fromDegrees(bestTarget.getYaw()),
Rotation2d.fromDegrees(bestTarget.getPitch()),
bestTarget.getBestCameraToTarget(),
bestTarget.getPoseAmbiguity(),
stale);

var targetObservations = new TargetObservation[result.targets.size()];
var targetIndex = 0;
for (var target : result.targets) {
targetObservations[targetIndex++] = new TargetObservation(
result.getTimestampSeconds(),
target.fiducialId,
Rotation2d.fromDegrees(target.getYaw()),
Rotation2d.fromDegrees(target.getPitch()),
target.getBestCameraToTarget(),
target.getPoseAmbiguity());
target.getPoseAmbiguity(),
stale);
}
inputs.targetObservations = targetObservations;
} else {
Expand Down Expand Up @@ -141,8 +151,8 @@ public void updateInputs(VisionIOInputs inputs) {
// Calculate robot pose
var tagPose = this.aprilTagFieldLayout.getTagPose(target.fiducialId);
if (tagPose.isPresent()) {
Transform3d fieldToTarget =
new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation());
Transform3d fieldToTarget = new Transform3d(tagPose.get().getTranslation(),
tagPose.get().getRotation());
Transform3d cameraToTarget = target.bestCameraToTarget;
Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse());
Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse());
Expand Down
8 changes: 7 additions & 1 deletion src/main/java/xbot/common/subsystems/vision/MockAprilTagVisionIO.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import dagger.assisted.AssistedInject;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.wpilibj.Timer;

import java.lang.annotation.Target;

Expand All @@ -19,7 +20,8 @@ public class MockAprilTagVisionIO implements AprilTagVisionIO {
public boolean connected = true;
public int[] tagIds = {};
public PoseObservation[] poseObservations = {};
public TargetObservation latestTargetObservation = new TargetObservation(1, new Rotation2d(), new Rotation2d(), new Transform3d(), 1);
public TargetObservation latestTargetObservation = new TargetObservation(0, 1, new Rotation2d(), new Rotation2d(),
new Transform3d(), 1, false);
public TargetObservation[] targetObservations = { latestTargetObservation };

@AssistedFactory
Expand All @@ -35,6 +37,10 @@ public MockAprilTagVisionIO(@Assisted String name, @Assisted Transform3d robotTo

@Override
public void updateInputs(VisionIOInputs inputs) {
this.latestTargetObservation = new TargetObservation(Timer.getFPGATimestamp(),
this.latestTargetObservation.fiducialId(), this.latestTargetObservation.tx(),
this.latestTargetObservation.ty(), this.latestTargetObservation.cameraToTarget(),
this.latestTargetObservation.ambiguity(), false);
inputs.connected = this.connected;
inputs.tagIds = this.tagIds;
inputs.poseObservations = this.poseObservations;
Expand Down
29 changes: 18 additions & 11 deletions src/test/java/xbot/common/subsystems/vision/AprilTagVisionSubsystemTest.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,16 +29,19 @@ public void testBasicOperation() {
public void testHandleResult() {
var subsystem = this.getInjectorComponent().getAprilTagVisionSubsystem();

var io = (MockAprilTagVisionIO)subsystem.io[0];
var io = (MockAprilTagVisionIO) subsystem.io[0];
io.poseObservations = new AprilTagVisionIO.PoseObservation[] {
new AprilTagVisionIO.PoseObservation(1, new Pose3d(new Translation3d(0.1, 0.1, 0.1), new Rotation3d()), 0, 2, 0,
new AprilTagVisionIO.PoseObservation(1, new Pose3d(new Translation3d(0.1, 0.1, 0.1), new Rotation3d()),
0, 2, 0,
AprilTagVisionIO.PoseObservationType.PHOTONVISION),
new AprilTagVisionIO.PoseObservation(2, new Pose3d(new Translation3d(0.1, 0.1, 0.1), new Rotation3d()), 0, 2, 0,
new AprilTagVisionIO.PoseObservation(2, new Pose3d(new Translation3d(0.1, 0.1, 0.1), new Rotation3d()),
0, 2, 0,
AprilTagVisionIO.PoseObservationType.PHOTONVISION),
new AprilTagVisionIO.PoseObservation(3, new Pose3d(new Translation3d(0.1, 0.1, 0.1), new Rotation3d()), 0, 2, 0,
new AprilTagVisionIO.PoseObservation(3, new Pose3d(new Translation3d(0.1, 0.1, 0.1), new Rotation3d()),
0, 2, 0,
AprilTagVisionIO.PoseObservationType.PHOTONVISION)
};
io.tagIds = new int[] {1, 2, 3};
io.tagIds = new int[] { 1, 2, 3 };
subsystem.refreshDataFrame();
subsystem.periodic();

Expand All @@ -59,14 +62,18 @@ public void testHandleResult() {
public void testHandleTargetObservations() {
var subsystem = this.getInjectorComponent().getAprilTagVisionSubsystem();

var io = (MockAprilTagVisionIO)subsystem.io[0];
io.latestTargetObservation = new AprilTagVisionIO.TargetObservation(1, new Rotation2d(), new Rotation2d(), new Transform3d(), 0.2);
var io = (MockAprilTagVisionIO) subsystem.io[0];
io.latestTargetObservation = new AprilTagVisionIO.TargetObservation(0, 1, new Rotation2d(), new Rotation2d(),
new Transform3d(), 0.2, false);
io.targetObservations = new AprilTagVisionIO.TargetObservation[] {
new AprilTagVisionIO.TargetObservation(1, new Rotation2d(), new Rotation2d(), new Transform3d(), 0.2),
new AprilTagVisionIO.TargetObservation(2, new Rotation2d(), new Rotation2d(), new Transform3d(), 0.2),
new AprilTagVisionIO.TargetObservation(3, new Rotation2d(), new Rotation2d(), new Transform3d(), 0.2)
new AprilTagVisionIO.TargetObservation(0, 1, new Rotation2d(), new Rotation2d(), new Transform3d(), 0.2,
false),
new AprilTagVisionIO.TargetObservation(0, 2, new Rotation2d(), new Rotation2d(), new Transform3d(), 0.2,
false),
new AprilTagVisionIO.TargetObservation(0, 3, new Rotation2d(), new Rotation2d(), new Transform3d(), 0.2,
false)
};
io.tagIds = new int[] {1, 2, 3};
io.tagIds = new int[] { 1, 2, 3 };
subsystem.refreshDataFrame();
subsystem.periodic();

Expand Down