return one-sided values in interpolation helper

koide3 · Nov 22, 2023 · be4f9a3 · be4f9a3
1 parent 518cda4
commit be4f9a3
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 0 deletions.
diff --git a/include/glim/util/concurrent_vector.hpp b/include/glim/util/concurrent_vector.hpp
@@ -134,6 +134,10 @@ class ConcurrentVector {
   std::vector<T, Alloc> get_and_clear(int num_max) {
     std::vector<T, Alloc> buffer;
     std::lock_guard<std::mutex> lock(mutex);
+    if (values.empty()) {
+      return buffer;
+    }
+
     if (values.size() <= num_max) {
       buffer.assign(values.begin(), values.end());
       values.clear();

diff --git a/include/glim/util/interpolation_helper.hpp b/include/glim/util/interpolation_helper.hpp
@@ -32,10 +32,16 @@ class InterpolationHelper {
 
   InterpolationHelperResult find(const double stamp, StampedValue* left_ptr, StampedValue* right_ptr, int* remove_cursor_ptr) const {
     if (values.empty() || values.back().first < stamp) {
+      if (!values.empty() && left_ptr) {
+        *left_ptr = values.back();
+      }
       return InterpolationHelperResult::WAITING;
     }
 
     if (values.front().first > stamp) {
+      if (right_ptr) {
+        *right_ptr = values.front();
+      }
       return InterpolationHelperResult::FAILURE;
     }