Add time-since-last-target to server
And draw little cameras on the robot that turn green when we got a
target from that camera in the last 0.25sec.
Change-Id: Icd6107163fd3f5145fcba69ac6ebb2700f602f8d
diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index 54eb399..235054b 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -125,6 +125,8 @@
auto &drivetrain_status = frc971::control_loops::drivetrain_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();
while (true) {
@@ -134,6 +136,7 @@
// Try again if we don't have any drivetrain statuses.
continue;
}
+ const auto now = aos::monotonic_clock::now();
{
const auto &new_frame = *camera_frames;
@@ -142,6 +145,9 @@
aos::monotonic_clock::time_point(
std::chrono::nanoseconds(new_frame.timestamp));
latest_frames[new_frame.camera].targets.clear();
+ if (new_frame.num_targets > 0) {
+ last_target_time[new_frame.camera] = now;
+ }
for (int target = 0; target < new_frame.num_targets; ++target) {
latest_frames[new_frame.camera].targets.emplace_back();
// TODO: Do something useful.
@@ -149,8 +155,9 @@
}
}
- const auto now = aos::monotonic_clock::now();
if (now > last_send_time + std::chrono::milliseconds(100)) {
+ // TODO(james): Use protobuf or the such to generate JSON rather than
+ // doing so manually.
last_send_time = now;
std::ostringstream stream;
stream << "{\n";
@@ -171,6 +178,18 @@
<< drivetrain_status->line_follow_logging.have_target;
stream << "} ";
+ stream << ", \"last_target_age\": [";
+ bool first = true;
+ for (const auto t : last_target_time) {
+ if (!first) {
+ stream << ",";
+ }
+ first = false;
+ stream << ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ now - t).count();
+ }
+ stream << "]";
+
stream << "}";
server->execute(
std::make_shared<UpdateData>(websocket_handler, stream.str()));