Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix TimeInterpolatableBuffer crash (#5972)

Browse Source 
Don't decrement buffer iterator if it's at the beginning of the
container.
This commit is contained in:
Tyler Veness
2023-11-30 23:18:38 -08:00
committed by GitHub
parent 5cc923de33
commit 30816111db
3 changed files with 69 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
View File

@@ -11,20 +11,41 @@ import edu.wpi.first.math.geometry.Rotation2d;
import org.junit.jupiter.api.Test;
class TimeInterpolatableBufferTest {
@Test
void testAddSample() {
TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
// No entries
buffer.addSample(1.0, new Rotation2d());
assertEquals(0.0, buffer.getSample(1.0).get().getRadians(), 0.001);
// New entry at start of container
buffer.addSample(0.0, new Rotation2d(1.0));
assertEquals(1.0, buffer.getSample(0.0).get().getRadians(), 0.001);
// New entry in middle of container
buffer.addSample(0.5, new Rotation2d(0.5));
assertEquals(0.5, buffer.getSample(0.5).get().getRadians(), 0.001);
// Override sample
buffer.addSample(0.5, new Rotation2d(1.0));
assertEquals(1.0, buffer.getSample(0.5).get().getRadians(), 0.001);
}
@Test
void testInterpolation() {
TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
buffer.addSample(0, new Rotation2d());
assertEquals(0, buffer.getSample(0).get().getRadians(), 0.001);
buffer.addSample(1, new Rotation2d(1));
buffer.addSample(0.0, new Rotation2d());
assertEquals(0.0, buffer.getSample(0.0).get().getRadians(), 0.001);
buffer.addSample(1.0, new Rotation2d(1.0));
assertEquals(0.5, buffer.getSample(0.5).get().getRadians(), 0.001);
assertEquals(1.0, buffer.getSample(1.0).get().getRadians(), 0.001);
buffer.addSample(3, new Rotation2d(2));
assertEquals(1.5, buffer.getSample(2).get().getRadians(), 0.001);
buffer.addSample(3.0, new Rotation2d(2.0));
assertEquals(1.5, buffer.getSample(2.0).get().getRadians(), 0.001);
buffer.addSample(10.5, new Rotation2d(2));
assertEquals(new Rotation2d(1), buffer.getSample(0).get());
assertEquals(new Rotation2d(1.0), buffer.getSample(0.0).get());
}
@Test
@@ -32,12 +53,12 @@ class TimeInterpolatableBufferTest {
TimeInterpolatableBuffer<Pose2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
// We expect to be at (1 - 1/Math.sqrt(2), 1/Math.sqrt(2), 45deg) at t=0.5
buffer.addSample(0, new Pose2d(0, 0, Rotation2d.fromDegrees(90)));
buffer.addSample(1, new Pose2d(1, 1, Rotation2d.fromDegrees(0)));
buffer.addSample(0.0, new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(90.0)));
buffer.addSample(1.0, new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(0.0)));
Pose2d sample = buffer.getSample(0.5).get();
assertEquals(1 - 1 / Math.sqrt(2), sample.getTranslation().getX(), 0.01);
assertEquals(1 / Math.sqrt(2), sample.getTranslation().getY(), 0.01);
assertEquals(45, sample.getRotation().getDegrees(), 0.01);
assertEquals(1.0 - 1.0 / Math.sqrt(2.0), sample.getTranslation().getX(), 0.01);
assertEquals(1.0 / Math.sqrt(2.0), sample.getTranslation().getY(), 0.01);
assertEquals(45.0, sample.getRotation().getDegrees(), 0.01);
}
}

29
wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp
View File

@@ -11,6 +11,26 @@
#include "frc/interpolation/TimeInterpolatableBuffer.h"
#include "units/time.h"
TEST(TimeInterpolatableBufferTest, AddSample) {
frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
// No entries
buffer.AddSample(1_s, 0_rad);
EXPECT_TRUE(buffer.Sample(1_s).value() == 0_rad);
// New entry at start of container
buffer.AddSample(0_s, 1_rad);
EXPECT_TRUE(buffer.Sample(0_s).value() == 1_rad);
// New entry in middle of container
buffer.AddSample(0.5_s, 0.5_rad);
EXPECT_TRUE(buffer.Sample(0.5_s).value() == 0.5_rad);
// Override sample
buffer.AddSample(0.5_s, 1_rad);
EXPECT_TRUE(buffer.Sample(0.5_s).value() == 1_rad);
}
TEST(TimeInterpolatableBufferTest, Interpolation) {
frc::TimeInterpolatableBuffer<frc::Rotation2d> buffer{10_s};
@@ -28,11 +48,14 @@ TEST(TimeInterpolatableBufferTest, Interpolation) {
TEST(TimeInterpolatableBufferTest, Pose2d) {
frc::TimeInterpolatableBuffer<frc::Pose2d> buffer{10_s};
// We expect to be at (1 - 1/std::sqrt(2), 1/std::sqrt(2), 45deg) at t=0.5
buffer.AddSample(0_s, frc::Pose2d{0_m, 0_m, 90_deg});
buffer.AddSample(1_s, frc::Pose2d{1_m, 1_m, 0_deg});
frc::Pose2d sample = buffer.Sample(0.5_s).value();
EXPECT_TRUE(std::abs(sample.X().value() - (1 - 1 / std::sqrt(2))) < 0.01);
EXPECT_TRUE(std::abs(sample.Y().value() - (1 / std::sqrt(2))) < 0.01);
EXPECT_TRUE(std::abs(sample.Rotation().Degrees().value() - 45) < 0.01);
EXPECT_TRUE(std::abs(sample.X().value() - (1.0 - 1.0 / std::sqrt(2.0))) <
0.01);
EXPECT_TRUE(std::abs(sample.Y().value() - (1.0 / std::sqrt(2.0))) < 0.01);
EXPECT_TRUE(std::abs(sample.Rotation().Degrees().value() - 45.0) < 0.01);
}