commit 85d8187a2444d2db9971153b7d683fcbd8b6ea6b
author Maxwell Henderson <mxwhenderson@gmail.com> Fri Apr 07 18:21:59 2023 -0700
committer Maxwell Henderson <mxwhenderson@gmail.com> Sat Apr 08 20:21:56 2023 -0700
tree 8c5e98e1e0d3e9459e1fd296ddb8d18a76b7ce83
parent 40344e277eef059c41783e9c2904296e370b4722

Change localizer code to send vision pi statuses

Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Idc6e050098aab62c15768e531815561acc301aea

y2023/localizer/localizer.cc

diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index c663954..502fbde 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -112,7 +112,13 @@
       utils_(event_loop),
       status_sender_(event_loop->MakeSender<Status>("/localizer")),
       output_sender_(event_loop->MakeSender<frc971::controls::LocalizerOutput>(
-          "/localizer")) {
+          "/localizer")),
+      server_statistics_fetcher_(
+          event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
+              "/aos")),
+      client_statistics_fetcher_(
+          event_loop_->MakeFetcher<aos::message_bridge::ClientStatistics>(
+              "/aos")) {
   if (dt_config_.is_simulated) {
     down_estimator_.assume_perfect_gravity();
   }
@@ -451,6 +457,34 @@
   quaternion.mutate_z(orientation.z());
   quaternion.mutate_w(orientation.w());
   output_builder.add_orientation(&quaternion);
+  server_statistics_fetcher_.Fetch();
+  client_statistics_fetcher_.Fetch();
+
+  bool pis_connected = true;
+
+  if (server_statistics_fetcher_.get()) {
+    for (const auto *pi_server_status :
+         *server_statistics_fetcher_->connections()) {
+      if (pi_server_status->state() ==
+              aos::message_bridge::State::DISCONNECTED &&
+          pi_server_status->node()->name()->string_view() != "logger") {
+        pis_connected = false;
+      }
+    }
+  }
+
+  if (client_statistics_fetcher_.get()) {
+    for (const auto *pi_client_status :
+         *client_statistics_fetcher_->connections()) {
+      if (pi_client_status->state() ==
+              aos::message_bridge::State::DISCONNECTED &&
+          pi_client_status->node()->name()->string_view() != "logger") {
+        pis_connected = false;
+      }
+    }
+  }
+
+  output_builder.add_all_pis_connected(pis_connected);
   builder.CheckOk(builder.Send(output_builder.Finish()));
 }