Fix crash when simulated event loop gets destroyed before OnRun called
We were saving a std::function for each OnRun callback inside the
EventScheduler, and continuing to call it regardless of if the EventLoop
which added it was still alive. This was triggering segfaults when
trying to access the destroyed event loop. Instead, save a pointer to
the event loop, and clean it up when the event loop is destroyed so this
works as the user would expect.
While we are here, and since the behavior needs to change as part of
this change, only let OnRun callbacks be scheduled before the event loop
has run any. Test this too.
Change-Id: I467a00c9f0981669e77fd03927ceeb149a79e6c4
Signed-off-by: James Kuszmaul <james.kuszmaul@bluerivertech.com>
diff --git a/aos/events/event_scheduler.cc b/aos/events/event_scheduler.cc
index df6ab81..03abdd6 100644
--- a/aos/events/event_scheduler.cc
+++ b/aos/events/event_scheduler.cc
@@ -101,9 +101,9 @@
void EventScheduler::RunOnRun() {
CHECK(is_running_);
while (!on_run_.empty()) {
- std::function<void()> fn = std::move(*on_run_.begin());
+ Event *event = *on_run_.begin();
on_run_.erase(on_run_.begin());
- fn();
+ event->Handle();
}
}
diff --git a/aos/events/event_scheduler.h b/aos/events/event_scheduler.h
index 84201ed..68fcf28 100644
--- a/aos/events/event_scheduler.h
+++ b/aos/events/event_scheduler.h
@@ -114,8 +114,13 @@
// entered the running state. Callbacks are cleared after being called once.
// Will not get called until a node starts (a node does not start until its
// monotonic clock has reached at least monotonic_clock::epoch()).
- void ScheduleOnRun(std::function<void()> callback) {
- on_run_.emplace_back(std::move(callback));
+ void ScheduleOnRun(Event *callback) { on_run_.emplace_back(callback); }
+
+ // Removes an event from the OnRun list without running it.
+ void DeleteOnRun(Event *callback) {
+ auto it = std::find(on_run_.begin(), on_run_.end(), callback);
+ CHECK(it != on_run_.end());
+ on_run_.erase(it);
}
// Schedules a callback whenever the event scheduler starts, before we have
@@ -220,7 +225,7 @@
size_t boot_count_ = 0;
// List of functions to run (once) when running.
- std::vector<std::function<void()>> on_run_;
+ std::vector<Event *> on_run_;
std::vector<std::function<void()>> on_startup_;
// Multimap holding times to run functions. These are stored in order, and
diff --git a/aos/events/event_scheduler_test.cc b/aos/events/event_scheduler_test.cc
index d328552..45a0dd0 100644
--- a/aos/events/event_scheduler_test.cc
+++ b/aos/events/event_scheduler_test.cc
@@ -264,7 +264,7 @@
schedulers_.at(0).monotonic_now());
EXPECT_EQ(monotonic_clock::epoch(), schedulers_.at(1).monotonic_now());
});
- schedulers_.at(1).ScheduleOnRun([this, &observed_on_run_1]() {
+ FunctionEvent on_run_event([this, &observed_on_run_1]() {
observed_on_run_1 = true;
// Note that we do not *stop* execution on node zero just to get 1 started.
EXPECT_TRUE(schedulers_.at(0).is_running());
@@ -275,6 +275,7 @@
schedulers_.at(0).monotonic_now());
EXPECT_EQ(monotonic_clock::epoch(), schedulers_.at(1).monotonic_now());
});
+ schedulers_.at(1).ScheduleOnRun(&on_run_event);
FunctionEvent e([]() {});
schedulers_.at(0).Schedule(monotonic_clock::epoch() + chrono::seconds(1), &e);
@@ -303,8 +304,9 @@
});
schedulers_.at(1).ScheduleOnStartup(
[]() { FAIL() << "Should never have hit startup handlers for node 1."; });
- schedulers_.at(1).ScheduleOnRun(
+ FunctionEvent fail(
[]() { FAIL() << "Should never have hit OnRun handlers for node 1."; });
+ schedulers_.at(1).ScheduleOnRun(&fail);
schedulers_.at(1).set_stopped(
[]() { FAIL() << "Should never have hit stopped handlers for node 1."; });
@@ -363,25 +365,25 @@
++startup_counter_b;
};
- auto on_run_handler_a = [this, &on_run_counter_a]() {
+ auto on_run_handler_a = FunctionEvent([this, &on_run_counter_a]() {
EXPECT_TRUE(schedulers_.at(0).is_running());
++on_run_counter_a;
- };
+ });
- auto on_run_handler_b = [this, &on_run_counter_b]() {
+ auto on_run_handler_b = FunctionEvent([this, &on_run_counter_b]() {
EXPECT_TRUE(schedulers_.at(0).is_running());
++on_run_counter_b;
- };
+ });
schedulers_.at(0).set_stopped([this, &stopped_counter]() {
EXPECT_FALSE(schedulers_.at(0).is_running());
++stopped_counter;
});
schedulers_.at(0).set_on_shutdown(
- [this, &shutdown_counter, startup_handler_a, on_run_handler_a]() {
+ [this, &shutdown_counter, startup_handler_a, &on_run_handler_a]() {
EXPECT_FALSE(schedulers_.at(0).is_running());
schedulers_.at(0).ScheduleOnStartup(startup_handler_a);
- schedulers_.at(0).ScheduleOnRun(on_run_handler_a);
+ schedulers_.at(0).ScheduleOnRun(&on_run_handler_a);
++shutdown_counter;
});
schedulers_.at(0).ScheduleOnStartup(startup_handler_a);
@@ -389,7 +391,7 @@
EXPECT_FALSE(schedulers_.at(0).is_running());
++started_counter;
});
- schedulers_.at(0).ScheduleOnRun(on_run_handler_a);
+ schedulers_.at(0).ScheduleOnRun(&on_run_handler_a);
FunctionEvent e([]() {});
schedulers_.at(0).Schedule(monotonic_clock::epoch() + chrono::seconds(1), &e);
@@ -405,12 +407,12 @@
// In the middle, execute a TemporarilyStopAndRun. Use it to re-register the
// startup handlers.
schedulers_.at(0).ScheduleOnStartup(startup_handler_b);
- schedulers_.at(0).ScheduleOnRun(on_run_handler_b);
- FunctionEvent stop_and_run([this, startup_handler_a, on_run_handler_a]() {
+ schedulers_.at(0).ScheduleOnRun(&on_run_handler_b);
+ FunctionEvent stop_and_run([this, startup_handler_a, &on_run_handler_a]() {
scheduler_scheduler_.TemporarilyStopAndRun(
- [this, startup_handler_a, on_run_handler_a]() {
+ [this, startup_handler_a, &on_run_handler_a]() {
schedulers_.at(0).ScheduleOnStartup(startup_handler_a);
- schedulers_.at(0).ScheduleOnRun(on_run_handler_a);
+ schedulers_.at(0).ScheduleOnRun(&on_run_handler_a);
});
});
schedulers_.at(1).Schedule(monotonic_clock::epoch() + chrono::seconds(2),
@@ -496,7 +498,8 @@
BootTimestamp{0, monotonic_clock::epoch() - chrono::seconds(10)}});
int counter = 0;
- schedulers_[1].ScheduleOnRun([]() { FAIL(); });
+ FunctionEvent fail_event([]() { FAIL(); });
+ schedulers_[1].ScheduleOnRun(&fail_event);
schedulers_[1].ScheduleOnStartup([]() { FAIL(); });
schedulers_[1].set_on_shutdown([]() { FAIL(); });
schedulers_[1].set_started([]() { FAIL(); });
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index 1b05841..d698b7d 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -605,9 +605,17 @@
});
event_loops_->push_back(this);
+ scheduler_->ScheduleOnRun(&on_run_event_);
+ on_run_scheduled_ = true;
}
~SimulatedEventLoop() override {
+ // Unregister any on_run callbacks to handle cleanup before they run
+ // correctly.
+ if (on_run_scheduled_) {
+ scheduler_->DeleteOnRun(&on_run_event_);
+ }
+
// Trigger any remaining senders or fetchers to be cleared before destroying
// the event loop so the book keeping matches.
timing_report_sender_.reset();
@@ -683,18 +691,41 @@
scheduler_, this, callback, interval, offset)));
}
+ class OnRunEvent : public EventScheduler::Event {
+ public:
+ OnRunEvent(SimulatedEventLoop *loop) : loop_(loop) {}
+
+ virtual void Handle() noexcept { loop_->DoOnRun(); }
+
+ private:
+ SimulatedEventLoop *loop_;
+ };
+
void OnRun(::std::function<void()> on_run) override {
CHECK(!is_running()) << ": Cannot register OnRun callback while running.";
- scheduler_->ScheduleOnRun([this, on_run = std::move(on_run)]() {
+ CHECK(on_run_scheduled_)
+ << "Registering OnRun callback after running on " << name();
+ on_run_.emplace_back(std::move(on_run));
+ }
+
+ // Called by OnRunEvent when we need to process OnRun callbacks.
+ void DoOnRun() {
+ VLOG(1) << distributed_now() << " " << NodeName(node()) << monotonic_now()
+ << " " << name() << " OnRun()";
+ on_run_scheduled_ = false;
+ while (!on_run_.empty()) {
+ std::function<void()> fn = std::move(*on_run_.begin());
+ on_run_.erase(on_run_.begin());
+
logging::ScopedLogRestorer prev_logger;
if (log_impl_) {
prev_logger.Swap(log_impl_);
}
ScopedMarkRealtimeRestorer rt(runtime_realtime_priority() > 0);
SetTimerContext(monotonic_now());
- on_run();
+ fn();
ClearContext();
- });
+ }
}
const Node *node() const override { return node_; }
@@ -787,6 +818,15 @@
std::shared_ptr<StartupTracker> startup_tracker_;
EventLoopOptions options_;
+
+ // Event used by EventScheduler to run OnRun callbacks.
+ OnRunEvent on_run_event_{this};
+
+ // True if the on run callbacks have been scheduled in the scheduler, and
+ // false if they have been executed.
+ bool on_run_scheduled_;
+
+ std::vector<std::function<void()>> on_run_;
};
void SimulatedEventLoopFactory::set_send_delay(
@@ -1308,8 +1348,9 @@
void SimulatedPhasedLoopHandler::HandleEvent() noexcept {
monotonic_clock::time_point monotonic_now =
simulated_event_loop_->monotonic_now();
- VLOG(1) << monotonic_now << " Phased loop " << simulated_event_loop_->name()
- << ", " << name();
+ VLOG(1) << simulated_event_loop_->scheduler_->distributed_now() << " "
+ << NodeName(simulated_event_loop_->node()) << monotonic_now << " "
+ << simulated_event_loop_->name() << " Phased loop '" << name() << "'";
logging::ScopedLogRestorer prev_logger;
if (simulated_event_loop_->log_impl_) {
prev_logger.Swap(simulated_event_loop_->log_impl_);
diff --git a/aos/events/simulated_event_loop_test.cc b/aos/events/simulated_event_loop_test.cc
index 745a273..429d39c 100644
--- a/aos/events/simulated_event_loop_test.cc
+++ b/aos/events/simulated_event_loop_test.cc
@@ -181,6 +181,66 @@
EXPECT_EQ(test_message_counter2.count(), 0u);
}
+// Test that OnRun callbacks get deleted if the event loop gets deleted.
+TEST(SimulatedEventLoopTest, DestructEventLoopBeforeOnRun) {
+ SimulatedEventLoopTestFactory factory;
+
+ SimulatedEventLoopFactory simulated_event_loop_factory(
+ factory.configuration());
+
+ {
+ ::std::unique_ptr<EventLoop> test_event_loop =
+ simulated_event_loop_factory.MakeEventLoop("test");
+ test_event_loop->OnRun([]() { LOG(FATAL) << "Don't run this"; });
+ }
+
+ simulated_event_loop_factory.RunFor(chrono::seconds(1));
+}
+
+// Tests that the order event loops are created is the order that the OnRun
+// callbacks are run.
+TEST(SimulatedEventLoopTest, OnRunOrderFollowsConstructionOrder) {
+ SimulatedEventLoopTestFactory factory;
+
+ SimulatedEventLoopFactory simulated_event_loop_factory(
+ factory.configuration());
+
+ int count = 0;
+
+ std::unique_ptr<EventLoop> test1_event_loop =
+ simulated_event_loop_factory.MakeEventLoop("test1");
+ std::unique_ptr<EventLoop> test2_event_loop =
+ simulated_event_loop_factory.MakeEventLoop("test2");
+ test2_event_loop->OnRun([&count]() {
+ EXPECT_EQ(count, 1u);
+ ++count;
+ });
+ test1_event_loop->OnRun([&count]() {
+ EXPECT_EQ(count, 0u);
+ ++count;
+ });
+
+ simulated_event_loop_factory.RunFor(chrono::seconds(1));
+
+ EXPECT_EQ(count, 2u);
+}
+
+// Test that we can't register OnRun callbacks after starting.
+TEST(SimulatedEventLoopDeathTest, OnRunAfterRunning) {
+ SimulatedEventLoopTestFactory factory;
+
+ SimulatedEventLoopFactory simulated_event_loop_factory(
+ factory.configuration());
+
+ std::unique_ptr<EventLoop> test_event_loop =
+ simulated_event_loop_factory.MakeEventLoop("test");
+ test_event_loop->OnRun([]() {});
+
+ simulated_event_loop_factory.RunFor(chrono::seconds(1));
+
+ EXPECT_DEATH(test_event_loop->OnRun([]() {}), "OnRun");
+}
+
// Test that if we configure an event loop to be able to send too fast that we
// do allow it to do so.
TEST(SimulatedEventLoopTest, AllowSendTooFast) {
diff --git a/frc971/imu_fdcan/dual_imu_blender_lib_test.cc b/frc971/imu_fdcan/dual_imu_blender_lib_test.cc
index 4237bd1..59c7e81 100644
--- a/frc971/imu_fdcan/dual_imu_blender_lib_test.cc
+++ b/frc971/imu_fdcan/dual_imu_blender_lib_test.cc
@@ -158,33 +158,10 @@
dual_imu_builder.CheckOk(dual_imu_builder.Send());
});
- event_loop_factory_.RunFor(std::chrono::milliseconds(200));
-
- ASSERT_TRUE(imu_values_batch_fetcher_.Fetch());
- ASSERT_TRUE(dual_imu_blender_status_fetcher_.Fetch());
-
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_x(), 0.71,
- 0.0001);
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_y(), 0.79,
- 0.0001);
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_z(), 0.78,
- 0.0001);
-
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_x(),
- 1.3, 0.0001);
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_y(),
- 1.1, 0.0001);
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_z(),
- 1.1, 0.0001);
-
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->temperature(), 20,
- 0.0001);
-
- CheckImuType(frc971::imu::ImuType::MURATA);
-
- // Make sure we switch to TDK on saturation
+ bool enable_tdk_sender = false;
dual_imu_blender_event_loop_->AddPhasedLoop(
- [this](int) {
+ [this, &enable_tdk_sender](int) {
+ if (!enable_tdk_sender) return;
aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder
dual_imu_builder = dual_imu_sender_.MakeStaticBuilder();
@@ -221,33 +198,11 @@
},
std::chrono::milliseconds(1));
- event_loop_factory_.RunFor(std::chrono::milliseconds(200));
-
- ASSERT_TRUE(imu_values_batch_fetcher_.Fetch());
- ASSERT_TRUE(dual_imu_blender_status_fetcher_.Fetch());
-
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_x(), 6.0,
- 0.0001);
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_y(), 6.0,
- 0.0001);
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_z(), 6.0,
- 0.0001);
-
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_x(),
- 6.2, 0.0001);
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_y(),
- 6.3, 0.0001);
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_z(),
- 6.5, 0.0001);
-
- EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->temperature(), 20,
- 0.0001);
-
- CheckImuType(frc971::imu::ImuType::TDK);
-
- // Check negative values as well
+ bool send_negative = false;
dual_imu_blender_event_loop_->AddPhasedLoop(
- [this](int) {
+ [this, &send_negative](int) {
+ if (!send_negative) return;
+
aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder
dual_imu_builder = dual_imu_sender_.MakeStaticBuilder();
@@ -289,6 +244,61 @@
ASSERT_TRUE(imu_values_batch_fetcher_.Fetch());
ASSERT_TRUE(dual_imu_blender_status_fetcher_.Fetch());
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_x(), 0.71,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_y(), 0.79,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_z(), 0.78,
+ 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_x(),
+ 1.3, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_y(),
+ 1.1, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_z(),
+ 1.1, 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->temperature(), 20,
+ 0.0001);
+
+ CheckImuType(frc971::imu::ImuType::MURATA);
+
+ // Make sure we switch to TDK on saturation
+ enable_tdk_sender = true;
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(200));
+
+ ASSERT_TRUE(imu_values_batch_fetcher_.Fetch());
+ ASSERT_TRUE(dual_imu_blender_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_x(), 6.0,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_y(), 6.0,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_z(), 6.0,
+ 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_x(),
+ 6.2, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_y(),
+ 6.3, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_z(),
+ 6.5, 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->temperature(), 20,
+ 0.0001);
+
+ CheckImuType(frc971::imu::ImuType::TDK);
+
+ // Check negative values as well
+ enable_tdk_sender = false;
+ send_negative = true;
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(200));
+
+ ASSERT_TRUE(imu_values_batch_fetcher_.Fetch());
+ ASSERT_TRUE(dual_imu_blender_status_fetcher_.Fetch());
+
EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_x(), -6.0,
0.0001);
EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_y(), -6.0,
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index ae92f97..ca25137 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -113,6 +113,16 @@
BootTimestamp::epoch() + kPiTimeOffset, BootTimestamp::epoch()}}),
roborio_(aos::configuration::GetNode(configuration(), "roborio")),
pi1_(aos::configuration::GetNode(configuration(), "pi1")),
+ drivetrain_event_loop_(MakeEventLoop("drivetrain", roborio_)),
+ dt_config_(GetTest2020DrivetrainConfig()),
+ pi1_event_loop_(MakeEventLoop("test", pi1_)),
+ camera_sender_(
+ pi1_event_loop_->MakeSender<ImageMatchResult>("/pi1/camera")),
+ localizer_(drivetrain_event_loop_.get(), dt_config_),
+ drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
+ drivetrain_plant_event_loop_(MakeEventLoop("plant", roborio_)),
+ drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_),
+ last_frame_(monotonic_now()),
test_event_loop_(MakeEventLoop("test", roborio_)),
drivetrain_goal_sender_(
test_event_loop_->MakeSender<Goal>("/drivetrain")),
@@ -129,17 +139,7 @@
"/superstructure")),
server_statistics_sender_(
test_event_loop_->MakeSender<aos::message_bridge::ServerStatistics>(
- "/aos")),
- drivetrain_event_loop_(MakeEventLoop("drivetrain", roborio_)),
- dt_config_(GetTest2020DrivetrainConfig()),
- pi1_event_loop_(MakeEventLoop("test", pi1_)),
- camera_sender_(
- pi1_event_loop_->MakeSender<ImageMatchResult>("/pi1/camera")),
- localizer_(drivetrain_event_loop_.get(), dt_config_),
- drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
- drivetrain_plant_event_loop_(MakeEventLoop("plant", roborio_)),
- drivetrain_plant_(drivetrain_plant_event_loop_.get(), dt_config_),
- last_frame_(monotonic_now()) {
+ "/aos")) {
CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), roborio_), 6);
CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), pi1_), 1);
set_team_id(frc971::control_loops::testing::kTeamNumber);
@@ -348,15 +348,6 @@
const aos::Node *const roborio_;
const aos::Node *const pi1_;
- std::unique_ptr<aos::EventLoop> test_event_loop_;
- aos::Sender<Goal> drivetrain_goal_sender_;
- aos::Fetcher<Goal> drivetrain_goal_fetcher_;
- aos::Fetcher<frc971::control_loops::drivetrain::Status>
- drivetrain_status_fetcher_;
- aos::Sender<LocalizerControl> localizer_control_sender_;
- aos::Sender<superstructure::Status> superstructure_status_sender_;
- aos::Sender<aos::message_bridge::ServerStatistics> server_statistics_sender_;
-
std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
@@ -370,6 +361,15 @@
DrivetrainSimulation drivetrain_plant_;
monotonic_clock::time_point last_frame_;
+ std::unique_ptr<aos::EventLoop> test_event_loop_;
+ aos::Sender<Goal> drivetrain_goal_sender_;
+ aos::Fetcher<Goal> drivetrain_goal_fetcher_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+ aos::Sender<LocalizerControl> localizer_control_sender_;
+ aos::Sender<superstructure::Status> superstructure_status_sender_;
+ aos::Sender<aos::message_bridge::ServerStatistics> server_statistics_sender_;
+
// A queue of camera frames so that we can add a time delay to the data
// coming from the cameras.
std::queue<std::tuple<aos::monotonic_clock::time_point,
@@ -595,13 +595,14 @@
// Tests that we don't blow up if we stop getting updates for an extended period
// of time and fall behind on fetching fron the cameras.
TEST_F(LocalizedDrivetrainTest, FetchersHandleTimeGap) {
+ std::unique_ptr<aos::EventLoop> test = MakeEventLoop("test", roborio_);
+
set_enable_cameras(false);
set_send_delay(std::chrono::seconds(0));
event_loop_factory()->set_network_delay(std::chrono::nanoseconds(1));
- test_event_loop_
- ->AddTimer([this]() { drivetrain_plant_.set_send_messages(false); })
- ->Schedule(test_event_loop_->monotonic_now());
- test_event_loop_->AddPhasedLoop(
+ test->AddTimer([this]() { drivetrain_plant_.set_send_messages(false); })
+ ->Schedule(test->monotonic_now());
+ test->AddPhasedLoop(
[this](int) {
auto builder = camera_sender_.MakeBuilder();
ImageMatchResultT image;
@@ -609,12 +610,11 @@
aos::RawSender::Error::kOk);
},
std::chrono::milliseconds(40));
- test_event_loop_
- ->AddTimer([this]() {
+ test->AddTimer([this]() {
drivetrain_plant_.set_send_messages(true);
SimulateSensorReset();
})
- ->Schedule(test_event_loop_->monotonic_now() + std::chrono::seconds(10));
+ ->Schedule(test->monotonic_now() + std::chrono::seconds(10));
RunFor(chrono::seconds(20));
}
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 950593e..fad888c 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -554,9 +554,10 @@
expected_voltage);
}
- void StartSendingFinisherGoals() {
+ void RegisterFinisherGoalsLoop() {
test_event_loop_->AddPhasedLoop(
[this](int) {
+ if (!send_finisher_goals_) return;
auto builder = superstructure_goal_sender_.MakeBuilder();
auto shooter_goal_builder = builder.MakeBuilder<ShooterGoal>();
shooter_goal_builder.add_velocity_finisher(finisher_goal_);
@@ -569,6 +570,8 @@
dt());
}
+ void StartSendingFinisherGoals() { send_finisher_goals_ = true; }
+
// Sets the finisher velocity goal (radians/s)
void SetFinisherGoalAfter(const double velocity_finisher,
const monotonic_clock::duration time_offset) {
@@ -619,6 +622,8 @@
double finisher_goal_ = 0;
bool ball_in_finisher_ = false;
+
+ bool send_finisher_goals_ = false;
};
// Tests that the superstructure does nothing when the goal is to remain
@@ -1032,18 +1037,18 @@
// Tests that we detect that a ball was shot whenever the average angular
// velocity is lower than a certain threshold compared to the goal.
TEST_F(SuperstructureTest, BallsShot) {
- SetEnabled(true);
- WaitUntilZeroed();
+ bool run_test = false;
int balls_shot = 0;
// When there is a ball in the flywheel, the finisher velocity should drop
// below the goal
bool finisher_velocity_below_goal = false;
- StartSendingFinisherGoals();
-
test_event_loop_->AddPhasedLoop(
[&](int) {
+ if (!run_test) {
+ return;
+ }
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
const double finisher_velocity =
superstructure_status_fetcher_->shooter()
@@ -1078,6 +1083,13 @@
}
},
dt());
+ RegisterFinisherGoalsLoop();
+
+ SetEnabled(true);
+
+ WaitUntilZeroed();
+
+ StartSendingFinisherGoals();
constexpr int kFastShootingSpeed = 500;
constexpr int kSlowShootingSpeed = 300;
diff --git a/y2023/vision/april_detection_test.cc b/y2023/vision/april_detection_test.cc
index fea5105..71e34cb 100644
--- a/y2023/vision/april_detection_test.cc
+++ b/y2023/vision/april_detection_test.cc
@@ -35,7 +35,9 @@
constants_sender_(receive_pose_event_loop_.get(),
"y2023/constants/constants.json", 7971, "/constants"),
detector_(
- AprilRoboticsDetector(send_pose_event_loop_.get(), "/camera")) {}
+ AprilRoboticsDetector(send_pose_event_loop_.get(), "/camera")) {
+ event_loop_factory_.RunFor(std::chrono::milliseconds(5));
+ }
void SendImage(std::string path) {
aos::FlatbufferVector<frc971::vision::CameraImage> image =
@@ -51,7 +53,7 @@
void TestDistanceAngle(std::string image_path, double expected_distance,
double expected_angle) {
- receive_pose_event_loop_->OnRun([&]() { SendImage(image_path); });
+ SendImage(image_path);
event_loop_factory_.RunFor(std::chrono::milliseconds(5));
ASSERT_TRUE(april_pose_fetcher_.Fetch());