commit 0be8deb4cab06050c33c66aa8dcc99d43b95cc98
author James Kuszmaul <jabukuszmaul@gmail.com> Wed May 01 21:35:27 2019 -0500
committer James Kuszmaul <jabukuszmaul@gmail.com> Wed May 01 20:10:50 2019 -0700
tree 65c57599a804ca42374b9c8d020d13ff7ea874ef
parent f0d5a7bd70bfcdfb9f63b0309b21326664fccbe2

Add more checks for non-finite inputs

Change-Id: I6681b7f9550f6c71b9c29ad09d0f8c55d216493e

y2019/control_loops/drivetrain/event_loop_localizer.cc

diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 889133f..7aa380e 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -54,6 +54,10 @@
     ::aos::monotonic_clock::time_point now, double left_encoder,
     double right_encoder, double gyro_rate,
     double /*longitudinal_accelerometer*/) {
+  CHECK(U.allFinite());
+  CHECK(::std::isfinite(left_encoder));
+  CHECK(::std::isfinite(right_encoder));
+  CHECK(::std::isfinite(gyro_rate));
   localizer_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U,
                                    now);
   while (frame_fetcher_.FetchNext()) {