Improve handling of absent encoder values
This is the first year that we have relied on the network to forward
encoder values to the localizer, which makes it so that we can no longer
rely on encoder values being available. Improve our behavior in these
cases to prevent us from destroying the localizer's state too much when
we lose encoders/network.
Change-Id: I664d331a8249873987402a819a38ad75e70fc4ff
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index 0986037..14bc2ef 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -41,7 +41,9 @@
template <typename T>
std::optional<Eigen::Vector2d> GetPosition(
T &fetcher, aos::monotonic_clock::time_point now) {
- fetcher.Fetch();
+ if (!fetcher.Fetch()) {
+ return std::nullopt;
+ }
const bool stale =
(fetcher.get() == nullptr) ||
(fetcher.context().monotonic_event_time + std::chrono::milliseconds(10) <
diff --git a/frc971/control_loops/drivetrain/localization/utils.h b/frc971/control_loops/drivetrain/localization/utils.h
index 4d21e3b..8c3a239 100644
--- a/frc971/control_loops/drivetrain/localization/utils.h
+++ b/frc971/control_loops/drivetrain/localization/utils.h
@@ -31,9 +31,9 @@
// [left_voltage, right_voltage]
Eigen::Vector2d VoltageOrZero(aos::monotonic_clock::time_point now);
- // Returns the latest drivetrain encoder values (in meters), or nullopt if no
- // position message is available (or if the message is stale).
- // Returns encoders as [left_position, right_position]
+ // Returns the latest drivetrain encoder values (in meters), or nullopt if
+ // there has been no new encoder reading since the last call. Returns encoders
+ // as [left_position, right_position]
std::optional<Eigen::Vector2d> Encoders(aos::monotonic_clock::time_point now);
// Returns true if either there is no JoystickState message available or if