Convert control loop tests over to simulated event loop
This makes it so that we properly only use ShmEventLoop for running in
realtime on a robot. Very nice.
Change-Id: I46b770b336f59e08cfaf28511b3bd5689f72fff1
diff --git a/aos/controls/BUILD b/aos/controls/BUILD
index 30fe5c6..a612ecb 100644
--- a/aos/controls/BUILD
+++ b/aos/controls/BUILD
@@ -27,7 +27,7 @@
"control_loop_test.h",
],
deps = [
- "//aos/events:shm-event-loop",
+ "//aos/events:simulated_event_loop",
"//aos/logging:queue_logging",
"//aos/robot_state",
"//aos/testing:googletest",
diff --git a/aos/controls/control_loop-tmpl.h b/aos/controls/control_loop-tmpl.h
index 2b57a0b..4f48737 100644
--- a/aos/controls/control_loop-tmpl.h
+++ b/aos/controls/control_loop-tmpl.h
@@ -23,26 +23,7 @@
}
template <class T>
-void ControlLoop<T>::Iterate() {
- if (!has_iterate_fetcher_) {
- iterate_position_fetcher_ =
- event_loop_->MakeFetcher<PositionType>(name_ + ".position");
- has_iterate_fetcher_ = true;
- }
- const bool did_fetch = iterate_position_fetcher_.Fetch();
- if (!did_fetch) {
- LOG(FATAL, "Failed to fetch from position queue\n");
- }
- IteratePosition(*iterate_position_fetcher_);
-}
-
-template <class T>
void ControlLoop<T>::IteratePosition(const PositionType &position) {
- // Since Exit() isn't async safe, we want to call Exit from the periodic
- // handler.
- if (!run_) {
- event_loop_->Exit();
- }
no_goal_.Print();
no_sensor_state_.Print();
motors_off_log_.Print();
@@ -119,28 +100,5 @@
status.Send();
}
-template <class T>
-void ControlLoop<T>::Run() {
- struct sigaction action;
- action.sa_handler = &ControlLoop<T>::Quit;
- sigemptyset(&action.sa_mask);
- action.sa_flags = SA_RESETHAND;
-
- PCHECK(sigaction(SIGTERM, &action, nullptr));
- PCHECK(sigaction(SIGQUIT, &action, nullptr));
- PCHECK(sigaction(SIGINT, &action, nullptr));
-
- event_loop_->MakeWatcher(name_ + ".position",
- [this](const PositionType &position) {
- this->IteratePosition(position);
- });
-
- event_loop_->Run();
- LOG(INFO, "Shutting down\n");
-}
-
-template <class T>
-::std::atomic<bool> ControlLoop<T>::run_{true};
-
} // namespace controls
} // namespace aos
diff --git a/aos/controls/control_loop.h b/aos/controls/control_loop.h
index 56353c1..fe72022 100644
--- a/aos/controls/control_loop.h
+++ b/aos/controls/control_loop.h
@@ -15,16 +15,6 @@
namespace aos {
namespace controls {
-// Interface to describe runnable jobs.
-class Runnable {
- public:
- virtual ~Runnable() {}
- // Runs forever.
- virtual void Run() = 0;
- // Does one quick piece of work and return. Does _not_ block.
- virtual void Iterate() = 0;
-};
-
// Control loops run this often, "starting" at time 0.
constexpr ::std::chrono::nanoseconds kLoopFrequency =
::std::chrono::milliseconds(5);
@@ -35,7 +25,7 @@
// It will then call the RunIteration method every cycle that it has enough
// valid data for the control loop to run.
template <class T>
-class ControlLoop : public Runnable {
+class ControlLoop {
public:
// Create some convenient typedefs to reference the Goal, Position, Status,
// and Output structures.
@@ -58,6 +48,11 @@
event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state");
joystick_state_fetcher_ =
event_loop_->MakeFetcher<::aos::JoystickState>(".aos.joystick_state");
+
+ event_loop_->MakeWatcher(name_ + ".position",
+ [this](const PositionType &position) {
+ this->IteratePosition(position);
+ });
}
const ::aos::RobotState &robot_state() const { return *robot_state_fetcher_; }
@@ -87,22 +82,12 @@
// subsystem.
virtual void Zero(OutputType *output) { output->Zero(); }
- // Runs the loop forever.
- // TODO(austin): This should move to the event loop once it gets hoisted out.
- void Run() override;
-
- // Runs one cycle of the loop.
- // TODO(austin): This should go away when all the tests use event loops
- // directly.
- void Iterate() override;
-
protected:
+ // Runs one cycle of the loop.
void IteratePosition(const PositionType &position);
EventLoop *event_loop() { return event_loop_; }
- static void Quit(int /*signum*/) { run_ = false; }
-
// Runs an iteration of the control loop.
// goal is the last goal that was sent. It might be any number of cycles old
// or nullptr if we haven't ever received a goal.
@@ -126,7 +111,6 @@
::std::chrono::milliseconds(100);
// Pointer to the queue group
- ::std::unique_ptr<ShmEventLoop> shm_event_loop_;
EventLoop *event_loop_;
::std::string name_;
@@ -154,8 +138,6 @@
SimpleLogInterval(kStaleLogInterval, WARNING, "motors disabled");
SimpleLogInterval no_goal_ =
SimpleLogInterval(kStaleLogInterval, ERROR, "no goal");
-
- static ::std::atomic<bool> run_;
};
} // namespace controls
diff --git a/aos/controls/control_loop_test.h b/aos/controls/control_loop_test.h
index 95f1c7d..85d610e 100644
--- a/aos/controls/control_loop_test.h
+++ b/aos/controls/control_loop_test.h
@@ -5,7 +5,7 @@
#include "gtest/gtest.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/simulated-event-loop.h"
#include "aos/logging/queue_logging.h"
#include "aos/robot_state/robot_state.q.h"
#include "aos/testing/test_shm.h"
@@ -22,68 +22,43 @@
template <typename TestBaseClass>
class ControlLoopTestTemplated : public TestBaseClass {
public:
- ControlLoopTestTemplated() {
+ ControlLoopTestTemplated(::std::chrono::nanoseconds dt = kTimeTick)
+ : dt_(dt), robot_status_event_loop_(MakeEventLoop()) {
robot_state_sender_ =
- test_event_loop_.MakeSender<::aos::RobotState>(".aos.robot_state");
+ robot_status_event_loop_->MakeSender<::aos::RobotState>(
+ ".aos.robot_state");
joystick_state_sender_ =
- test_event_loop_.MakeSender<::aos::JoystickState>(".aos.joystick_state");
+ robot_status_event_loop_->MakeSender<::aos::JoystickState>(
+ ".aos.joystick_state");
- ::aos::time::EnableMockTime(current_time_);
+ // Schedule the robot status send 1 nanosecond before the loop runs.
+ send_robot_state_phased_loop_ = robot_status_event_loop_->AddPhasedLoop(
+ [this](int) { SendRobotState(); }, dt_,
+ dt - ::std::chrono::nanoseconds(1));
- SendMessages(false);
+ send_joystick_state_timer_ =
+ robot_status_event_loop_->AddTimer([this]() { SendJoystickState(); });
+
+ robot_status_event_loop_->OnRun([this]() {
+ send_joystick_state_timer_->Setup(
+ robot_status_event_loop_->monotonic_now(), dt_);
+ });
}
- virtual ~ControlLoopTestTemplated() {
- ::aos::time::DisableMockTime();
- }
+ virtual ~ControlLoopTestTemplated() {}
void set_team_id(uint16_t team_id) { team_id_ = team_id; }
uint16_t team_id() const { return team_id_; }
- // Sends out all of the required queue messages.
- void SendMessages(bool enabled) {
- if (current_time_ >= kDSPacketTime + last_ds_time_ ||
- last_enabled_ != enabled) {
- last_ds_time_ = current_time_;
- auto new_state = joystick_state_sender_.MakeMessage();
- new_state->fake = true;
-
- new_state->enabled = enabled;
- new_state->autonomous = false;
- new_state->team_id = team_id_;
-
- new_state.Send();
- last_enabled_ = enabled;
+ // Sets the enabled/disabled bit and (potentially) rebroadcasts the robot
+ // state messages.
+ void SetEnabled(bool enabled) {
+ if (enabled_ != enabled) {
+ enabled_ = enabled;
+ SendJoystickState();
+ SendRobotState();
+ send_joystick_state_timer_->Setup(
+ robot_status_event_loop_->monotonic_now(), dt_);
}
-
- {
- auto new_state = robot_state_sender_.MakeMessage();
-
- new_state->reader_pid = reader_pid_;
- new_state->outputs_enabled = enabled;
- new_state->browned_out = false;
-
- new_state->is_3v3_active = true;
- new_state->is_5v_active = true;
- new_state->voltage_3v3 = 3.3;
- new_state->voltage_5v = 5.0;
-
- new_state->voltage_roborio_in = battery_voltage_;
- new_state->voltage_battery = battery_voltage_;
-
- LOG_STRUCT(INFO, "robot_state", *new_state);
- new_state.Send();
- }
- }
- // Ticks time for a single control loop cycle.
- void TickTime(::std::chrono::nanoseconds dt = kTimeTick) {
- ::aos::time::SetMockTime(current_time_ += dt);
- }
-
- // Simulates everything that happens during 1 loop time step.
- void SimulateTimestep(bool enabled,
- ::std::chrono::nanoseconds dt = kTimeTick) {
- SendMessages(enabled);
- TickTime(dt);
}
// Simulate a reset of the process reading sensors, which tells loops that all
@@ -97,33 +72,91 @@
battery_voltage_ = battery_voltage;
}
+ ::std::unique_ptr<::aos::EventLoop> MakeEventLoop() {
+ return event_loop_factory_.MakeEventLoop();
+ }
+
+ void RunFor(monotonic_clock::duration duration) {
+ event_loop_factory_.RunFor(duration);
+ }
+
+ ::aos::monotonic_clock::time_point monotonic_now() {
+ return event_loop_factory_.monotonic_now();
+ }
+
+ ::std::chrono::nanoseconds dt() const { return dt_; }
+
private:
- static constexpr ::std::chrono::milliseconds kTimeTick{5};
+ // Sends out all of the required queue messages.
+ void SendJoystickState() {
+ if (monotonic_now() >= kDSPacketTime + last_ds_time_ ||
+ last_enabled_ != enabled_) {
+ auto new_state = joystick_state_sender_.MakeMessage();
+ new_state->fake = true;
+
+ new_state->enabled = enabled_;
+ new_state->autonomous = false;
+ new_state->team_id = team_id_;
+
+ LOG_STRUCT(INFO, "joystick_state", *new_state);
+ new_state.Send();
+
+ last_ds_time_ = monotonic_now();
+ last_enabled_ = enabled_;
+ }
+ }
+
+ bool last_enabled_ = false;
+
+ void SendRobotState() {
+ auto new_state = robot_state_sender_.MakeMessage();
+
+ new_state->reader_pid = reader_pid_;
+ new_state->outputs_enabled = enabled_;
+ new_state->browned_out = false;
+
+ new_state->is_3v3_active = true;
+ new_state->is_5v_active = true;
+ new_state->voltage_3v3 = 3.3;
+ new_state->voltage_5v = 5.0;
+
+ new_state->voltage_roborio_in = battery_voltage_;
+ new_state->voltage_battery = battery_voltage_;
+
+ LOG_STRUCT(INFO, "robot_state", *new_state);
+ new_state.Send();
+ }
+
+ static constexpr ::std::chrono::microseconds kTimeTick{5000};
static constexpr ::std::chrono::milliseconds kDSPacketTime{20};
+ const ::std::chrono::nanoseconds dt_;
+
+ SimulatedEventLoopFactory event_loop_factory_;
+
uint16_t team_id_ = 971;
int32_t reader_pid_ = 1;
double battery_voltage_ = 12.4;
::aos::monotonic_clock::time_point last_ds_time_ =
- ::aos::monotonic_clock::epoch();
- ::aos::monotonic_clock::time_point current_time_ =
- ::aos::monotonic_clock::epoch();
+ ::aos::monotonic_clock::min_time;
- ::aos::testing::TestSharedMemory my_shm_;
+ bool enabled_ = false;
- bool last_enabled_ = false;
-
- ::aos::ShmEventLoop test_event_loop_;
+ ::std::unique_ptr<::aos::EventLoop> robot_status_event_loop_;
::aos::Sender<::aos::RobotState> robot_state_sender_;
::aos::Sender<::aos::JoystickState> joystick_state_sender_;
+
+ ::aos::PhasedLoopHandler *send_robot_state_phased_loop_ = nullptr;
+ ::aos::TimerHandler *send_joystick_state_timer_ = nullptr;
};
typedef ControlLoopTestTemplated<::testing::Test> ControlLoopTest;
template <typename TestBaseClass>
-constexpr ::std::chrono::milliseconds ControlLoopTestTemplated<TestBaseClass>::kTimeTick;
+constexpr ::std::chrono::microseconds
+ ControlLoopTestTemplated<TestBaseClass>::kTimeTick;
template <typename TestBaseClass>
constexpr ::std::chrono::milliseconds ControlLoopTestTemplated<TestBaseClass>::kDSPacketTime;