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/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 9b9ba30..1575584 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -1,5 +1,5 @@
-load("//aos/build:queues.bzl", "queue_library")
-load("//tools/build_rules:select.bzl", "cpu_select", "compiler_select")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//tools/build_rules:select.bzl", "compiler_select", "cpu_select")
genrule(
name = "genrule_drivetrain",
@@ -31,39 +31,6 @@
],
)
-cc_binary(
- name = "replay_localizer",
- srcs = [
- "replay_localizer.cc",
- ],
- defines =
- cpu_select({
- "amd64": [
- "SUPPORT_PLOT=1",
- ],
- "arm": [],
- }),
- linkstatic = True,
- deps = [
- "//frc971/control_loops/drivetrain:localizer_queue",
- ":localizer",
- ":event_loop_localizer",
- ":drivetrain_base",
- "@com_github_gflags_gflags//:gflags",
- "//y2019:constants",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
- "//aos:init",
- "//aos/controls:replay_control_loop",
- "//frc971/queues:gyro",
- "//frc971/wpilib:imu_queue",
- ] + cpu_select({
- "amd64": [
- "//third_party/matplotlib-cpp",
- ],
- "arm": [],
- }),
-)
-
cc_library(
name = "polydrivetrain_plants",
srcs = [
@@ -111,24 +78,24 @@
":drivetrain_base",
":event_loop_localizer",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
-queue_library(
- name = "target_selector_queue",
- srcs = [
- "target_selector.q",
- ],
+flatbuffer_cc_library(
+ name = "target_selector_fbs",
+ srcs = ["target_selector.fbs"],
+ gen_reflections = 1,
visibility = ["//visibility:public"],
)
-queue_library(
- name = "camera_queue",
+flatbuffer_cc_library(
+ name = "camera_fbs",
srcs = [
- "camera.q",
+ "camera.fbs",
],
+ gen_reflections = 1,
visibility = ["//visibility:public"],
)
@@ -167,17 +134,18 @@
hdrs = ["target_selector.h"],
deps = [
":camera",
- ":target_selector_queue",
+ ":target_selector_fbs",
"//frc971/control_loops:pose",
"//frc971/control_loops/drivetrain:localizer",
"//y2019:constants",
- "//y2019/control_loops/superstructure:superstructure_queue",
+ "//y2019/control_loops/superstructure:superstructure_goal_fbs",
],
)
cc_test(
name = "target_selector_test",
srcs = ["target_selector_test.cc"],
+ data = ["//y2019:config.json"],
deps = [
":target_selector",
"//aos/events:simulated_event_loop",
@@ -191,7 +159,7 @@
srcs = ["event_loop_localizer.cc"],
hdrs = ["event_loop_localizer.h"],
deps = [
- ":camera_queue",
+ ":camera_fbs",
":localizer",
":target_selector",
"//frc971/control_loops/drivetrain:localizer",
@@ -232,13 +200,14 @@
cc_test(
name = "localized_drivetrain_test",
srcs = ["localized_drivetrain_test.cc"],
+ data = ["//y2019:config.json"],
deps = [
- ":camera_queue",
+ ":camera_fbs",
":drivetrain_base",
":event_loop_localizer",
":localizer",
"//aos/controls:control_loop_test",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//aos/network:team_number",
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_lib",
diff --git a/y2019/control_loops/drivetrain/camera.fbs b/y2019/control_loops/drivetrain/camera.fbs
new file mode 100644
index 0000000..154dd2e
--- /dev/null
+++ b/y2019/control_loops/drivetrain/camera.fbs
@@ -0,0 +1,24 @@
+namespace y2019.control_loops.drivetrain;
+
+// See the Target structure in //y2019/jevois:structures.h for documentation.
+table CameraTarget {
+ distance:float;
+ height:float;
+ heading:float;
+ skew:float;
+}
+
+// Frames from a camera.
+table CameraFrame {
+ // Number of nanoseconds since the aos::monotonic_clock epoch at which this
+ // frame was captured.
+ timestamp:long;
+
+ // Buffer for the 3 targets.
+ targets:[CameraTarget];
+
+ // Index of the camera position (not serial number) which this frame is from.
+ camera:ubyte;
+}
+
+root_type CameraFrame;
diff --git a/y2019/control_loops/drivetrain/camera.q b/y2019/control_loops/drivetrain/camera.q
deleted file mode 100644
index 9d5dffc..0000000
--- a/y2019/control_loops/drivetrain/camera.q
+++ /dev/null
@@ -1,26 +0,0 @@
-package y2019.control_loops.drivetrain;
-
-// See the Target structure in //y2019/jevois:structures.h for documentation.
-struct CameraTarget {
- float distance;
- float height;
- float heading;
- float skew;
-};
-
-// Frames from a camera.
-// Published on ".y2019.control_loops.drivetrain.camera_frames"
-message CameraFrame {
- // Number of nanoseconds since the aos::monotonic_clock epoch at which this
- // frame was captured.
- int64_t timestamp;
-
- // Number of targets actually in this frame.
- uint8_t num_targets;
-
- // Buffer for the targets.
- CameraTarget[3] targets;
-
- // Index of the camera position (not serial number) which this frame is from.
- uint8_t camera;
-};
diff --git a/y2019/control_loops/drivetrain/drivetrain_main.cc b/y2019/control_loops/drivetrain/drivetrain_main.cc
index ada8f53..77df69b 100644
--- a/y2019/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_main.cc
@@ -1,6 +1,6 @@
#include "aos/init.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
@@ -10,7 +10,10 @@
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2019::control_loops::drivetrain::EventLoopLocalizer localizer(
&event_loop, ::y2019::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 67a708a..a372df4 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -45,8 +45,7 @@
localizer_.ResetInitialState(monotonic_now, localizer_.X_hat(),
localizer_.P());
});
- frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>(
- ".y2019.control_loops.drivetrain.camera_frames");
+ frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>("/drivetrain");
}
void EventLoopLocalizer::Reset(::aos::monotonic_clock::time_point now,
@@ -71,37 +70,39 @@
localizer_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U,
now);
while (frame_fetcher_.FetchNext()) {
- HandleFrame(*frame_fetcher_.get());
+ HandleFrame(frame_fetcher_.get());
}
}
-void EventLoopLocalizer::HandleFrame(const CameraFrame &frame) {
+void EventLoopLocalizer::HandleFrame(const CameraFrame *frame) {
// We need to construct TargetView's and pass them to the localizer:
::aos::SizedArray<TargetView, kMaxTargetsPerFrame> views;
// Note: num_targets and camera are unsigned integers and so don't need to be
// checked for < 0.
- if (frame.num_targets > kMaxTargetsPerFrame) {
- AOS_LOG(ERROR, "Got bad num_targets %d\n", frame.num_targets);
+ size_t camera = frame->camera();
+ if (!frame->has_targets() || frame->targets()->size() > kMaxTargetsPerFrame) {
+ AOS_LOG(ERROR, "Got bad num_targets %d\n",
+ frame->has_targets() ? frame->targets()->size() : 0);
return;
}
- if (frame.camera > cameras_.size()) {
- AOS_LOG(ERROR, "Got bad camera number %d\n", frame.camera);
+ if (camera > cameras_.size()) {
+ AOS_LOG(ERROR, "Got bad camera number %zu\n", camera);
return;
}
- for (int ii = 0; ii < frame.num_targets; ++ii) {
+ for (const CameraTarget *target : *frame->targets()) {
TargetView view;
- view.reading.heading = frame.targets[ii].heading;
- view.reading.distance = frame.targets[ii].distance;
- view.reading.skew = frame.targets[ii].skew;
- view.reading.height = frame.targets[ii].height;
+ view.reading.heading = target->heading();
+ view.reading.distance = target->distance();
+ view.reading.skew = target->skew();
+ view.reading.height = target->height();
if (view.reading.distance < 2.25) {
- cameras_[frame.camera].PopulateNoise(&view);
+ cameras_[camera].PopulateNoise(&view);
views.push_back(view);
}
}
::aos::monotonic_clock::time_point t(
- ::std::chrono::nanoseconds(frame.timestamp));
- localizer_.UpdateTargets(cameras_[frame.camera], views, t);
+ ::std::chrono::nanoseconds(frame->timestamp()));
+ localizer_.UpdateTargets(cameras_[camera], views, t);
}
} // namespace drivetrain
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index dac614b..83b8e79 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -3,7 +3,7 @@
#include "frc971/control_loops/drivetrain/localizer.h"
#include "y2019/constants.h"
-#include "y2019/control_loops/drivetrain/camera.q.h"
+#include "y2019/control_loops/drivetrain/camera_generated.h"
#include "y2019/control_loops/drivetrain/localizer.h"
#include "y2019/control_loops/drivetrain/target_selector.h"
@@ -92,7 +92,7 @@
}
private:
- void HandleFrame(const CameraFrame &frame);
+ void HandleFrame(const CameraFrame *frame);
::aos::EventLoop *event_loop_;
// TODO(james): Make this use Watchers once we have them working in our
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index ad7223e..06d2f24 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -7,9 +7,9 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/team_number_test_environment.h"
-#include "y2019/control_loops/drivetrain/camera.q.h"
-#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
+#include "y2019/control_loops/drivetrain/camera_generated.h"
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
+#include "y2019/control_loops/drivetrain/event_loop_localizer.h"
// This file tests that the full Localizer, when used with queues within the
// drivetrain, will behave properly. The purpose of this test is to make sure
@@ -20,7 +20,9 @@
namespace drivetrain {
namespace testing {
-using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using frc971::control_loops::drivetrain::DrivetrainConfig;
+using frc971::control_loops::drivetrain::Goal;
+using frc971::control_loops::drivetrain::LocalizerControl;
namespace {
DrivetrainConfig<double> GetTest2019DrivetrainConfig() {
@@ -42,25 +44,21 @@
// We must use the 2019 drivetrain config so that we don't have to deal
// with shifting:
LocalizedDrivetrainTest()
- : ::aos::testing::ControlLoopTest(GetTest2019DrivetrainConfig().dt),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2019/config.json"),
+ GetTest2019DrivetrainConfig().dt),
test_event_loop_(MakeEventLoop()),
drivetrain_goal_sender_(
- test_event_loop_
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")),
+ test_event_loop_->MakeSender<Goal>("/drivetrain")),
drivetrain_goal_fetcher_(
- test_event_loop_
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
- ".frc971.control_loops.drivetrain_queue.goal")),
+ test_event_loop_->MakeFetcher<Goal>("/drivetrain")),
localizer_control_sender_(
- test_event_loop_->MakeSender<
- ::frc971::control_loops::drivetrain::LocalizerControl>(
- ".frc971.control_loops.drivetrain.localizer_control")),
+ test_event_loop_->MakeSender<LocalizerControl>("/drivetrain")),
drivetrain_event_loop_(MakeEventLoop()),
dt_config_(GetTest2019DrivetrainConfig()),
- camera_sender_(test_event_loop_->MakeSender<CameraFrame>(
- ".y2019.control_loops.drivetrain.camera_frames")),
+ camera_sender_(
+ test_event_loop_->MakeSender<CameraFrame>("/drivetrain")),
localizer_(drivetrain_event_loop_.get(), dt_config_),
drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
@@ -76,8 +74,8 @@
set_battery_voltage(12.0);
test_event_loop_->MakeWatcher(
- ".frc971.control_loops.drivetrain_queue.status",
- [this](const ::frc971::control_loops::DrivetrainQueue::Status &) {
+ "/drivetrain",
+ [this](const ::frc971::control_loops::drivetrain::Status &) {
// Needs to do camera updates right after we run the control loop.
if (enable_cameras_) {
SendDelayedFrames();
@@ -104,9 +102,9 @@
void VerifyNearGoal() {
drivetrain_goal_fetcher_.Fetch();
- EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal,
+ EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal(),
drivetrain_plant_.GetLeftPosition(), 1e-3);
- EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal,
+ EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal(),
drivetrain_plant_.GetRightPosition(), 1e-3);
}
@@ -125,32 +123,36 @@
robot_pose_.set_theta(drivetrain_plant_.state()(2, 0));
for (size_t ii = 0; ii < cameras_.size(); ++ii) {
const auto target_views = cameras_[ii].target_views();
- CameraFrame frame;
- frame.timestamp = chrono::duration_cast<chrono::nanoseconds>(
- monotonic_now().time_since_epoch())
- .count();
- frame.camera = ii;
- frame.num_targets = 0;
+ std::unique_ptr<CameraFrameT> frame(new CameraFrameT());
+
for (size_t jj = 0;
jj < ::std::min(EventLoopLocalizer::kMaxTargetsPerFrame,
target_views.size());
++jj) {
EventLoopLocalizer::TargetView view = target_views[jj];
- ++frame.num_targets;
+ std::unique_ptr<CameraTargetT> camera_target(new CameraTargetT());
+
const float nan = ::std::numeric_limits<float>::quiet_NaN();
if (send_bad_frames_) {
- frame.targets[jj].heading = nan;
- frame.targets[jj].distance = nan;
- frame.targets[jj].skew = nan;
- frame.targets[jj].height = nan;
+ camera_target->heading = nan;
+ camera_target->distance = nan;
+ camera_target->skew = nan;
+ camera_target->height = nan;
} else {
- frame.targets[jj].heading = view.reading.heading;
- frame.targets[jj].distance = view.reading.distance;
- frame.targets[jj].skew = view.reading.skew;
- frame.targets[jj].height = view.reading.height;
+ camera_target->heading = view.reading.heading;
+ camera_target->distance = view.reading.distance;
+ camera_target->skew = view.reading.skew;
+ camera_target->height = view.reading.height;
}
+ frame->targets.emplace_back(std::move(camera_target));
}
- camera_delay_queue_.emplace(monotonic_now(), frame);
+
+ frame->timestamp = chrono::duration_cast<chrono::nanoseconds>(
+ monotonic_now().time_since_epoch())
+ .count();
+ frame->camera = ii;
+
+ camera_delay_queue_.emplace(monotonic_now(), std::move(frame));
}
}
@@ -159,20 +161,17 @@
while (!camera_delay_queue_.empty() &&
::std::get<0>(camera_delay_queue_.front()) <
monotonic_now() - camera_latency) {
- auto message = camera_sender_.MakeMessage();
- *message = ::std::get<1>(camera_delay_queue_.front());
- ASSERT_TRUE(message.Send());
+ auto builder = camera_sender_.MakeBuilder();
+ ASSERT_TRUE(builder.Send(CameraFrame::Pack(
+ *builder.fbb(), ::std::get<1>(camera_delay_queue_.front()).get())));
camera_delay_queue_.pop();
}
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
- drivetrain_goal_sender_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
- drivetrain_goal_fetcher_;
- ::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
- localizer_control_sender_;
+ ::aos::Sender<Goal> drivetrain_goal_sender_;
+ ::aos::Fetcher<Goal> drivetrain_goal_fetcher_;
+ ::aos::Sender<LocalizerControl> localizer_control_sender_;
::std::unique_ptr<::aos::EventLoop> drivetrain_event_loop_;
const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
@@ -192,7 +191,8 @@
// A queue of camera frames so that we can add a time delay to the data
// coming from the cameras.
- ::std::queue<::std::tuple<::aos::monotonic_clock::time_point, CameraFrame>>
+ ::std::queue<::std::tuple<::aos::monotonic_clock::time_point,
+ std::unique_ptr<CameraFrameT>>>
camera_delay_queue_;
void set_enable_cameras(bool enable) { enable_cameras_ = enable; }
@@ -210,11 +210,15 @@
set_enable_cameras(false);
VerifyEstimatorAccurate(1e-10);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -227,11 +231,15 @@
set_enable_cameras(true);
set_bad_frames(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -243,11 +251,15 @@
SetEnabled(true);
set_enable_cameras(true);
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -261,17 +273,20 @@
set_enable_cameras(false);
(*drivetrain_plant_.mutable_state())(0, 0) += 0.05;
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(3));
// VerifyNearGoal succeeds because it is just checking wheel positions:
VerifyNearGoal();
- const Eigen::Matrix<double, 5, 1> true_state =
- drivetrain_plant_.state();
+ const Eigen::Matrix<double, 5, 1> true_state = drivetrain_plant_.state();
// Everything but X-value should be correct:
EXPECT_NEAR(true_state.x(), localizer_.x() + 0.05, 1e-5);
EXPECT_NEAR(true_state.y(), localizer_.y(), 1e-5);
@@ -287,19 +302,28 @@
set_enable_cameras(false);
(*drivetrain_plant_.mutable_state())(0, 0) += 0.05;
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
{
- auto message = localizer_control_sender_.MakeMessage();
- message->x = drivetrain_plant_.state().x();
- message->y = drivetrain_plant_.state().y();
- message->theta = drivetrain_plant_.state()(2, 0);
- ASSERT_TRUE(message.Send());
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+
+ localizer_control_builder.add_x(drivetrain_plant_.state().x());
+ localizer_control_builder.add_y(drivetrain_plant_.state().y());
+ localizer_control_builder.add_theta(drivetrain_plant_.state()(2, 0));
+
+ EXPECT_TRUE(builder.Send(localizer_control_builder.Finish()));
}
RunFor(chrono::seconds(3));
VerifyNearGoal();
@@ -314,11 +338,15 @@
SetStartingPosition({4.0, 0.5, 0.0});
(*drivetrain_plant_.mutable_state())(0, 0) += 0.05;
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 1;
- message->left_goal = -1.0;
- message->right_goal = 1.0;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(-1.0);
+ drivetrain_builder.add_right_goal(1.0);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(5));
VerifyNearGoal();
@@ -326,7 +354,9 @@
}
namespace {
-EventLoopLocalizer::Pose HPSlotLeft() { return constants::Field().targets()[7].pose(); }
+EventLoopLocalizer::Pose HPSlotLeft() {
+ return constants::Field().targets()[7].pose();
+}
} // namespace
// Tests that using the line following drivetrain and just driving straight
@@ -336,10 +366,14 @@
set_enable_cameras(false);
SetStartingPosition({4, HPSlotLeft().abs_pos().y(), M_PI});
{
- auto message = drivetrain_goal_sender_.MakeMessage();
- message->controller_type = 3;
- message->throttle = 0.5;
- EXPECT_TRUE(message.Send());
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_LINE_FOLLOWER);
+ drivetrain_builder.add_throttle(0.5);
+
+ EXPECT_TRUE(builder.Send(drivetrain_builder.Finish()));
}
RunFor(chrono::seconds(6));
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index 1950468..d9a55f6 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -141,26 +141,75 @@
}
void SetUp() {
- ::frc971::control_loops::DrivetrainQueue::Goal goal;
- goal.controller_type = 2;
- goal.spline.spline_idx = 1;
- goal.spline.spline_count = 1;
- goal.spline_handle = 1;
- ::std::copy(GetParam().control_pts_x.begin(),
- GetParam().control_pts_x.end(), goal.spline.spline_x.begin());
- ::std::copy(GetParam().control_pts_y.begin(),
- GetParam().control_pts_y.end(), goal.spline.spline_y.begin());
- spline_drivetrain_.SetGoal(goal);
+ flatbuffers::DetachedBuffer goal_buffer;
+ {
+ flatbuffers::FlatBufferBuilder fbb;
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ fbb.CreateVector<float>(GetParam().control_pts_x.begin(),
+ GetParam().control_pts_x.size());
+
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ fbb.CreateVector<float>(GetParam().control_pts_y.begin(),
+ GetParam().control_pts_y.size());
+
+ frc971::MultiSpline::Builder multispline_builder(fbb);
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ flatbuffers::Offset<frc971::MultiSpline> multispline_offset =
+ multispline_builder.Finish();
+
+ frc971::control_loops::drivetrain::SplineGoal::Builder spline_builder(
+ fbb);
+
+ spline_builder.add_spline_idx(1);
+ spline_builder.add_spline(multispline_offset);
+
+ flatbuffers::Offset<frc971::control_loops::drivetrain::SplineGoal>
+ spline_offset = spline_builder.Finish();
+
+ frc971::control_loops::drivetrain::Goal::Builder goal_builder(fbb);
+
+ goal_builder.add_spline(spline_offset);
+ goal_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType_SPLINE_FOLLOWER);
+ goal_builder.add_spline_handle(1);
+
+ fbb.Finish(goal_builder.Finish());
+
+ goal_buffer = fbb.Release();
+ }
+ aos::FlatbufferDetachedBuffer<frc971::control_loops::drivetrain::Goal> goal(
+ std::move(goal_buffer));
+
+ spline_drivetrain_.SetGoal(&goal.message());
// Let the spline drivetrain compute the spline.
- ::frc971::control_loops::DrivetrainQueue::Status status;
- do {
+ while (true) {
::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
- spline_drivetrain_.PopulateStatus(&status);
- } while (status.trajectory_logging.planning_state !=
- (int8_t)::frc971::control_loops::drivetrain::SplineDrivetrain::
- PlanState::kPlannedTrajectory);
- spline_drivetrain_.SetGoal(goal);
+
+ flatbuffers::FlatBufferBuilder fbb;
+
+ flatbuffers::Offset<frc971::control_loops::drivetrain::TrajectoryLogging>
+ trajectory_logging_offset =
+ spline_drivetrain_.MakeTrajectoryLogging(&fbb);
+
+ ::frc971::control_loops::drivetrain::Status::Builder status_builder(fbb);
+ status_builder.add_trajectory_logging(trajectory_logging_offset);
+ spline_drivetrain_.PopulateStatus(&status_builder);
+ fbb.Finish(status_builder.Finish());
+ aos::FlatbufferDetachedBuffer<::frc971::control_loops::drivetrain::Status>
+ status(fbb.Release());
+
+ if (status.message().trajectory_logging()->planning_state() ==
+ ::frc971::control_loops::drivetrain::PlanningState_PLANNED) {
+ break;
+ }
+ }
+ spline_drivetrain_.SetGoal(&goal.message());
}
void TearDown() {
@@ -373,7 +422,7 @@
spline_drivetrain_.Update(true, known_state);
- ::frc971::control_loops::DrivetrainQueue::Output output;
+ ::frc971::control_loops::drivetrain::OutputT output;
output.left_voltage = 0;
output.right_voltage = 0;
spline_drivetrain_.SetOutput(&output);
diff --git a/y2019/control_loops/drivetrain/target_selector.cc b/y2019/control_loops/drivetrain/target_selector.cc
index 3e43db9..7f2259d 100644
--- a/y2019/control_loops/drivetrain/target_selector.cc
+++ b/y2019/control_loops/drivetrain/target_selector.cc
@@ -1,5 +1,7 @@
#include "y2019/control_loops/drivetrain/target_selector.h"
+#include "aos/json_to_flatbuffer.h"
+
namespace y2019 {
namespace control_loops {
@@ -11,10 +13,9 @@
back_viewer_({&robot_pose_, {0.0, 0.0, 0.0}, M_PI}, kFakeFov, fake_noise_,
constants::Field().targets(), {}),
hint_fetcher_(event_loop->MakeFetcher<drivetrain::TargetSelectorHint>(
- ".y2019.control_loops.drivetrain.target_selector_hint")),
- superstructure_goal_fetcher_(event_loop->MakeFetcher<
- superstructure::SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")) {}
+ "/drivetrain")),
+ superstructure_goal_fetcher_(
+ event_loop->MakeFetcher<superstructure::Goal>("/superstructure")) {}
bool TargetSelector::UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
double command_speed) {
@@ -22,14 +23,14 @@
return false;
}
if (superstructure_goal_fetcher_.Fetch()) {
- ball_mode_ = superstructure_goal_fetcher_->suction.gamepiece_mode == 0;
+ ball_mode_ = superstructure_goal_fetcher_->suction()->gamepiece_mode() == 0;
}
if (hint_fetcher_.Fetch()) {
- AOS_LOG_STRUCT(DEBUG, "selector_hint", *hint_fetcher_);
+ VLOG(1) << "selector_hint: " << aos::FlatbufferToJson(hint_fetcher_.get());
// suggested_target is unsigned so we don't check for >= 0.
- if (hint_fetcher_->suggested_target < 4) {
+ if (hint_fetcher_->suggested_target() < 4) {
target_hint_ =
- static_cast<SelectionHint>(hint_fetcher_->suggested_target);
+ static_cast<SelectionHint>(hint_fetcher_->suggested_target());
} else {
AOS_LOG(ERROR, "Got invalid suggested target.\n");
}
diff --git a/y2019/control_loops/drivetrain/target_selector.fbs b/y2019/control_loops/drivetrain/target_selector.fbs
new file mode 100644
index 0000000..d264992
--- /dev/null
+++ b/y2019/control_loops/drivetrain/target_selector.fbs
@@ -0,0 +1,23 @@
+namespace y2019.control_loops.drivetrain;
+
+// These should match the SelectionHint enum in target_selector.h.
+enum SelectionHint : byte {
+ // No selection, we should just default to whatever.
+ NONE,
+ // Near, middle, and far targets.
+ NEAR_SHIP,
+ MID_SHIP,
+ FAR_SHIP,
+ // Far side of the rocket ship.
+ FAR_ROCKET,
+}
+
+
+// A message to provide information to the target selector about what it should
+// The drivetrain listens on ".y2019.control_loops.drivetrain.target_selector_hint"
+table TargetSelectorHint {
+ // Which target we should go for:
+ suggested_target:SelectionHint;
+}
+
+root_type TargetSelectorHint;
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
index 7f86f89..e75e122 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -5,8 +5,8 @@
#include "frc971/control_loops/drivetrain/localizer.h"
#include "y2019/constants.h"
#include "y2019/control_loops/drivetrain/camera.h"
-#include "y2019/control_loops/drivetrain/target_selector.q.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/drivetrain/target_selector_generated.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
namespace y2019 {
namespace control_loops {
@@ -68,8 +68,7 @@
FakeCamera back_viewer_;
::aos::Fetcher<drivetrain::TargetSelectorHint> hint_fetcher_;
- ::aos::Fetcher<superstructure::SuperstructureQueue::Goal>
- superstructure_goal_fetcher_;
+ ::aos::Fetcher<superstructure::Goal> superstructure_goal_fetcher_;
// Whether we are currently in ball mode.
bool ball_mode_ = false;
diff --git a/y2019/control_loops/drivetrain/target_selector.q b/y2019/control_loops/drivetrain/target_selector.q
deleted file mode 100644
index 021bd3a..0000000
--- a/y2019/control_loops/drivetrain/target_selector.q
+++ /dev/null
@@ -1,12 +0,0 @@
-package y2019.control_loops.drivetrain;
-
-// A message to provide information to the target selector about what it should
-// The drivetrain listens on ".y2019.control_loops.drivetrain.target_selector_hint"
-message TargetSelectorHint {
- // Which target we should go for:
- // 0 implies no selection, we should just default to whatever.
- // 1, 2, and 3 imply the near, middle, and far targets.
- // 4 implies far side of the rocket ship.
- // These should match the SelectionHint enum in target_selector.h.
- uint8_t suggested_target;
-};
diff --git a/y2019/control_loops/drivetrain/target_selector_test.cc b/y2019/control_loops/drivetrain/target_selector_test.cc
index 32e060f..2e9028f 100644
--- a/y2019/control_loops/drivetrain/target_selector_test.cc
+++ b/y2019/control_loops/drivetrain/target_selector_test.cc
@@ -1,8 +1,8 @@
#include "y2019/control_loops/drivetrain/target_selector.h"
-#include "aos/events/simulated-event-loop.h"
+#include "aos/events/simulated_event_loop.h"
#include "gtest/gtest.h"
-#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
namespace y2019 {
namespace control_loops {
@@ -10,7 +10,6 @@
typedef ::frc971::control_loops::TypedPose<double> Pose;
typedef ::Eigen::Matrix<double, 5, 1> State;
-using SelectionHint = TargetSelector::SelectionHint;
namespace {
// Accessors to get some useful particular targets on the field:
@@ -29,7 +28,7 @@
struct TestParams {
State state;
bool ball_mode;
- SelectionHint selection_hint;
+ drivetrain::SelectionHint selection_hint;
double command_speed;
bool expect_target;
Pose expected_pose;
@@ -38,18 +37,21 @@
class TargetSelectorParamTest : public ::testing::TestWithParam<TestParams> {
public:
TargetSelectorParamTest()
- : event_loop_(this->event_loop_factory_.MakeEventLoop()),
+ : configuration_(aos::configuration::ReadConfig("y2019/config.json")),
+ event_loop_factory_(&configuration_.message()),
+ event_loop_(this->event_loop_factory_.MakeEventLoop()),
test_event_loop_(this->event_loop_factory_.MakeEventLoop()),
target_selector_hint_sender_(
test_event_loop_->MakeSender<
::y2019::control_loops::drivetrain::TargetSelectorHint>(
- ".y2019.control_loops.drivetrain.target_selector_hint")),
- superstructure_goal_sender_(test_event_loop_->MakeSender<
- ::y2019::control_loops::superstructure::
- SuperstructureQueue::Goal>(
- ".y2019.control_loops.superstructure.superstructure_queue.goal")) {}
+ "/drivetrain")),
+ superstructure_goal_sender_(
+ test_event_loop_
+ ->MakeSender<::y2019::control_loops::superstructure::Goal>(
+ "/superstructure")) {}
private:
+ aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
::aos::SimulatedEventLoopFactory event_loop_factory_;
protected:
@@ -58,22 +60,33 @@
::aos::Sender<::y2019::control_loops::drivetrain::TargetSelectorHint>
target_selector_hint_sender_;
- ::aos::Sender<::y2019::control_loops::superstructure::SuperstructureQueue::Goal>
+ ::aos::Sender<::y2019::control_loops::superstructure::Goal>
superstructure_goal_sender_;
-
};
TEST_P(TargetSelectorParamTest, ExpectReturn) {
TargetSelector selector(event_loop_.get());
{
- auto super_goal = superstructure_goal_sender_.MakeMessage();
- super_goal->suction.gamepiece_mode = GetParam().ball_mode ? 0 : 1;
- ASSERT_TRUE(super_goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ superstructure::SuctionGoal::Builder suction_builder =
+ builder.MakeBuilder<superstructure::SuctionGoal>();
+
+ suction_builder.add_gamepiece_mode(GetParam().ball_mode ? 0 : 1);
+
+ flatbuffers::Offset<superstructure::SuctionGoal> suction_offset =
+ suction_builder.Finish();
+
+ superstructure::Goal::Builder goal_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+
+ goal_builder.add_suction(suction_offset);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
{
- auto hint = target_selector_hint_sender_.MakeMessage();
- hint->suggested_target = static_cast<int>(GetParam().selection_hint);
- ASSERT_TRUE(hint.Send());
+ auto builder = target_selector_hint_sender_.MakeBuilder();
+ ASSERT_TRUE(builder.Send(drivetrain::CreateTargetSelectorHint(
+ *builder.fbb(), GetParam().selection_hint)));
}
bool expect_target = GetParam().expect_target;
const State state = GetParam().state;
@@ -105,7 +118,7 @@
// targets:
TestParams{(State() << 0.0, 0.0, 0.0, 1.0, 1.0).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
1.0,
false,
{},
@@ -114,41 +127,43 @@
// anything.
TestParams{(State() << 4.0, 2.0, M_PI, 0.05, 0.05).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
0.05,
false,
{},
/*expected_radius=*/0.0},
TestParams{(State() << 4.0, 2.0, M_PI, -0.05, -0.05).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
-0.05,
false,
{},
/*expected_radius=*/0.0},
TestParams{(State() << 4.0, 2.0, M_PI, 0.5, 0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, 1.0, true,
- HPSlotLeft(), /*expected_radius=*/0.0},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, 1.0,
+ true, HPSlotLeft(), /*expected_radius=*/0.0},
// Put ourselves between the rocket and cargo ship; we should see the
// hatches driving one direction and the near cargo ship port the other.
// We also command a speed opposite the current direction of motion and
// confirm that that behaves as expected.
TestParams{(State() << 6.0, 2.0, -M_PI_2, -0.5, -0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, 1.0, true,
- CargoNearLeft(), /*expected_radius=*/HatchRadius()},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, 1.0,
+ true, CargoNearLeft(), /*expected_radius=*/HatchRadius()},
TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, -1.0, true,
- CargoNearLeft(), /*expected_radius=*/HatchRadius()},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, -1.0,
+ true, CargoNearLeft(), /*expected_radius=*/HatchRadius()},
TestParams{(State() << 6.0, 2.0, -M_PI_2, 0.5, 0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, -1.0, true,
- RocketHatchFarLeft(), /*expected_radius=*/HatchRadius()},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, -1.0,
+ true, RocketHatchFarLeft(),
+ /*expected_radius=*/HatchRadius()},
TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, -0.5).finished(),
- /*ball_mode=*/false, SelectionHint::kNone, 1.0, true,
- RocketHatchFarLeft(), /*expected_radius=*/HatchRadius()},
+ /*ball_mode=*/false, drivetrain::SelectionHint_NONE, 1.0,
+ true, RocketHatchFarLeft(),
+ /*expected_radius=*/HatchRadius()},
// And we shouldn't see anything spinning in place:
TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, 0.5).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
0.0,
false,
{},
@@ -156,7 +171,7 @@
// Drive backwards off the field--we should not see anything.
TestParams{(State() << -0.1, 0.0, 0.0, -0.5, -0.5).finished(),
/*ball_mode=*/false,
- SelectionHint::kNone,
+ drivetrain::SelectionHint_NONE,
-1.0,
false,
{},
@@ -164,21 +179,14 @@
// In ball mode, we should be able to see the portal, and get zero
// radius.
TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(),
- /*ball_mode=*/true,
- SelectionHint::kNone,
- 1.0,
- true,
- RocketPortal(),
+ /*ball_mode=*/true, drivetrain::SelectionHint_NONE, 1.0,
+ true, RocketPortal(),
/*expected_radius=*/0.0},
// Reversing direction should get cargo ship with zero radius.
TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(),
- /*ball_mode=*/true,
- SelectionHint::kNone,
- -1.0,
- true,
- CargoNearLeft(),
- /*expected_radius=*/0.0}
- ));
+ /*ball_mode=*/true, drivetrain::SelectionHint_NONE, -1.0,
+ true, CargoNearLeft(),
+ /*expected_radius=*/0.0}));
} // namespace testing
} // namespace control_loops