Move Position, robot_state, and joystick_state over
ControlLoop should now be completely using the event loop.
Change-Id: Ifb936c384d1031abbc9e61d834b1ef157ad0841c
diff --git a/aos/controls/control_loop-tmpl.h b/aos/controls/control_loop-tmpl.h
index 6953575..49bad45 100644
--- a/aos/controls/control_loop-tmpl.h
+++ b/aos/controls/control_loop-tmpl.h
@@ -3,7 +3,6 @@
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
-#include "aos/robot_state/robot_state.q.h"
namespace aos {
namespace controls {
@@ -25,8 +24,16 @@
template <class T>
void ControlLoop<T>::Iterate() {
- control_loop_->position.FetchAnother();
- IteratePosition(*control_loop_->position.get());
+ 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>
@@ -52,35 +59,35 @@
LOG_INTERVAL(no_goal_);
}
- const bool new_robot_state = ::aos::robot_state.FetchLatest();
- if (!::aos::robot_state.get()) {
+ const bool new_robot_state = robot_state_fetcher_.Fetch();
+ if (!robot_state_fetcher_.get()) {
LOG_INTERVAL(no_sensor_state_);
return;
}
- if (sensor_reader_pid_ != ::aos::robot_state->reader_pid) {
+ if (sensor_reader_pid_ != robot_state_fetcher_->reader_pid) {
LOG(INFO, "new sensor reader PID %" PRId32 ", old was %" PRId32 "\n",
- ::aos::robot_state->reader_pid, sensor_reader_pid_);
+ robot_state_fetcher_->reader_pid, sensor_reader_pid_);
reset_ = true;
- sensor_reader_pid_ = ::aos::robot_state->reader_pid;
+ sensor_reader_pid_ = robot_state_fetcher_->reader_pid;
}
- bool outputs_enabled = ::aos::robot_state->outputs_enabled;
+ bool outputs_enabled = robot_state_fetcher_->outputs_enabled;
// Check to see if we got a driver station packet recently.
if (new_robot_state) {
- if (::aos::robot_state->outputs_enabled) {
+ if (robot_state_fetcher_->outputs_enabled) {
// If the driver's station reports being disabled, we're probably not
// actually going to send motor values regardless of what the FPGA
// reports.
- last_pwm_sent_ = ::aos::robot_state->sent_time;
+ last_pwm_sent_ = robot_state_fetcher_->sent_time;
}
}
const ::aos::monotonic_clock::time_point now = ::aos::monotonic_clock::now();
const bool motors_off = now >= kPwmDisableTime + last_pwm_sent_;
- ::aos::joystick_state.FetchLatest();
+ joystick_state_fetcher_.Fetch();
if (motors_off) {
- if (::aos::joystick_state.get() && ::aos::joystick_state->enabled) {
+ if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled) {
LOG_INTERVAL(motors_off_log_);
}
outputs_enabled = false;
@@ -122,7 +129,7 @@
PCHECK(sigaction(SIGQUIT, &action, nullptr));
PCHECK(sigaction(SIGINT, &action, nullptr));
- event_loop_->MakeWatcher(::std::string(control_loop_->name()) + ".position",
+ event_loop_->MakeWatcher(name_ + ".position",
[this](const PositionType &position) {
this->IteratePosition(position);
});
diff --git a/aos/controls/control_loop.h b/aos/controls/control_loop.h
index 0b6e391..1f05c33 100644
--- a/aos/controls/control_loop.h
+++ b/aos/controls/control_loop.h
@@ -7,6 +7,7 @@
#include "aos/events/event-loop.h"
#include "aos/events/shm-event-loop.h"
#include "aos/queue.h"
+#include "aos/robot_state/robot_state.q.h"
#include "aos/time/time.h"
#include "aos/type_traits/type_traits.h"
#include "aos/util/log_interval.h"
@@ -52,13 +53,22 @@
OutputType;
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");
+ : shm_event_loop_(new ::aos::ShmEventLoop()),
+ event_loop_(shm_event_loop_.get()),
+ name_(control_loop->name()) {
+ output_sender_ = event_loop_->MakeSender<OutputType>(name_ + ".output");
+ status_sender_ = event_loop_->MakeSender<StatusType>(name_ + ".status");
+ goal_fetcher_ = event_loop_->MakeFetcher<GoalType>(name_ + ".goal");
+ robot_state_fetcher_ =
+ event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state");
+ joystick_state_fetcher_ =
+ event_loop_->MakeFetcher<::aos::JoystickState>(".aos.joystick_state");
+ }
+
+ const ::aos::RobotState &robot_state() const { return *robot_state_fetcher_; }
+ bool has_joystick_state() const { return joystick_state_fetcher_.get(); }
+ const ::aos::JoystickState &joystick_state() const {
+ return *joystick_state_fetcher_;
}
// Returns true if all the counters etc in the sensor data have been reset.
@@ -120,12 +130,20 @@
::std::chrono::milliseconds(100);
// Pointer to the queue group
- ::std::unique_ptr<EventLoop> event_loop_;
- T *control_loop_;
+ ::std::unique_ptr<ShmEventLoop> shm_event_loop_;
+ EventLoop *event_loop_;
+ ::std::string name_;
::aos::Sender<OutputType> output_sender_;
::aos::Sender<StatusType> status_sender_;
::aos::Fetcher<GoalType> goal_fetcher_;
+ ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
+ ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
+
+ // Fetcher only to be used for the Iterate method. If Iterate is called, Run
+ // can't be called.
+ bool has_iterate_fetcher_ = false;
+ ::aos::Fetcher<PositionType> iterate_position_fetcher_;
bool reset_ = false;
int32_t sensor_reader_pid_ = 0;