[wpimath] Fix TimeInterpolatableBuffer crash (#5972)

Don't decrement buffer iterator if it's at the beginning of the
container.

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;
       }
     }

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<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);
   }
 }

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<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);
 }