Convert aos over to flatbuffers
Everything builds, and all the tests pass. I suspect that some entries
are missing from the config files, but those will be found pretty
quickly on startup.
There is no logging or live introspection of queue messages.
Change-Id: I496ee01ed68f202c7851bed7e8786cee30df29f5
diff --git a/aos/input/BUILD b/aos/input/BUILD
index 0da8d01..b4ceea6 100644
--- a/aos/input/BUILD
+++ b/aos/input/BUILD
@@ -9,11 +9,10 @@
"joystick_input.h",
],
deps = [
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//aos/input:driver_station_data",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
],
)
@@ -29,10 +28,11 @@
"//aos:math",
"//aos/input:driver_station_data",
"//aos/logging",
- "//aos/logging:queue_logging",
- "//aos/robot_state",
+ "//aos/robot_state:robot_state_fbs",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
],
)
@@ -45,7 +45,8 @@
"driver_station_data.h",
],
deps = [
- "//aos/robot_state",
+ "//aos/robot_state:joystick_state_fbs",
+ "@com_github_google_glog//:glog",
],
)
@@ -54,12 +55,12 @@
srcs = ["action_joystick_input.cc"],
hdrs = ["action_joystick_input.h"],
deps = [
+ ":drivetrain_input",
"//aos:init",
"//aos/actions:action_lib",
- "//aos/input:drivetrain_input",
"//aos/input:joystick_input",
"//aos/logging",
- "//frc971/autonomous:auto_queue",
+ "//frc971/autonomous:auto_fbs",
"//frc971/autonomous:base_autonomous_actor",
],
)
diff --git a/aos/input/action_joystick_input.cc b/aos/input/action_joystick_input.cc
index 509f4c5..376009b 100644
--- a/aos/input/action_joystick_input.cc
+++ b/aos/input/action_joystick_input.cc
@@ -1,7 +1,7 @@
#include "aos/input/action_joystick_input.h"
#include "aos/input/driver_station_data.h"
-#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/auto_generated.h"
#include "frc971/autonomous/base_autonomous_actor.h"
using ::aos::input::driver_station::ControlBit;
@@ -49,8 +49,10 @@
void ActionJoystickInput::StartAuto() {
AOS_LOG(INFO, "Starting auto mode\n");
- action_queue_.EnqueueAction(
- autonomous_action_factory_.Make(GetAutonomousMode()));
+ frc971::autonomous::AutonomousActionParamsT params;
+ params.mode = GetAutonomousMode();
+
+ action_queue_.EnqueueAction(autonomous_action_factory_.Make(params));
auto_action_running_ = true;
}
diff --git a/aos/input/action_joystick_input.h b/aos/input/action_joystick_input.h
index 3057ace..781e224 100644
--- a/aos/input/action_joystick_input.h
+++ b/aos/input/action_joystick_input.h
@@ -4,7 +4,7 @@
#include "aos/input/driver_station_data.h"
#include "aos/input/drivetrain_input.h"
#include "aos/input/joystick_input.h"
-#include "frc971/autonomous/auto.q.h"
+#include "frc971/autonomous/auto_generated.h"
#include "frc971/autonomous/base_autonomous_actor.h"
namespace aos {
@@ -99,7 +99,7 @@
AOS_LOG(WARNING, "no auto mode values\n");
return 0;
}
- return autonomous_mode_fetcher_->mode;
+ return autonomous_mode_fetcher_->mode();
}
// True if the internal state machine thinks auto is running right now.
diff --git a/aos/input/driver_station_data.cc b/aos/input/driver_station_data.cc
index 8898ca1..9e3bc52 100644
--- a/aos/input/driver_station_data.cc
+++ b/aos/input/driver_station_data.cc
@@ -1,20 +1,39 @@
#include "aos/input/driver_station_data.h"
+#include "glog/logging.h"
+
namespace aos {
namespace input {
namespace driver_station {
Data::Data() : current_values_(), old_values_() {}
-void Data::Update(const JoystickState &new_values) {
+void Data::Update(const JoystickState *new_values) {
old_values_ = current_values_;
- current_values_ = new_values;
+ CHECK(new_values->has_joysticks());
+ CHECK_EQ(new_values->joysticks()->size(), current_values_.joysticks.size());
+ for (size_t i = 0; i < current_values_.joysticks.size(); ++i) {
+ const Joystick *joystick = new_values->joysticks()->Get(i);
+ current_values_.joysticks[i].buttons =
+ joystick->buttons();
+ current_values_.joysticks[i].pov = joystick->pov();
+ for (size_t j = 0; j < joystick->axis()->size(); ++j) {
+ current_values_.joysticks[i].axis[j] = joystick->axis()->Get(j);
+ }
+
+ current_values_.joysticks[i].pov = joystick->pov();
+ }
+ current_values_.test_mode = new_values->test_mode();
+ current_values_.fms_attached = new_values->fms_attached();
+ current_values_.enabled = new_values->enabled();
+ current_values_.autonomous = new_values->autonomous();
+ current_values_.team_id = new_values->team_id();
+ current_values_.switch_left = new_values->switch_left();
+ current_values_.scale_left = new_values->scale_left();
}
-namespace {
-
-bool GetButton(const ButtonLocation location,
- const JoystickState &values) {
+bool Data::GetButton(const ButtonLocation location,
+ const Data::SavedJoystickState &values) {
if (location.joystick() < 0 ||
location.joystick() > static_cast<int>(values.joysticks.size())) {
return false;
@@ -23,15 +42,16 @@
return false;
}
return values.joysticks[location.joystick() - 1].buttons &
- (1 << (location.number() - 1));
+ (1 << (location.number() - 1));
}
-bool DoGetPOV(const POVLocation location, const JoystickState &values) {
+bool Data::DoGetPOV(const POVLocation location,
+ const Data::SavedJoystickState &values) {
return values.joysticks[location.joystick() - 1].pov == location.number();
}
-bool GetControlBitValue(const ControlBit bit,
- const JoystickState &values) {
+bool Data::GetControlBitValue(const ControlBit bit,
+ const Data::SavedJoystickState &values) {
switch (bit) {
case ControlBit::kTestMode:
return values.test_mode;
@@ -46,8 +66,6 @@
}
}
-} // namespace
-
bool Data::IsPressed(const ButtonLocation location) const {
return GetButton(location, current_values_);
}
diff --git a/aos/input/driver_station_data.h b/aos/input/driver_station_data.h
index 6a52248..18eb579 100644
--- a/aos/input/driver_station_data.h
+++ b/aos/input/driver_station_data.h
@@ -4,9 +4,10 @@
// This file defines several types to support nicely looking at the data
// received from the driver's station.
+#include <array>
#include <memory>
-#include "aos/robot_state/robot_state.q.h"
+#include "aos/robot_state/joystick_state_generated.h"
namespace aos {
namespace input {
@@ -20,8 +21,7 @@
: joystick_(joystick), number_(number) {}
// How many joysticks there are.
- static const int kJoysticks = sizeof(JoystickState::joysticks) /
- sizeof(JoystickState::joysticks[0]);
+ static const int kJoysticks = 6;
// Which joystick number this is (1-based).
int joystick() const { return joystick_; }
@@ -68,8 +68,7 @@
: JoystickFeature(joystick, number) {}
// How many axes there are available on each joystick.
- static const int kAxes = sizeof(Joystick::axis) /
- sizeof(Joystick::axis[0]);
+ static const int kAxes = 6;
};
class Data {
@@ -79,7 +78,7 @@
Data();
// Updates the current information with a new set of values.
- void Update(const JoystickState &new_values);
+ void Update(const JoystickState *new_values);
bool IsPressed(POVLocation location) const;
bool PosEdge(POVLocation location) const;
@@ -101,7 +100,34 @@
float GetAxis(JoystickAxis axis) const;
private:
- JoystickState current_values_, old_values_;
+ struct SavedJoystickState {
+ struct SavedJoystick {
+ uint16_t buttons = 0;
+ std::array<double, JoystickAxis::kAxes> axis;
+ int pov = 0;
+ };
+
+ std::array<SavedJoystick, JoystickFeature::kJoysticks> joysticks;
+ bool test_mode = false;
+ bool fms_attached = false;
+ bool enabled = false;
+ bool autonomous = false;
+ uint16_t team_id = 0;
+
+ // 2018 scale and switch positions.
+ bool switch_left = false;
+ bool scale_left = false;
+ };
+
+ static bool GetButton(const ButtonLocation location,
+ const Data::SavedJoystickState &values);
+ static bool DoGetPOV(const POVLocation location,
+ const Data::SavedJoystickState &values);
+
+ static bool GetControlBitValue(const ControlBit bit,
+ const Data::SavedJoystickState &values);
+
+ SavedJoystickState current_values_, old_values_;
};
} // namespace driver_station
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index a0bbf2c..687c8bb 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -8,13 +8,17 @@
#include "aos/commonmath.h"
#include "aos/input/driver_station_data.h"
#include "aos/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
using ::aos::input::driver_station::POVLocation;
+namespace drivetrain = frc971::control_loops::drivetrain;
+
namespace aos {
namespace input {
@@ -33,7 +37,7 @@
drivetrain_status_fetcher_.Fetch();
if (drivetrain_status_fetcher_.get()) {
- robot_velocity_ = drivetrain_status_fetcher_->robot_speed;
+ robot_velocity_ = drivetrain_status_fetcher_->robot_speed();
}
// If we have a vision align function, and it is in control, don't run the
@@ -71,9 +75,9 @@
if (drivetrain_status_fetcher_.get()) {
if (is_control_loop_driving && !last_is_control_loop_driving_) {
- left_goal_ = drivetrain_status_fetcher_->estimated_left_position +
+ left_goal_ = drivetrain_status_fetcher_->estimated_left_position() +
wheel * wheel_multiplier_;
- right_goal_ = drivetrain_status_fetcher_->estimated_right_position -
+ right_goal_ = drivetrain_status_fetcher_->estimated_right_position() -
wheel * wheel_multiplier_;
}
}
@@ -82,24 +86,36 @@
left_goal_ - wheel * wheel_multiplier_ + throttle * 0.3;
const double current_right_goal =
right_goal_ + wheel * wheel_multiplier_ + throttle * 0.3;
- auto new_drivetrain_goal = drivetrain_goal_sender_.MakeMessage();
- new_drivetrain_goal->wheel = wheel;
- new_drivetrain_goal->wheel_velocity = wheel_velocity;
- new_drivetrain_goal->wheel_torque = wheel_torque;
- new_drivetrain_goal->throttle = throttle;
- new_drivetrain_goal->throttle_velocity = throttle_velocity;
- new_drivetrain_goal->throttle_torque = throttle_torque;
- new_drivetrain_goal->highgear = high_gear;
- new_drivetrain_goal->quickturn = data.IsPressed(quick_turn_);
- new_drivetrain_goal->controller_type =
- is_line_following ? 3 : (is_control_loop_driving ? 1 : 0);
- new_drivetrain_goal->left_goal = current_left_goal;
- new_drivetrain_goal->right_goal = current_right_goal;
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
- new_drivetrain_goal->linear.max_velocity = 3.0;
- new_drivetrain_goal->linear.max_acceleration = 20.0;
+ frc971::ProfileParameters::Builder linear_builder =
+ builder.MakeBuilder<frc971::ProfileParameters>();
- if (!new_drivetrain_goal.Send()) {
+ linear_builder.add_max_velocity(3.0);
+ linear_builder.add_max_acceleration(20.0);
+
+ flatbuffers::Offset<frc971::ProfileParameters> linear_offset =
+ linear_builder.Finish();
+
+ auto goal_builder = builder.MakeBuilder<drivetrain::Goal>();
+ goal_builder.add_wheel(wheel);
+ goal_builder.add_wheel_velocity(wheel_velocity);
+ goal_builder.add_wheel_torque(wheel_torque);
+ goal_builder.add_throttle(throttle);
+ goal_builder.add_throttle_velocity(throttle_velocity);
+ goal_builder.add_throttle_torque(throttle_torque);
+ goal_builder.add_highgear(high_gear);
+ goal_builder.add_quickturn(data.IsPressed(quick_turn_));
+ goal_builder.add_controller_type(
+ is_line_following
+ ? drivetrain::ControllerType_LINE_FOLLOWER
+ : (is_control_loop_driving ? drivetrain::ControllerType_MOTION_PROFILE
+ : drivetrain::ControllerType_POLYDRIVE));
+ goal_builder.add_left_goal(current_left_goal);
+ goal_builder.add_right_goal(current_right_goal);
+ goal_builder.add_linear(linear_offset);
+
+ if (!builder.Send(goal_builder.Finish())) {
AOS_LOG(WARNING, "sending stick values failed\n");
}
@@ -304,8 +320,7 @@
}
::std::unique_ptr<DrivetrainInputReader> DrivetrainInputReader::Make(
::aos::EventLoop *event_loop, InputType type,
- const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &dt_config) {
+ const drivetrain::DrivetrainConfig<double> &dt_config) {
std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader;
using InputType = DrivetrainInputReader::InputType;
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
index 26dfe95..0c43d99 100644
--- a/aos/input/drivetrain_input.h
+++ b/aos/input/drivetrain_input.h
@@ -9,8 +9,10 @@
#include "aos/input/driver_station_data.h"
#include "aos/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
namespace aos {
namespace input {
@@ -60,12 +62,11 @@
turn2_use_(turn2_use),
drivetrain_status_fetcher_(
event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ ->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
drivetrain_goal_sender_(
- event_loop
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")) {}
+ event_loop->MakeSender<::frc971::control_loops::drivetrain::Goal>(
+ "/drivetrain")) {}
virtual ~DrivetrainInputReader() = default;
@@ -130,9 +131,9 @@
virtual WheelAndThrottle GetWheelAndThrottle(
const ::aos::input::driver_station::Data &data) = 0;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_sender_;
double robot_velocity_ = 0.0;
diff --git a/aos/input/joystick_input.cc b/aos/input/joystick_input.cc
index d1bd302..743e138 100644
--- a/aos/input/joystick_input.cc
+++ b/aos/input/joystick_input.cc
@@ -3,18 +3,17 @@
#include <string.h>
#include <atomic>
-#include "aos/robot_state/robot_state.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
+#include "aos/robot_state/robot_state_generated.h"
namespace aos {
namespace input {
-void JoystickInput::HandleData(const ::aos::JoystickState &joystick_state) {
+void JoystickInput::HandleData(const ::aos::JoystickState *joystick_state) {
data_.Update(joystick_state);
- mode_ = static_cast<int>(joystick_state.switch_left) |
- (static_cast<int>(joystick_state.scale_left) << 1);
+ mode_ = static_cast<int>(joystick_state->switch_left()) |
+ (static_cast<int>(joystick_state->scale_left()) << 1);
{
using driver_station::JoystickFeature;
diff --git a/aos/input/joystick_input.h b/aos/input/joystick_input.h
index ed72e78..5e8bf5a 100644
--- a/aos/input/joystick_input.h
+++ b/aos/input/joystick_input.h
@@ -3,7 +3,7 @@
#include <atomic>
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/input/driver_station_data.h"
namespace aos {
@@ -21,7 +21,7 @@
event_loop_->MakeWatcher(
".aos.joystick_state",
[this](const ::aos::JoystickState &joystick_state) {
- this->HandleData(joystick_state);
+ this->HandleData(&joystick_state);
});
event_loop->SetRuntimeRealtimePriority(29);
}
@@ -30,7 +30,7 @@
int mode() const { return mode_; }
private:
- void HandleData(const ::aos::JoystickState &joystick_state);
+ void HandleData(const ::aos::JoystickState *joystick_state);
// Subclasses should do whatever they want with data here.
virtual void RunIteration(const driver_station::Data &data) = 0;
@@ -41,12 +41,6 @@
int mode_;
};
-// Class which will proxy joystick information from UDP packets to the queues.
-class JoystickProxy {
- public:
- void Run();
-};
-
} // namespace input
} // namespace aos