Kill camera_frames queue
Use EventLoop instead.
Change-Id: I39352fecbddd677a32de1a0e77ac11947bc76c6f
diff --git a/y2019/BUILD b/y2019/BUILD
index c49e7c4..792844f 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -56,6 +56,7 @@
"//aos:make_unique",
"//aos:math",
"//aos/controls:control_loop",
+ "//aos/events:shm-event-loop",
"//aos/logging",
"//aos/logging:queue_logging",
"//aos/robot_state",
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 49cda13..8cfa45e 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -203,6 +203,7 @@
":event_loop_localizer",
":localizer",
"//aos/controls:control_loop_test",
+ "//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.q b/y2019/control_loops/drivetrain/camera.q
index 6ef6f49..9d5dffc 100644
--- a/y2019/control_loops/drivetrain/camera.q
+++ b/y2019/control_loops/drivetrain/camera.q
@@ -8,6 +8,8 @@
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.
@@ -22,5 +24,3 @@
// Index of the camera position (not serial number) which this frame is from.
uint8_t camera;
};
-
-queue CameraFrame camera_frames;
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index 75a8af3..3052b5b 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -48,7 +48,7 @@
".frc971.control_loops.drivetrain_queue.position",
".frc971.control_loops.drivetrain_queue.output",
".frc971.control_loops.drivetrain_queue.status"),
- camera_queue_(::y2019::control_loops::drivetrain::camera_frames.name()),
+ camera_queue_(".y2019.control_loops.drivetrain.camera_frames"),
localizer_(dt_config_, &event_loop_),
drivetrain_motor_(dt_config_, &event_loop_, &localizer_),
drivetrain_motor_plant_(&simulation_event_loop_, dt_config_),
@@ -60,7 +60,6 @@
// is just prone to causing confusion.
SetStartingPosition({3.0, 2.0, 0.0});
::frc971::sensors::gyro_reading.Clear();
- ::y2019::control_loops::drivetrain::camera_frames.Clear();
set_battery_voltage(12.0);
}
@@ -168,7 +167,6 @@
virtual ~LocalizedDrivetrainTest() {
::frc971::sensors::gyro_reading.Clear();
- ::y2019::control_loops::drivetrain::camera_frames.Clear();
}
const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
diff --git a/y2019/vision/server/BUILD b/y2019/vision/server/BUILD
index 01260e1..3600194 100644
--- a/y2019/vision/server/BUILD
+++ b/y2019/vision/server/BUILD
@@ -49,6 +49,7 @@
":server_data_proto",
"//aos:init",
"//aos/containers:ring_buffer",
+ "//aos/events:shm-event-loop",
"//aos/logging",
"//aos/seasocks:seasocks_logger",
"//aos/time",
diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index 4d00cdf..44a7c6f 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -134,126 +134,148 @@
}
void DataThread(seasocks::Server *server, WebsocketHandler *websocket_handler) {
- auto &camera_frames = y2019::control_loops::drivetrain::camera_frames;
- auto &drivetrain_status = frc971::control_loops::drivetrain_queue.status;
- auto &superstructure_status =
- ::y2019::control_loops::superstructure::superstructure_queue.status;
+ ::aos::ShmEventLoop event_loop;
- std::array<LocalCameraFrame, 5> latest_frames;
- std::array<aos::monotonic_clock::time_point, 5> last_target_time;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher =
+ event_loop
+ .MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status");
+
+ ::aos::Fetcher<
+ ::y2019::control_loops::superstructure::SuperstructureQueue::Status>
+ superstructure_status_fetcher = event_loop.MakeFetcher<
+ ::y2019::control_loops::superstructure::SuperstructureQueue::Status>(
+ ".y2019.control_loops.superstructure.superstructure_queue.status");
+
+ ::std::array<LocalCameraFrame, 5> latest_frames;
+ ::std::array<aos::monotonic_clock::time_point, 5> last_target_time;
last_target_time.fill(aos::monotonic_clock::epoch());
aos::monotonic_clock::time_point last_send_time = aos::monotonic_clock::now();
DebugData debug_data;
::aos::RingBuffer<DrivetrainPosition, 200> drivetrain_log;
- while (true) {
- camera_frames.FetchNextBlocking();
- while (drivetrain_status.FetchNext()) {
- DrivetrainPosition drivetrain_position{
- drivetrain_status->sent_time, drivetrain_status->x,
- drivetrain_status->y, drivetrain_status->theta};
+ event_loop.MakeWatcher(
+ ".y2019.control_loops.drivetrain.camera_frames",
+ [websocket_handler, server, &latest_frames, &last_target_time,
+ &drivetrain_status_fetcher, &superstructure_status_fetcher,
+ &last_send_time, &drivetrain_log,
+ &debug_data](const ::y2019::control_loops::drivetrain::CameraFrame
+ &camera_frames) {
+ while (drivetrain_status_fetcher.FetchNext()) {
+ DrivetrainPosition drivetrain_position{
+ drivetrain_status_fetcher->sent_time,
+ drivetrain_status_fetcher->x, drivetrain_status_fetcher->y,
+ drivetrain_status_fetcher->theta};
- drivetrain_log.Push(drivetrain_position);
- }
- superstructure_status.FetchLatest();
- if (!drivetrain_status.get() || !superstructure_status.get()) {
- // Try again if we don't have any drivetrain statuses.
- continue;
- }
- const auto now = aos::monotonic_clock::now();
-
- {
- const auto &new_frame = *camera_frames;
- // TODO(james): Maybe we only want to fill out a new frame if it has
- // targets or the saved target is > 0.1 sec old? Not sure, but for now
- if (new_frame.camera < latest_frames.size()) {
- latest_frames[new_frame.camera].capture_time =
- aos::monotonic_clock::time_point(
- chrono::nanoseconds(new_frame.timestamp));
- latest_frames[new_frame.camera].targets.clear();
- if (new_frame.num_targets > 0) {
- last_target_time[new_frame.camera] =
- latest_frames[new_frame.camera].capture_time;
+ drivetrain_log.Push(drivetrain_position);
}
- for (int target = 0; target < new_frame.num_targets; ++target) {
- latest_frames[new_frame.camera].targets.emplace_back();
- const float heading = new_frame.targets[target].heading;
- const float distance = new_frame.targets[target].distance;
- latest_frames[new_frame.camera].targets.back().x =
- ::std::cos(heading) * distance;
- latest_frames[new_frame.camera].targets.back().y =
- ::std::sin(heading) * distance;
- latest_frames[new_frame.camera].targets.back().theta =
- new_frame.targets[target].skew;
+ superstructure_status_fetcher.Fetch();
+ if (!drivetrain_status_fetcher.get() ||
+ !superstructure_status_fetcher.get()) {
+ // Try again if we don't have any drivetrain statuses.
+ return;
}
- }
- }
+ const auto now = aos::monotonic_clock::now();
- for (size_t ii = 0; ii < latest_frames.size(); ++ii) {
- CameraDebug *camera_debug = debug_data.add_camera_debug();
- LocalCameraFrame cur_frame = latest_frames[ii];
- constants::Values::CameraCalibration camera_info =
- constants::GetValues().cameras[ii];
- frc971::control_loops::TypedPose<double> camera_pose = camera_info.pose;
+ {
+ const auto &new_frame = camera_frames;
+ // TODO(james): Maybe we only want to fill out a new frame if it has
+ // targets or the saved target is > 0.1 sec old? Not sure, but for now
+ if (new_frame.camera < latest_frames.size()) {
+ latest_frames[new_frame.camera].capture_time =
+ aos::monotonic_clock::time_point(
+ chrono::nanoseconds(new_frame.timestamp));
+ latest_frames[new_frame.camera].targets.clear();
+ if (new_frame.num_targets > 0) {
+ last_target_time[new_frame.camera] =
+ latest_frames[new_frame.camera].capture_time;
+ }
+ for (int target = 0; target < new_frame.num_targets; ++target) {
+ latest_frames[new_frame.camera].targets.emplace_back();
+ const float heading = new_frame.targets[target].heading;
+ const float distance = new_frame.targets[target].distance;
+ latest_frames[new_frame.camera].targets.back().x =
+ ::std::cos(heading) * distance;
+ latest_frames[new_frame.camera].targets.back().y =
+ ::std::sin(heading) * distance;
+ latest_frames[new_frame.camera].targets.back().theta =
+ new_frame.targets[target].skew;
+ }
+ }
+ }
- const DrivetrainPosition robot_position = ComputePosition(
- drivetrain_log, cur_frame.capture_time);
- const ::frc971::control_loops::TypedPose<double> robot_pose(
- {robot_position.x, robot_position.y, 0}, robot_position.theta);
+ for (size_t ii = 0; ii < latest_frames.size(); ++ii) {
+ CameraDebug *camera_debug = debug_data.add_camera_debug();
+ LocalCameraFrame cur_frame = latest_frames[ii];
+ constants::Values::CameraCalibration camera_info =
+ constants::GetValues().cameras[ii];
+ frc971::control_loops::TypedPose<double> camera_pose =
+ camera_info.pose;
- camera_pose.set_base(&robot_pose);
+ const DrivetrainPosition robot_position =
+ ComputePosition(drivetrain_log, cur_frame.capture_time);
+ const ::frc971::control_loops::TypedPose<double> robot_pose(
+ {robot_position.x, robot_position.y, 0}, robot_position.theta);
- camera_debug->set_current_frame_age(
- chrono::duration_cast<chrono::duration<double>>(
- now - cur_frame.capture_time)
- .count());
- camera_debug->set_time_since_last_target(
- chrono::duration_cast<chrono::duration<double>>(now -
- last_target_time[ii])
- .count());
- for (const auto &target : cur_frame.targets) {
- frc971::control_loops::TypedPose<double> target_pose(
- &camera_pose, {target.x, target.y, 0}, target.theta);
- Pose *pose = camera_debug->add_targets();
- pose->set_x(target_pose.abs_pos().x());
- pose->set_y(target_pose.abs_pos().y());
- pose->set_theta(target_pose.abs_theta());
- }
- }
+ camera_pose.set_base(&robot_pose);
- if (now > last_send_time + chrono::milliseconds(100)) {
- last_send_time = now;
- debug_data.mutable_robot_pose()->set_x(drivetrain_status->x);
- debug_data.mutable_robot_pose()->set_y(drivetrain_status->y);
- debug_data.mutable_robot_pose()->set_theta(drivetrain_status->theta);
- {
- LineFollowDebug *line_debug = debug_data.mutable_line_follow_debug();
- line_debug->set_frozen(drivetrain_status->line_follow_logging.frozen);
- line_debug->set_have_target(
- drivetrain_status->line_follow_logging.have_target);
- line_debug->mutable_goal_target()->set_x(
- drivetrain_status->line_follow_logging.x);
- line_debug->mutable_goal_target()->set_y(
- drivetrain_status->line_follow_logging.y);
- line_debug->mutable_goal_target()->set_theta(
- drivetrain_status->line_follow_logging.theta);
- }
+ camera_debug->set_current_frame_age(
+ chrono::duration_cast<chrono::duration<double>>(
+ now - cur_frame.capture_time)
+ .count());
+ camera_debug->set_time_since_last_target(
+ chrono::duration_cast<chrono::duration<double>>(
+ now - last_target_time[ii])
+ .count());
+ for (const auto &target : cur_frame.targets) {
+ frc971::control_loops::TypedPose<double> target_pose(
+ &camera_pose, {target.x, target.y, 0}, target.theta);
+ Pose *pose = camera_debug->add_targets();
+ pose->set_x(target_pose.abs_pos().x());
+ pose->set_y(target_pose.abs_pos().y());
+ pose->set_theta(target_pose.abs_theta());
+ }
+ }
- Sensors *sensors = debug_data.mutable_sensors();
- sensors->set_wrist(superstructure_status->wrist.position);
- sensors->set_elevator(superstructure_status->elevator.position);
- sensors->set_intake(superstructure_status->intake.position);
- sensors->set_stilts(superstructure_status->stilts.position);
- sensors->set_has_piece(superstructure_status->has_piece);
+ if (now > last_send_time + chrono::milliseconds(100)) {
+ last_send_time = now;
+ debug_data.mutable_robot_pose()->set_x(drivetrain_status_fetcher->x);
+ debug_data.mutable_robot_pose()->set_y(drivetrain_status_fetcher->y);
+ debug_data.mutable_robot_pose()->set_theta(
+ drivetrain_status_fetcher->theta);
+ {
+ LineFollowDebug *line_debug =
+ debug_data.mutable_line_follow_debug();
+ line_debug->set_frozen(
+ drivetrain_status_fetcher->line_follow_logging.frozen);
+ line_debug->set_have_target(
+ drivetrain_status_fetcher->line_follow_logging.have_target);
+ line_debug->mutable_goal_target()->set_x(
+ drivetrain_status_fetcher->line_follow_logging.x);
+ line_debug->mutable_goal_target()->set_y(
+ drivetrain_status_fetcher->line_follow_logging.y);
+ line_debug->mutable_goal_target()->set_theta(
+ drivetrain_status_fetcher->line_follow_logging.theta);
+ }
- ::std::string json;
- google::protobuf::util::MessageToJsonString(debug_data, &json);
- server->execute(
- std::make_shared<UpdateData>(websocket_handler, ::std::move(json)));
+ Sensors *sensors = debug_data.mutable_sensors();
+ sensors->set_wrist(superstructure_status_fetcher->wrist.position);
+ sensors->set_elevator(
+ superstructure_status_fetcher->elevator.position);
+ sensors->set_intake(superstructure_status_fetcher->intake.position);
+ sensors->set_stilts(superstructure_status_fetcher->stilts.position);
+ sensors->set_has_piece(superstructure_status_fetcher->has_piece);
- debug_data.Clear();
- }
- }
+ ::std::string json;
+ google::protobuf::util::MessageToJsonString(debug_data, &json);
+ server->execute(std::make_shared<UpdateData>(websocket_handler,
+ ::std::move(json)));
+
+ debug_data.Clear();
+ }
+ });
+ event_loop.Run();
}
} // namespace vision
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 29f110a..b910a7f 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -20,6 +20,7 @@
#undef ERROR
#include "aos/commonmath.h"
+#include "aos/events/event-loop.h"
#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
@@ -307,7 +308,12 @@
class CameraReader {
public:
- CameraReader() = default;
+ CameraReader(::aos::EventLoop *event_loop)
+ : camera_frame_sender_(
+ event_loop
+ ->MakeSender<::y2019::control_loops::drivetrain::CameraFrame>(
+ ".y2019.control_loops.drivetrain.camera_frames")) {}
+
CameraReader(const CameraReader &) = delete;
CameraReader &operator=(const CameraReader &) = delete;
@@ -366,7 +372,7 @@
const auto now = aos::monotonic_clock::now();
for (const auto &received : unpacked->frames) {
- auto to_send = control_loops::drivetrain::camera_frames.MakeMessage();
+ auto to_send = camera_frame_sender_.MakeMessage();
// Add an extra 10ms delay to account for unmodeled delays that Austin
// thinks exists.
to_send->timestamp =
@@ -420,6 +426,9 @@
}
private:
+ ::aos::Sender<::y2019::control_loops::drivetrain::CameraFrame>
+ camera_frame_sender_;
+
frc::SPI *spi_ = nullptr;
::std::unique_ptr<frc::SPI> dummy_spi_;
@@ -709,7 +718,7 @@
::std::thread reader_thread(::std::ref(reader));
- CameraReader camera_reader;
+ CameraReader camera_reader(&event_loop);
frc::SPI camera_spi(frc::SPI::Port::kOnboardCS3);
camera_reader.set_spi(&camera_spi);
camera_reader.SetDummySPI(frc::SPI::Port::kOnboardCS2);