diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h index 9dd8e62890..94d28180d8 100644 --- a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h +++ b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h @@ -64,7 +64,7 @@ class TimeInterpolatableBuffer { * @param sample The sample object. */ void AddSample(units::second_t time, T sample) { - // Add the new state into the vector. + // Add the new state into the vector if (m_pastSnapshots.size() == 0 || time > m_pastSnapshots.back().first) { m_pastSnapshots.emplace_back(time, sample); } else { @@ -72,20 +72,17 @@ class TimeInterpolatableBuffer { m_pastSnapshots.begin(), m_pastSnapshots.end(), time, [](auto t, const auto& pair) { return t < pair.first; }); - // Don't access this before ensuring first_after isn't first. - auto last_not_greater_than = first_after - 1; - - if (first_after == m_pastSnapshots.begin() || - 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)); + if (first_after == m_pastSnapshots.begin()) { + // All entries come after the sample + m_pastSnapshots.insert(first_after, std::pair{time, sample}); + } else if (auto last_not_greater_than = first_after - 1; + last_not_greater_than == m_pastSnapshots.begin() || + last_not_greater_than->first < time) { + // 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. + // An entry exists with the same recorded time last_not_greater_than->second = sample; } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java index e65d27fdf7..a66f71457b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java @@ -11,20 +11,41 @@ import edu.wpi.first.math.geometry.Rotation2d; import org.junit.jupiter.api.Test; class TimeInterpolatableBufferTest { + @Test + void testAddSample() { + TimeInterpolatableBuffer 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 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 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); } } diff --git a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp index e448db2c29..b41ba3bef1 100644 --- a/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp +++ b/wpimath/src/test/native/cpp/interpolation/TimeInterpolatableBufferTest.cpp @@ -11,6 +11,26 @@ #include "frc/interpolation/TimeInterpolatableBuffer.h" #include "units/time.h" +TEST(TimeInterpolatableBufferTest, AddSample) { + frc::TimeInterpolatableBuffer 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 buffer{10_s}; @@ -28,11 +48,14 @@ TEST(TimeInterpolatableBufferTest, Interpolation) { TEST(TimeInterpolatableBufferTest, Pose2d) { frc::TimeInterpolatableBuffer 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); }