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/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;