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/BUILD

diff --git a/y2023/localizer/BUILD b/y2023/localizer/BUILD
index 43c4481..3ef024c 100644
--- a/y2023/localizer/BUILD
+++ b/y2023/localizer/BUILD
@@ -146,6 +146,7 @@
         ":visualization_fbs",
         "//aos/containers:sized_array",
         "//aos/events:event_loop",
+        "//aos/network:message_bridge_client_fbs",
         "//frc971/constants:constants_sender_lib",
         "//frc971/control_loops:pose",
         "//frc971/control_loops/drivetrain:hybrid_ekf",

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()));
 }
 

y2023/localizer/localizer.h

diff --git a/y2023/localizer/localizer.h b/y2023/localizer/localizer.h
index 4b0715b..5190544 100644
--- a/y2023/localizer/localizer.h
+++ b/y2023/localizer/localizer.h
@@ -4,6 +4,8 @@
 #include <array>
 #include <map>
 
+#include "aos/network/message_bridge_client_generated.h"
+#include "aos/network/message_bridge_server_generated.h"
 #include "frc971/constants/constants_sender_lib.h"
 #include "frc971/control_loops/drivetrain/hybrid_ekf.h"
 #include "frc971/control_loops/drivetrain/improved_down_estimator.h"
@@ -117,6 +119,11 @@
 
   // For the status message.
   std::optional<Eigen::Vector2d> last_encoder_readings_;
+
+  aos::Fetcher<aos::message_bridge::ServerStatistics>
+      server_statistics_fetcher_;
+  aos::Fetcher<aos::message_bridge::ClientStatistics>
+      client_statistics_fetcher_;
 };
 }  // namespace y2023::localizer
 #endif  // Y2023_LOCALIZER_LOCALIZER_H_