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

Use ReadQueue for PhotonCamera timestamps (#1316)

Browse Source 
This removes the extra GetLastChange call to keep everything properly
atomic.

Closes #1303
This commit is contained in:
Matt
2024-08-04 14:23:46 -04:00
committed by GitHub
parent 37e9d40762
commit 67463a020a
29 changed files with 1057 additions and 1614 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java
View File

@@ -46,7 +46,6 @@ import org.photonvision.targeting.PhotonPipelineResult;
public class Vision {
private final PhotonCamera camera;
private final PhotonPoseEstimator photonEstimator;
private double lastEstTimestamp = 0;
// Simulation
private PhotonCameraSim cameraSim;
@@ -56,8 +55,7 @@ public class Vision {
camera = new PhotonCamera(kCameraName);
photonEstimator =
new PhotonPoseEstimator(
kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, kRobotToCam);
new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, kRobotToCam);
photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
// ----- Simulation
@@ -95,20 +93,21 @@ public class Vision {
* used for estimation.
*/
public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
var visionEst = photonEstimator.update();
double latestTimestamp = camera.getLatestResult().getTimestampSeconds();
boolean newResult = Math.abs(latestTimestamp - lastEstTimestamp) > 1e-5;
if (Robot.isSimulation()) {
visionEst.ifPresentOrElse(
est ->
getSimDebugField()
.getObject("VisionEstimation")
.setPose(est.estimatedPose.toPose2d()),
() -> {
if (newResult) getSimDebugField().getObject("VisionEstimation").setPoses();
});
Optional<EstimatedRobotPose> visionEst = Optional.empty();
for (var change : camera.getAllUnreadResults()) {
visionEst = photonEstimator.update(change);
if (Robot.isSimulation()) {
visionEst.ifPresentOrElse(
est ->
getSimDebugField()
.getObject("VisionEstimation")
.setPose(est.estimatedPose.toPose2d()),
() -> {
getSimDebugField().getObject("VisionEstimation").setPoses();
});
}
}
if (newResult) lastEstTimestamp = latestTimestamp;
return visionEst;
}

9
photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java
View File

@@ -24,6 +24,7 @@
package frc.robot.subsystems.drivetrain;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
@@ -114,18 +115,18 @@ public class SwerveDriveSim {
SwerveDriveKinematics kinematics) {
this(
new LinearSystem<N2, N1, N2>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -driveFF.kv / driveFF.ka),
MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -driveFF.kv / driveFF.ka),
VecBuilder.fill(0.0, 1.0 / driveFF.ka),
Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0),
MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0),
VecBuilder.fill(0.0, 0.0)),
driveFF.ks,
driveMotor,
driveGearing,
driveWheelRadius,
new LinearSystem<N2, N1, N2>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -steerFF.kv / steerFF.ka),
MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -steerFF.kv / steerFF.ka),
VecBuilder.fill(0.0, 1.0 / steerFF.ka),
Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0),
MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0),
VecBuilder.fill(0.0, 0.0)),
steerFF.ks,
steerMotor,