[wpimath] Fix invalid iterator access in TimeInterpolatableBuffer (#5138

)
wpilibsuite · Feb 27, 2023 · e5c4c6b · e5c4c6b
1 parent 099d048
commit e5c4c6b
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h b/wpimath/src/main/native/include/frc/interpolation/TimeInterpolatableBuffer.h
@@ -72,9 +72,11 @@ 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 (last_not_greater_than == m_pastSnapshots.begin() ||
+      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
@@ -125,6 +127,10 @@ class TimeInterpolatableBuffer {
         m_pastSnapshots.begin(), m_pastSnapshots.end(), time,
         [](const auto& pair, auto t) { return t > pair.first; });
 
+    if (upper_bound == m_pastSnapshots.begin()) {
+      return upper_bound->second;
+    }
+
     auto lower_bound = upper_bound - 1;
 
     double t = ((time - lower_bound->first) /