Pull some year-generic code out of y2022 localizer
Pull out some utilities that I want to re-use in 2023.
Change-Id: I5cd03e942708d240441b8356f37eed029e0d8710
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/localizer/BUILD b/y2022/localizer/BUILD
index e5c054b..a3b1774 100644
--- a/y2022/localizer/BUILD
+++ b/y2022/localizer/BUILD
@@ -107,6 +107,7 @@
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/control_loops/drivetrain:localization_utils",
"//frc971/control_loops/drivetrain:localizer_fbs",
"//frc971/imu_reader:imu_watcher",
"//frc971/input:joystick_state_fbs",
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index e7dfdf1..46eff94 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -871,18 +871,10 @@
output_sender_(event_loop_->MakeSender<LocalizerOutput>("/localizer")),
visualization_sender_(
event_loop_->MakeSender<LocalizerVisualization>("/localizer")),
- output_fetcher_(
- event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Output>(
- "/drivetrain")),
- clock_offset_fetcher_(
- event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
- "/aos")),
superstructure_fetcher_(
event_loop_
->MakeFetcher<y2022::control_loops::superstructure::Status>(
"/superstructure")),
- joystick_state_fetcher_(
- event_loop_->MakeFetcher<aos::JoystickState>("/roborio/aos")),
imu_watcher_(event_loop, dt_config,
y2022::constants::Values::DrivetrainEncoderToMeters(1),
[this](aos::monotonic_clock::time_point sample_time_pico,
@@ -891,7 +883,8 @@
Eigen::Vector3d gyro, Eigen::Vector3d accel) {
HandleImu(sample_time_pico, sample_time_pi, encoders, gyro,
accel);
- }) {
+ }),
+ utils_(event_loop) {
event_loop->SetRuntimeRealtimePriority(10);
event_loop_->MakeWatcher(
"/drivetrain",
@@ -929,10 +922,7 @@
absl::StrCat("/", kPisToUse[camera_index], "/camera"));
}
aos::TimerHandler *estimate_timer = event_loop_->AddTimer([this]() {
- joystick_state_fetcher_.Fetch();
- const bool maybe_in_auto = (joystick_state_fetcher_.get() != nullptr)
- ? joystick_state_fetcher_->autonomous()
- : true;
+ const bool maybe_in_auto = utils_.MaybeInAutonomous();
model_based_.set_use_aggressive_image_corrections(!maybe_in_auto);
for (size_t camera_index = 0; camera_index < kPisToUse.size();
++camera_index) {
@@ -946,8 +936,10 @@
}
if (target_estimate_fetchers_[camera_index].Fetch()) {
const std::optional<aos::monotonic_clock::duration> monotonic_offset =
- ClockOffset(kPisToUse[camera_index]);
+ utils_.ClockOffset(kPisToUse[camera_index]);
if (!monotonic_offset.has_value()) {
+ model_based_.TallyRejection(
+ RejectionReason::MESSAGE_BRIDGE_DISCONNECTED);
continue;
}
// TODO(james): Get timestamp from message contents.
@@ -980,20 +972,11 @@
aos::monotonic_clock::time_point sample_time_pi,
std::optional<Eigen::Vector2d> encoders, Eigen::Vector3d gyro,
Eigen::Vector3d accel) {
- output_fetcher_.Fetch();
- {
- const bool disabled = (output_fetcher_.get() == nullptr) ||
- (output_fetcher_.context().monotonic_event_time +
- std::chrono::milliseconds(10) <
- event_loop_->context().monotonic_event_time);
- // For gyros, use the most recent gyro reading if we aren't zeroed,
- // to avoid missing integration cycles.
- model_based_.HandleImu(
- sample_time_pico, gyro, accel, encoders,
- disabled ? Eigen::Vector2d::Zero()
- : Eigen::Vector2d{output_fetcher_->left_voltage(),
- output_fetcher_->right_voltage()});
- }
+
+ model_based_.HandleImu(
+ sample_time_pico, gyro, accel, encoders,
+ utils_.VoltageOrZero(event_loop_->context().monotonic_event_time));
+
{
auto builder = status_sender_.MakeBuilder();
const flatbuffers::Offset<ModelBasedStatus> model_based_status =
@@ -1052,30 +1035,4 @@
}
}
-std::optional<aos::monotonic_clock::duration> EventLoopLocalizer::ClockOffset(
- std::string_view pi) {
- std::optional<aos::monotonic_clock::duration> monotonic_offset;
- clock_offset_fetcher_.Fetch();
- if (clock_offset_fetcher_.get() != nullptr) {
- for (const auto connection : *clock_offset_fetcher_->connections()) {
- if (connection->has_node() && connection->node()->has_name() &&
- connection->node()->name()->string_view() == pi) {
- if (connection->has_monotonic_offset()) {
- monotonic_offset =
- std::chrono::nanoseconds(connection->monotonic_offset());
- } else {
- // If we don't have a monotonic offset, that means we aren't
- // connected.
- model_based_.TallyRejection(
- RejectionReason::MESSAGE_BRIDGE_DISCONNECTED);
- return std::nullopt;
- }
- break;
- }
- }
- }
- CHECK(monotonic_offset.has_value());
- return monotonic_offset;
-}
-
} // namespace frc971::controls
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index 917f131..a403ca8 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -12,6 +12,7 @@
#include "frc971/input/joystick_state_generated.h"
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/control_loops/drivetrain/localization_utils.h"
#include "frc971/zeroing/imu_zeroer.h"
#include "frc971/zeroing/wrap.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
@@ -326,8 +327,6 @@
ModelBasedLocalizer *localizer() { return &model_based_; }
private:
- std::optional<aos::monotonic_clock::duration> ClockOffset(
- std::string_view pi);
void HandleImu(aos::monotonic_clock::time_point sample_time_pico,
aos::monotonic_clock::time_point sample_time_pi,
std::optional<Eigen::Vector2d> encoders, Eigen::Vector3d gyro,
@@ -337,21 +336,18 @@
aos::Sender<LocalizerStatus> status_sender_;
aos::Sender<LocalizerOutput> output_sender_;
aos::Sender<LocalizerVisualization> visualization_sender_;
- aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
- aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
std::array<aos::Fetcher<y2022::vision::TargetEstimate>,
ModelBasedLocalizer::kNumPis>
target_estimate_fetchers_;
aos::Fetcher<y2022::control_loops::superstructure::Status>
superstructure_fetcher_;
- aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
- zeroing::ImuZeroer zeroer_;
aos::monotonic_clock::time_point last_output_send_ =
aos::monotonic_clock::min_time;
aos::monotonic_clock::time_point last_visualization_send_ =
aos::monotonic_clock::min_time;
ImuWatcher imu_watcher_;
+ control_loops::drivetrain::LocalizationUtils utils_;
};
} // namespace frc971::controls
#endif // Y2022_LOCALIZER_LOCALIZER_H_