Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +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

24
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -194,10 +194,16 @@ class SwerveDrivePoseEstimator {
// Step 5: Reset Odometry to state at sample with vision adjustment.
m_odometry.ResetPosition(sample.value().gyroAngle,
sample.value().modulePostions,
sample.value().modulePositions,
sample.value().pose.Exp(scaledTwist));
// Step 6: Replay odometry inputs between sample time and latest recorded
// Step 6: Record the current pose to allow multiple measurements from the
// same timestamp
m_poseBuffer.AddSample(timestamp,
{GetEstimatedPosition(), sample.value().gyroAngle,
sample.value().modulePositions});
// Step 7: Replay odometry inputs between sample time and latest recorded
// sample to update the pose buffer and correct odometry.
auto internal_buf = m_poseBuffer.GetInternalBuffer();
@@ -207,7 +213,7 @@ class SwerveDrivePoseEstimator {
for (auto entry = upper_bound; entry != internal_buf.end(); entry++) {
UpdateWithTime(entry->first, entry->second.gyroAngle,
entry->second.modulePostions);
entry->second.modulePositions);
}
}
@@ -300,7 +306,7 @@ class SwerveDrivePoseEstimator {
Rotation2d gyroAngle;
// The distances traveled and rotations meaured at each module.
wpi::array<SwerveModulePosition, NumModules> modulePostions;
wpi::array<SwerveModulePosition, NumModules> modulePositions;
/**
* Checks equality between this InterpolationRecord and another object.
@@ -344,14 +350,14 @@ class SwerveDrivePoseEstimator {
for (size_t i = 0; i < NumModules; i++) {
modulePositions[i].distance =
wpi::Lerp(this->modulePostions[i].distance,
endValue.modulePostions[i].distance, i);
wpi::Lerp(this->modulePositions[i].distance,
endValue.modulePositions[i].distance, i);
modulePositions[i].angle =
wpi::Lerp(this->modulePostions[i].angle,
endValue.modulePostions[i].angle, i);
wpi::Lerp(this->modulePositions[i].angle,
endValue.modulePositions[i].angle, i);
modulesDelta[i].distance =
modulePositions[i].distance - this->modulePostions[i].distance;
modulePositions[i].distance - this->modulePositions[i].distance;
modulesDelta[i].angle = modulePositions[i].angle;
}

23
wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
View File

@@ -68,11 +68,24 @@ class TimeInterpolatableBuffer {
if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) {
m_pastSnapshots.emplace_back(time, sample);
} else {
m_pastSnapshots.insert(
std::upper_bound(
m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
[](auto t, const auto& pair) { return t < pair.first; }),
std::pair(time, sample));
auto first_after = std::upper_bound(
m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
[](auto t, const auto& pair) { return t < pair.first; });
auto last_not_greater_than = first_after - 1;
if (last_not_greater_than == m_pastSnapshots.begin() ||
last_not_greater_than->first < time) {
// Two cases handled together:
// 1. All entries come after the sample
// 2. Some entries come before the sample, but none are recorded with
// the same time
m_pastSnapshots.insert(first_after, std::pair(time, sample));
} else {
// Final case:
// 3. An entry exists with the same recorded time.
last_not_greater_than->second = sample;
}
}
while (time - m_pastSnapshots[0].first > m_historySize) {
m_pastSnapshots.erase(m_pastSnapshots.begin());