Convert vision_status to event loops
Change-Id: I06c329f3fe42d2dbfa7da90d598c93eacdf746c7
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index d94e34d..d0ff71d 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -30,6 +30,9 @@
name),
status_light_sender_(
event_loop->MakeSender<::y2018::StatusLight>(".y2018.status_light")),
+ vision_status_fetcher_(
+ event_loop->MakeFetcher<::y2018::vision::VisionStatus>(
+ ".y2018.vision.vision_status")),
intake_left_(constants::GetValues().left_intake.zeroing),
intake_right_(constants::GetValues().right_intake.zeroing) {}
@@ -255,11 +258,13 @@
::frc971::control_loops::drivetrain_queue.output.FetchLatest();
- ::y2018::vision::vision_status.FetchLatest();
+ vision_status_fetcher_.Fetch();
+ monotonic_clock::time_point monotonic_now = event_loop()->monotonic_now();
if (status->estopped) {
SendColors(0.5, 0.0, 0.0);
- } else if (!y2018::vision::vision_status.get() ||
- y2018::vision::vision_status.Age() > chrono::seconds(1)) {
+ } else if (!vision_status_fetcher_.get() ||
+ monotonic_now >
+ vision_status_fetcher_->sent_time + chrono::seconds(1)) {
SendColors(0.5, 0.5, 0.0);
} else if (rotation_state_ == RotationState::ROTATING_LEFT ||
rotation_state_ == RotationState::ROTATING_RIGHT) {
diff --git a/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
index 8ab6bf3..bceb643 100644
--- a/y2018/control_loops/superstructure/superstructure.h
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -10,6 +10,7 @@
#include "y2018/control_loops/superstructure/intake/intake.h"
#include "y2018/control_loops/superstructure/superstructure.q.h"
#include "y2018/status_light.q.h"
+#include "y2018/vision/vision.q.h"
namespace y2018 {
namespace control_loops {
@@ -38,6 +39,7 @@
void SendColors(float red, float green, float blue);
::aos::Sender<::y2018::StatusLight> status_light_sender_;
+ ::aos::Fetcher<::y2018::vision::VisionStatus> vision_status_fetcher_;
intake::IntakeSide intake_left_;
intake::IntakeSide intake_right_;
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 5fa4abe..8533dce 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -274,7 +274,6 @@
".y2018.control_loops.superstructure.status",
".y2018.control_loops.superstructure.position"),
superstructure_(&event_loop_, ".y2018.control_loops.superstructure") {
- ::y2018::vision::vision_status.Clear();
::frc971::control_loops::drivetrain_queue.output.Clear();
set_team_id(::frc971::control_loops::testing::kTeamNumber);
}
diff --git a/y2018/vision/BUILD b/y2018/vision/BUILD
index b385784..8e5e950 100644
--- a/y2018/vision/BUILD
+++ b/y2018/vision/BUILD
@@ -34,6 +34,7 @@
deps = [
":vision_queue",
"//aos:init",
+ "//aos/events:shm-event-loop",
"//aos/logging",
"//aos/logging:queue_logging",
"//aos/time",
diff --git a/y2018/vision/vision.q b/y2018/vision/vision.q
index a3b57ea..fb59d69 100644
--- a/y2018/vision/vision.q
+++ b/y2018/vision/vision.q
@@ -1,7 +1,7 @@
package y2018.vision;
+// Published on ".y2018.vision.vision_status"
message VisionStatus {
uint32_t high_frame_count;
uint32_t low_frame_count;
};
-queue VisionStatus vision_status;
diff --git a/y2018/vision/vision_status.cc b/y2018/vision/vision_status.cc
index cfdae2e..3d3faf5 100644
--- a/y2018/vision/vision_status.cc
+++ b/y2018/vision/vision_status.cc
@@ -1,9 +1,10 @@
#include <netdb.h>
+#include "aos/events/shm-event-loop.h"
+#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
#include "aos/time/time.h"
-#include "aos/init.h"
#include "aos/vision/events/udp.h"
#include "y2018/vision.pb.h"
#include "y2018/vision/vision.q.h"
@@ -17,12 +18,16 @@
::aos::events::RXUdpSocket video_rx(5001);
char data[65507];
::y2018::VisionStatus status;
+ ::aos::ShmEventLoop event_loop;
+ ::aos::Sender<::y2018::vision::VisionStatus> vision_status_sender_ =
+ event_loop.MakeSender<::y2018::vision::VisionStatus>(
+ ".y2018.vision.vision_status");
while (true) {
const ssize_t rx_size = video_rx.Recv(data, sizeof(data));
if (rx_size > 0) {
status.ParseFromArray(data, rx_size);
- auto new_vision_status = vision_status.MakeMessage();
+ auto new_vision_status = vision_status_sender_.MakeMessage();
new_vision_status->high_frame_count = status.high_frame_count();
new_vision_status->low_frame_count = status.low_frame_count();
LOG_STRUCT(DEBUG, "vision", *new_vision_status);
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index f412846..d6bda66 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -395,6 +395,9 @@
".y2018.control_loops.superstructure_queue.output")),
status_light_fetcher_(event_loop->MakeFetcher<::y2018::StatusLight>(
".y2018.status_light")),
+ vision_status_fetcher_(
+ event_loop->MakeFetcher<::y2018::vision::VisionStatus>(
+ ".y2018.vision.vision_status")),
pcm_(pcm) {}
// left drive
@@ -473,23 +476,25 @@
LOG_STRUCT(DEBUG, "pneumatics info", to_log);
}
+ monotonic_clock::time_point monotonic_now = event_loop_->monotonic_now();
status_light_fetcher_.Fetch();
// If we don't have a light request (or it's an old one), we are borked.
// Flash the red light slowly.
if (!status_light_fetcher_.get() ||
- event_loop_->monotonic_now() >
+ monotonic_now >
status_light_fetcher_->sent_time + chrono::milliseconds(100)) {
StatusLight color;
color.red = 0.0;
color.green = 0.0;
color.blue = 0.0;
- ::y2018::vision::vision_status.FetchLatest();
+ vision_status_fetcher_.Fetch();
++light_flash_;
if (light_flash_ > 10) {
color.red = 0.5;
- } else if (!y2018::vision::vision_status.get() ||
- y2018::vision::vision_status.Age() > chrono::seconds(1)) {
+ } else if (!vision_status_fetcher_.get() ||
+ monotonic_now >
+ vision_status_fetcher_->sent_time + chrono::seconds(1)) {
color.red = 0.5;
color.green = 0.5;
}
@@ -543,6 +548,7 @@
::aos::Fetcher<::y2018::control_loops::SuperstructureQueue::Output>
superstructure_fetcher_;
::aos::Fetcher<::y2018::StatusLight> status_light_fetcher_;
+ ::aos::Fetcher<::y2018::vision::VisionStatus> vision_status_fetcher_;
::frc971::wpilib::BufferedPcm *pcm_;