Change localizer code to send vision pi statuses
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Idc6e050098aab62c15768e531815561acc301aea
diff --git a/frc971/control_loops/drivetrain/localization/localizer_output.fbs b/frc971/control_loops/drivetrain/localization/localizer_output.fbs
index ff25c31..8955b96 100644
--- a/frc971/control_loops/drivetrain/localization/localizer_output.fbs
+++ b/frc971/control_loops/drivetrain/localization/localizer_output.fbs
@@ -37,6 +37,9 @@
// Cumulative number of accepted images.
image_accepted_count:uint (id: 7);
+
+ // Is true if all vision pis are connected to the IMU pi
+ all_pis_connected:bool (id: 8);
}
root_type LocalizerOutput;
diff --git a/y2023/control_loops/superstructure/led_indicator.cc b/y2023/control_loops/superstructure/led_indicator.cc
index 5cee266..d67bdc2 100644
--- a/y2023/control_loops/superstructure/led_indicator.cc
+++ b/y2023/control_loops/superstructure/led_indicator.cc
@@ -49,29 +49,38 @@
static_cast<int>(b));
}
-namespace {
-bool DisconnectedPiServer(
- const aos::message_bridge::ServerStatistics &server_stats) {
- for (const auto *pi_server_status : *server_stats.connections()) {
- if (pi_server_status->state() == aos::message_bridge::State::DISCONNECTED &&
- pi_server_status->node()->name()->string_view() != "logger") {
+bool DisconnectedIMUPiServer(
+ const aos::message_bridge::ServerStatistics &server_statistics) {
+ for (const auto *node_status : *server_statistics.connections()) {
+ if (node_status->state() == aos::message_bridge::State::DISCONNECTED &&
+ node_status->node()->name()->string_view() != "logger") {
return true;
}
}
+
return false;
}
-bool DisconnectedPiClient(
- const aos::message_bridge::ClientStatistics &client_stats) {
- for (const auto *pi_client_status : *client_stats.connections()) {
- if (pi_client_status->state() == aos::message_bridge::State::DISCONNECTED &&
- pi_client_status->node()->name()->string_view() != "logger") {
+bool DisconnectedIMUPiClient(
+ const aos::message_bridge::ClientStatistics &client_statistics) {
+ for (const auto *node_status : *client_statistics.connections()) {
+ if (node_status->state() == aos::message_bridge::State::DISCONNECTED &&
+ node_status->node()->name()->string_view() != "logger") {
return true;
}
}
+
return false;
}
-} // namespace
+
+bool PisDisconnected(
+ const frc971::controls::LocalizerOutput &localizer_output) {
+ if (!localizer_output.all_pis_connected()) {
+ return true;
+ } else {
+ return false;
+ }
+}
void LedIndicator::DecideColor() {
superstructure_status_fetcher_.Fetch();
@@ -122,11 +131,12 @@
return;
}
- // Pi disconnected
- if ((server_statistics_fetcher_.get() &&
- DisconnectedPiServer(*server_statistics_fetcher_)) ||
- (client_statistics_fetcher_.get() &&
- DisconnectedPiClient(*client_statistics_fetcher_))) {
+ if (localizer_output_fetcher_.get() == nullptr ||
+ server_statistics_fetcher_.get() == nullptr ||
+ client_statistics_fetcher_.get() == nullptr ||
+ PisDisconnected(*localizer_output_fetcher_) ||
+ DisconnectedIMUPiServer(*server_statistics_fetcher_) ||
+ DisconnectedIMUPiClient(*client_statistics_fetcher_)) {
if (flash_counter_.Flash()) {
DisplayLed(255, 0, 0);
} else {
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",
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()));
}
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_