Convert ControlLoop over to using EventLoop
Next step is to pass in the event loop and flip over to using it in
tests and at runtime.
Change-Id: Ia3b8f099e4eacc0ffa8fbfe00d651f3684a8c783
diff --git a/aos/controls/BUILD b/aos/controls/BUILD
index d597e19..8c44823 100644
--- a/aos/controls/BUILD
+++ b/aos/controls/BUILD
@@ -91,6 +91,8 @@
deps = [
":control_loop_queues",
"//aos:queues",
+ "//aos/events:event-loop",
+ "//aos/events:shm-event-loop",
"//aos/logging",
"//aos/logging:queue_logging",
"//aos/robot_state",
diff --git a/aos/controls/control_loop-tmpl.h b/aos/controls/control_loop-tmpl.h
index 1834728..6953575 100644
--- a/aos/controls/control_loop-tmpl.h
+++ b/aos/controls/control_loop-tmpl.h
@@ -17,26 +17,35 @@
template <class T>
void ControlLoop<T>::ZeroOutputs() {
- aos::ScopedMessagePtr<OutputType> output =
- control_loop_->output.MakeMessage();
+ typename ::aos::Sender<OutputType>::Message output =
+ output_sender_.MakeMessage();
Zero(output.get());
output.Send();
}
template <class T>
void ControlLoop<T>::Iterate() {
+ control_loop_->position.FetchAnother();
+ IteratePosition(*control_loop_->position.get());
+}
+
+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();
- control_loop_->position.FetchAnother();
- const PositionType *const position = control_loop_->position.get();
- LOG_STRUCT(DEBUG, "position", *position);
+ LOG_STRUCT(DEBUG, "position", position);
// Fetch the latest control loop goal. If there is no new
// goal, we will just reuse the old one.
- control_loop_->goal.FetchLatest();
- const GoalType *goal = control_loop_->goal.get();
+ goal_fetcher_.Fetch();
+ const GoalType *goal = goal_fetcher_.get();
if (goal) {
LOG_STRUCT(DEBUG, "goal", *goal);
} else {
@@ -77,23 +86,23 @@
outputs_enabled = false;
}
- aos::ScopedMessagePtr<StatusType> status =
- control_loop_->status.MakeMessage();
+ typename ::aos::Sender<StatusType>::Message status =
+ status_sender_.MakeMessage();
if (status.get() == nullptr) {
return;
}
if (outputs_enabled) {
- aos::ScopedMessagePtr<OutputType> output =
- control_loop_->output.MakeMessage();
- RunIteration(goal, position, output.get(), status.get());
+ typename ::aos::Sender<OutputType>::Message output =
+ output_sender_.MakeMessage();
+ RunIteration(goal, &position, output.get(), status.get());
output->SetTimeToNow();
LOG_STRUCT(DEBUG, "output", *output);
output.Send();
} else {
// The outputs are disabled, so pass nullptr in for the output.
- RunIteration(goal, position, nullptr, status.get());
+ RunIteration(goal, &position, nullptr, status.get());
ZeroOutputs();
}
@@ -113,9 +122,12 @@
PCHECK(sigaction(SIGQUIT, &action, nullptr));
PCHECK(sigaction(SIGINT, &action, nullptr));
- while (run_) {
- Iterate();
- }
+ event_loop_->MakeWatcher(::std::string(control_loop_->name()) + ".position",
+ [this](const PositionType &position) {
+ this->IteratePosition(position);
+ });
+
+ event_loop_->Run();
LOG(INFO, "Shutting down\n");
}
diff --git a/aos/controls/control_loop.h b/aos/controls/control_loop.h
index 335a1bb..0b6e391 100644
--- a/aos/controls/control_loop.h
+++ b/aos/controls/control_loop.h
@@ -4,6 +4,8 @@
#include <string.h>
#include <atomic>
+#include "aos/events/event-loop.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/queue.h"
#include "aos/time/time.h"
#include "aos/type_traits/type_traits.h"
@@ -49,7 +51,15 @@
decltype(*(static_cast<T *>(NULL)->output.MakeMessage().get()))>::type
OutputType;
- ControlLoop(T *control_loop) : control_loop_(control_loop) {}
+ ControlLoop(T *control_loop)
+ : event_loop_(new ::aos::ShmEventLoop()), control_loop_(control_loop) {
+ output_sender_ = event_loop_->MakeSender<OutputType>(
+ ::std::string(control_loop_->name()) + ".output");
+ status_sender_ = event_loop_->MakeSender<StatusType>(
+ ::std::string(control_loop_->name()) + ".status");
+ goal_fetcher_ = event_loop_->MakeFetcher<GoalType>(
+ ::std::string(control_loop_->name()) + ".goal");
+ }
// Returns true if all the counters etc in the sensor data have been reset.
// This will return true only a single time per reset.
@@ -79,6 +89,8 @@
void Iterate() override;
protected:
+ void IteratePosition(const PositionType &position);
+
static void Quit(int /*signum*/) {
run_ = false;
}
@@ -108,8 +120,13 @@
::std::chrono::milliseconds(100);
// Pointer to the queue group
+ ::std::unique_ptr<EventLoop> event_loop_;
T *control_loop_;
+ ::aos::Sender<OutputType> output_sender_;
+ ::aos::Sender<StatusType> status_sender_;
+ ::aos::Fetcher<GoalType> goal_fetcher_;
+
bool reset_ = false;
int32_t sensor_reader_pid_ = 0;