diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java index ab01df41bb..54f9d70321 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java @@ -84,6 +84,10 @@ public AprilTagFieldLayout getApriltagFieldLayout() { return atfl; } + public void setApriltagFieldLayout(AprilTagFieldLayout atfl) { + this.atfl = atfl; + } + public void setNetworkConfig(NetworkConfig networkConfig) { this.networkConfig = networkConfig; } diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java index 727dfb04ad..f8a474dacb 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java @@ -17,9 +17,14 @@ package org.photonvision.common.dataflow.networktables; +import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEvent; +import edu.wpi.first.networktables.NetworkTableEvent.Kind; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringSubscriber; +import java.io.IOException; +import java.util.EnumSet; import java.util.HashMap; import java.util.function.Consumer; import org.photonvision.PhotonVersion; @@ -32,17 +37,33 @@ import org.photonvision.common.scripting.ScriptEventType; import org.photonvision.common.scripting.ScriptManager; import org.photonvision.common.util.TimedTaskManager; +import org.photonvision.common.util.file.JacksonUtils; public class NetworkTablesManager { private final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault(); private final String kRootTableName = "/photonvision"; + private final String kFieldLayoutName = "apriltag_field_layout"; public final NetworkTable kRootTable = ntInstance.getTable(kRootTableName); - private boolean isRetryingConnection = false; + private final NTLogger m_ntLogger = new NTLogger(); + + private boolean m_isRetryingConnection = false; + + private StringSubscriber m_fieldLayoutSubscriber = + kRootTable.getStringTopic(kFieldLayoutName).subscribe(""); private NetworkTablesManager() { - ntInstance.addLogger(0, 255, new NTLogger()); // to hide error messages + ntInstance.addLogger(255, 255, (event) -> {}); // to hide error messages + ntInstance.addConnectionListener(true, m_ntLogger); // to hide error messages + + ntInstance.addListener( + m_fieldLayoutSubscriber, EnumSet.of(Kind.kValueAll), this::onFieldLayoutChanged); + TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000); + + // Get the UI state in sync with the backend. NT should fire a callback when it first connects + // to the robot + broadcastConnectedStatus(); } private static NetworkTablesManager INSTANCE; @@ -56,24 +77,33 @@ public static NetworkTablesManager getInstance() { private static class NTLogger implements Consumer { private boolean hasReportedConnectionFailure = false; - private long lastConnectMessageMillis = 0; @Override public void accept(NetworkTableEvent event) { - if (!hasReportedConnectionFailure && event.logMessage.message.contains("timed out")) { - logger.error("NT Connection has failed! Will retry in background."); + var isConnEvent = event.is(Kind.kConnected); + var isDisconnEvent = event.is(Kind.kDisconnected); + + if (!hasReportedConnectionFailure && isDisconnEvent) { + var msg = + String.format( + "NT lost connection to %s:%d! (NT version %d). Will retry in background.", + event.connInfo.remote_ip, + event.connInfo.remote_port, + event.connInfo.protocol_version); + logger.error(msg); + hasReportedConnectionFailure = true; getInstance().broadcastConnectedStatus(); - } else if (event.logMessage.message.contains("connected") - && System.currentTimeMillis() - lastConnectMessageMillis > 125) { - String connectionDescription = "(unknown)"; - var connections = getInstance().ntInstance.getConnections(); - if (connections.length > 0) { - connectionDescription = connections[0].remote_ip + ":" + connections[0].remote_port; - } - logger.info("NT Connected to " + connectionDescription + "!"); + } else if (isConnEvent && event.connInfo != null) { + var msg = + String.format( + "NT connected to %s:%d! (NT version %d)", + event.connInfo.remote_ip, + event.connInfo.remote_port, + event.connInfo.protocol_version); + logger.info(msg); + hasReportedConnectionFailure = false; - lastConnectMessageMillis = System.currentTimeMillis(); ScriptManager.queueEvent(ScriptEventType.kNTConnected); getInstance().broadcastVersion(); getInstance().broadcastConnectedStatus(); @@ -81,6 +111,23 @@ public void accept(NetworkTableEvent event) { } } + private void onFieldLayoutChanged(NetworkTableEvent event) { + var atfl_json = event.valueData.value.getString(); + try { + System.out.println("Got new field layout!"); + var atfl = JacksonUtils.deserialize(atfl_json, AprilTagFieldLayout.class); + ConfigManager.getInstance().getConfig().setApriltagFieldLayout(atfl); + ConfigManager.getInstance().requestSave(); + DataChangeService.getInstance() + .publishEvent( + new OutgoingUIEvent<>( + "fullsettings", ConfigManager.getInstance().getConfig().toHashMap())); + } catch (IOException e) { + logger.error("Error deserializing atfl!"); + logger.error(atfl_json); + } + } + public void broadcastConnectedStatus() { TimedTaskManager.getInstance().addOneShotTask(this::broadcastConnectedStatusImpl, 1000L); } @@ -122,10 +169,10 @@ private void setClientMode(String ntServerAddress) { ntInstance.startClient4("photonvision"); try { Integer t = Integer.parseInt(ntServerAddress); - if (!isRetryingConnection) logger.info("Starting NT Client, server team is " + t); + if (!m_isRetryingConnection) logger.info("Starting NT Client, server team is " + t); ntInstance.setServerTeam(t); } catch (NumberFormatException e) { - if (!isRetryingConnection) + if (!m_isRetryingConnection) logger.info("Starting NT Client, server IP is \"" + ntServerAddress + "\""); ntInstance.setServer(ntServerAddress); } @@ -151,8 +198,8 @@ private void ntTick() { setConfig(ConfigManager.getInstance().getConfig().getNetworkConfig()); } - if (!ntInstance.isConnected() && !isRetryingConnection) { - isRetryingConnection = true; + if (!ntInstance.isConnected() && !m_isRetryingConnection) { + m_isRetryingConnection = true; logger.error( "[NetworkTablesManager] Could not connect to the robot! Will retry in the background..."); }