Swap to combined voltage/encoder message for localizer
Change-Id: I23b553b142bde46c321409105d49a716eb7059b6
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/localization/BUILD b/frc971/control_loops/drivetrain/localization/BUILD
index 747b16c..1cf83d4 100644
--- a/frc971/control_loops/drivetrain/localization/BUILD
+++ b/frc971/control_loops/drivetrain/localization/BUILD
@@ -13,6 +13,7 @@
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
"//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/control_loops/drivetrain:hybrid_ekf",
+ "//frc971/control_loops/drivetrain:rio_localizer_inputs_fbs",
"//frc971/input:joystick_state_fbs",
"//frc971/vision:calibration_fbs",
"@org_tuxfamily_eigen//:eigen",
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index 1f4aadc..0986037 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -4,39 +4,62 @@
LocalizationUtils::LocalizationUtils(aos::EventLoop *event_loop)
: event_loop_(event_loop),
- output_fetcher_(event_loop->MakeFetcher<Output>("/drivetrain")),
+ output_fetcher_(event_loop->TryMakeFetcher<Output>("/drivetrain")),
position_fetcher_(event_loop->TryMakeFetcher<Position>("/drivetrain")),
+ combined_fetcher_(
+ event_loop->TryMakeFetcher<RioLocalizerInputs>("/drivetrain")),
clock_offset_fetcher_(
event_loop->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/aos")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/roborio/aos")) {}
+namespace {
+template <typename T>
+Eigen::Vector2d GetVoltage(T &fetcher, aos::monotonic_clock::time_point now) {
+ fetcher.Fetch();
+ // Determine if the robot is likely to be disabled currently.
+ const bool disabled =
+ (fetcher.get() == nullptr) ||
+ (fetcher.context().monotonic_event_time + std::chrono::milliseconds(10) <
+ now);
+ return disabled ? Eigen::Vector2d::Zero()
+ : Eigen::Vector2d{fetcher->left_voltage(),
+ fetcher->right_voltage()};
+}
+} // namespace
Eigen::Vector2d LocalizationUtils::VoltageOrZero(
aos::monotonic_clock::time_point now) {
- output_fetcher_.Fetch();
- // Determine if the robot is likely to be disabled currently.
- const bool disabled = (output_fetcher_.get() == nullptr) ||
- (output_fetcher_.context().monotonic_event_time +
- std::chrono::milliseconds(10) <
- now);
- return disabled ? Eigen::Vector2d::Zero()
- : Eigen::Vector2d{output_fetcher_->left_voltage(),
- output_fetcher_->right_voltage()};
+ if (output_fetcher_.valid()) {
+ return GetVoltage(output_fetcher_, now);
+ } else {
+ CHECK(combined_fetcher_.valid());
+ return GetVoltage(combined_fetcher_, now);
+ }
}
+namespace {
+template <typename T>
+std::optional<Eigen::Vector2d> GetPosition(
+ T &fetcher, aos::monotonic_clock::time_point now) {
+ fetcher.Fetch();
+ const bool stale =
+ (fetcher.get() == nullptr) ||
+ (fetcher.context().monotonic_event_time + std::chrono::milliseconds(10) <
+ now);
+ return stale ? std::nullopt
+ : std::make_optional<Eigen::Vector2d>(fetcher->left_encoder(),
+ fetcher->right_encoder());
+}
+} // namespace
std::optional<Eigen::Vector2d> LocalizationUtils::Encoders(
aos::monotonic_clock::time_point now) {
- CHECK(position_fetcher_.valid());
- position_fetcher_.Fetch();
- const bool stale = (position_fetcher_.get() == nullptr) ||
- (position_fetcher_.context().monotonic_event_time +
- std::chrono::milliseconds(10) <
- now);
- return stale ? std::nullopt
- : std::make_optional<Eigen::Vector2d>(
- position_fetcher_->left_encoder(),
- position_fetcher_->right_encoder());
+ if (position_fetcher_.valid()) {
+ return GetPosition(position_fetcher_, now);
+ } else {
+ CHECK(combined_fetcher_.valid());
+ return GetPosition(combined_fetcher_, now);
+ }
}
bool LocalizationUtils::MaybeInAutonomous() {
diff --git a/frc971/control_loops/drivetrain/localization/utils.h b/frc971/control_loops/drivetrain/localization/utils.h
index 07f9aff..4d21e3b 100644
--- a/frc971/control_loops/drivetrain/localization/utils.h
+++ b/frc971/control_loops/drivetrain/localization/utils.h
@@ -7,6 +7,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+#include "frc971/control_loops/drivetrain/rio_localizer_inputs_generated.h"
#include "frc971/control_loops/pose.h"
#include "frc971/input/joystick_state_generated.h"
#include "frc971/vision/calibration_generated.h"
@@ -50,6 +51,8 @@
aos::EventLoop *const event_loop_;
aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
aos::Fetcher<frc971::control_loops::drivetrain::Position> position_fetcher_;
+ aos::Fetcher<frc971::control_loops::drivetrain::RioLocalizerInputs>
+ combined_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
};