Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Allow multiple vision measurements from same timestamp (#4917)

Browse Source 
Co-authored-by: Jordan McMichael <jlmcmchl@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2023-01-12 02:04:30 -05:00
committed by GitHub
parent befd12911c
commit f7f19207e0
13 changed files with 409 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
View File

@@ -212,7 +212,16 @@ public class DifferentialDrivePoseEstimator {
sample.get().rightMeters,
sample.get().poseMeters.exp(scaledTwist));
// Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
// Step 6: Record the current pose to allow multiple measurements from the same timestamp
m_poseBuffer.addSample(
timestampSeconds,
new InterpolationRecord(
getEstimatedPosition(),
sample.get().gyroAngle,
sample.get().leftMeters,
sample.get().rightMeters));
// Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
// pose buffer and correct odometry.
for (Map.Entry<Double, InterpolationRecord> entry :
m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {

8
wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
View File

@@ -199,7 +199,13 @@ public class MecanumDrivePoseEstimator {
sample.get().wheelPositions,
sample.get().poseMeters.exp(scaledTwist));
// Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
// Step 6: Record the current pose to allow multiple measurements from the same timestamp
m_poseBuffer.addSample(
timestampSeconds,
new InterpolationRecord(
getEstimatedPosition(), sample.get().gyroAngle, sample.get().wheelPositions));
// Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
// pose buffer and correct odometry.
for (Map.Entry<Double, InterpolationRecord> entry :
m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {

8
wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
View File

@@ -201,7 +201,13 @@ public class SwerveDrivePoseEstimator {
sample.get().modulePositions,
sample.get().poseMeters.exp(scaledTwist));
// Step 6: Replay odometry inputs between sample time and latest recorded sample to update the
// Step 6: Record the current pose to allow multiple measurements from the same timestamp
m_poseBuffer.addSample(
timestampSeconds,
new InterpolationRecord(
getEstimatedPosition(), sample.get().gyroAngle, sample.get().modulePositions));
// Step 7: Replay odometry inputs between sample time and latest recorded sample to update the
// pose buffer and correct odometry.
for (Map.Entry<Double, InterpolationRecord> entry :
m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) {