commit ba59dc98dbb4f4e646c76c4e9a4df520e641ac5e
author James Kuszmaul <jabukuszmaul@gmail.com> Sat Mar 12 10:46:54 2022 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 12 10:55:12 2022 -0800
tree 6997e588b6f4b888e7ac62e105a208215388bc2c
parent 1b73423dab39b17d0862839280351e010a4f77f1

check optional access in y2022 drivetrain

Change-Id: Ic098c4a0931799237ff04d5e2524fa672ba81b49
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>

frc971/control_loops/drivetrain/hybrid_ekf.h

diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 2a972b6..fcbed76 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -330,6 +330,12 @@
     }
     return X_hat();
   }
+  std::optional<State> OldestState() {
+    if (observations_.empty()) {
+      return std::nullopt;
+    }
+    return observations_.begin()->X_hat;
+  }
 
   // Returns the most recent input vector.
   Input MostRecentInput() {

y2022/control_loops/drivetrain/localizer.cc

diff --git a/y2022/control_loops/drivetrain/localizer.cc b/y2022/control_loops/drivetrain/localizer.cc
index 3d02312..0735534 100644
--- a/y2022/control_loops/drivetrain/localizer.cc
+++ b/y2022/control_loops/drivetrain/localizer.cc
@@ -71,8 +71,15 @@
         monotonic_offset);
     // TODO: Finish implementing simple x/y/theta updater with state_at_capture.
     // TODO: Implement turret/camera processing logic on pi side.
-    const std::optional<State> state_at_capture =
+    std::optional<State> state_at_capture =
         ekf_.LastStateBeforeTime(capture_time);
+    if (!state_at_capture.has_value()) {
+      state_at_capture = ekf_.OldestState();
+      if (!state_at_capture.has_value()) {
+        return;
+      }
+    }
+
     Eigen::Matrix<float, HybridEkf::kNOutputs, HybridEkf::kNStates> H;
     H.setZero();
     H(0, StateIdx::kX) = 1;