From a151f23319bb10332a36cc6063753b588fbed78f Mon Sep 17 00:00:00 2001 From: Matt Date: Thu, 25 Nov 2021 15:43:29 -0500 Subject: [PATCH] Add team number dialog, NT connected chip (#313) Makes NT connection status visible from the UI --- photon-client/src/App.vue | 106 ++++++++++++++---- photon-client/src/assets/robot-off.svg | 1 + photon-client/src/assets/robot.svg | 1 + photon-client/src/store/index.js | 9 ++ photon-client/src/views/PipelineView.vue | 16 ++- .../src/views/SettingsViews/Networking.vue | 89 ++++++++++++++- .../networktables/NetworkTablesManager.java | 105 ++++++++++++++++- .../vision/processes/VisionModule.java | 1 + .../server/UIInboundSubscriber.java | 2 + 9 files changed, 302 insertions(+), 28 deletions(-) create mode 100644 photon-client/src/assets/robot-off.svg create mode 100644 photon-client/src/assets/robot.svg diff --git a/photon-client/src/App.vue b/photon-client/src/App.vue index fb7a938706..a11ef97744 100644 --- a/photon-client/src/App.vue +++ b/photon-client/src/App.vue @@ -91,25 +91,51 @@ - - - - mdi-wifi - - - mdi-wifi-off - - - - - {{ $store.state.backendConnected ? "Connected" : "Trying to connect..." }} - - - +
+ + + mdi-server + + + + + + NetworkTables server running for {{$store.state.ntConnectionInfo.clients ? $store.state.ntConnectionInfo.clients : 'zero'}} clients! + + + Robot connected! {{$store.state.ntConnectionInfo.address}} + + + Not connected to robot! + + {{"Team: " + $store.state.settings.networkSettings.teamNumber}} + + + + + + + mdi-wifi + + + mdi-wifi-off + + + + + {{ $store.state.backendConnected ? "Backend Connected" : "Trying to connect..." }} + + + +
+ @@ -132,13 +158,35 @@ > + + + No team number set! + + PhotonVision cannot connect to your robot! Please + head to the settings page and set your team number. + + + - 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 e533e14183..d5cafa544c 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 @@ -16,16 +16,28 @@ */ package org.photonvision.common.dataflow.networktables; +import edu.wpi.first.cscore.CameraServerJNI; import edu.wpi.first.networktables.LogMessage; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import java.io.IOException; +import java.net.InetAddress; +import java.net.UnknownHostException; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.HashMap; +import java.util.List; import java.util.function.Consumer; import org.photonvision.PhotonVersion; +import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.configuration.NetworkConfig; +import org.photonvision.common.dataflow.DataChangeService; +import org.photonvision.common.dataflow.events.OutgoingUIEvent; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.scripting.ScriptEventType; import org.photonvision.common.scripting.ScriptManager; +import org.photonvision.common.util.TimedTaskManager; public class NetworkTablesManager { private final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault(); @@ -54,6 +66,7 @@ public void accept(LogMessage logMessage) { if (!hasReportedConnectionFailure && logMessage.message.contains("timed out")) { logger.error("NT Connection has failed! Will retry in background."); hasReportedConnectionFailure = true; + getInstance().broadcastConnectedStatus(); } else if (logMessage.message.contains("connected") && System.currentTimeMillis() - lastConnectMessageMillis > 125) { logger.info("NT Connected!"); @@ -61,10 +74,99 @@ public void accept(LogMessage logMessage) { lastConnectMessageMillis = System.currentTimeMillis(); ScriptManager.queueEvent(ScriptEventType.kNTConnected); getInstance().broadcastVersion(); + getInstance().broadcastConnectedStatus(); } } } + public void broadcastConnectedStatus() { + TimedTaskManager.getInstance().addOneShotTask(this::broadcastConnectedStatusImpl, 1000L); + } + + private void broadcastConnectedStatusImpl() { + HashMap map = new HashMap<>(); + var subMap = new HashMap(); + subMap.put("connected", ntInstance.isConnected()); + if (ntInstance.isConnected()) { + var connections = getInstance().ntInstance.getConnections(); + if (connections.length > 0) { + subMap.put("address", connections[0].remote_ip + ":" + connections[0].remote_port); + } + subMap.put("clients", connections.length); + } + + map.put("ntConnectionInfo", subMap); + DataChangeService.getInstance() + .publishEvent(new OutgoingUIEvent<>("networkTablesConnected", map)); + + // Seperate from the above so we don't hold stuff up + System.setProperty("java.net.preferIPv4Stack", "true"); + subMap.put( + "deviceips", + Arrays.stream(CameraServerJNI.getNetworkInterfaces()) + .filter(it -> !it.equals("0.0.0.0")) + .toArray()); + logger.info("Searching for rios"); + List possibleRioList = new ArrayList<>(); + for (var ip : CameraServerJNI.getNetworkInterfaces()) { + logger.info("Trying " + ip); + var possibleRioAddr = getPossibleRioAddress(ip); + if (possibleRioAddr != null) { + logger.info("Maybe found " + ip); + searchForHost(possibleRioList, possibleRioAddr); + } else { + logger.info("Didn't match RIO IP"); + } + } + String name = + "roboRIO-" + + ConfigManager.getInstance().getConfig().getNetworkConfig().teamNumber + + "-FRC.local"; + searchForHost(possibleRioList, name); + name = + "roboRIO-" + + ConfigManager.getInstance().getConfig().getNetworkConfig().teamNumber + + "-FRC.lan"; + searchForHost(possibleRioList, name); + name = + "roboRIO-" + + ConfigManager.getInstance().getConfig().getNetworkConfig().teamNumber + + "-FRC.frc-field.local"; + searchForHost(possibleRioList, name); + subMap.put("possibleRios", possibleRioList.toArray()); + DataChangeService.getInstance() + .publishEvent(new OutgoingUIEvent<>("networkTablesConnected", map)); + } + + String getPossibleRioAddress(String ip) { + try { + InetAddress addr = InetAddress.getByName(ip); + var address = addr.getAddress(); + if (address[0] != (byte) (10 & 0xff)) return null; + address[3] = (byte) (2 & 0xff); + return InetAddress.getByAddress(address).getHostAddress(); + } catch (UnknownHostException e) { + e.printStackTrace(); + } + return null; + } + + void searchForHost(List list, String hostname) { + try { + logger.info("Looking up " + hostname); + InetAddress testAddr = InetAddress.getByName(hostname); + logger.info("Pinging " + hostname); + var canContact = testAddr.isReachable(500); + if (canContact) { + logger.info("Was able to connect to " + hostname); + if (!list.contains(hostname)) list.add(hostname); + } else { + logger.info("Unable to reach " + hostname); + } + } catch (IOException ignored) { + } + } + private void broadcastVersion() { kRootTable.getEntry("version").setString(PhotonVersion.versionString); kRootTable.getEntry("buildDate").setString(PhotonVersion.buildDate); @@ -83,7 +185,8 @@ private void setClientMode(int teamNumber) { logger.info("Starting NT Client"); ntInstance.stopServer(); - ntInstance.startClientTeam(teamNumber); + // ntInstance.startClientTeam(teamNumber); + ntInstance.startClient("localhost"); ntInstance.startDSClient(); if (ntInstance.isConnected()) { logger.info("[NetworkTablesManager] Connected to the robot!"); diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index 5fe6a8a260..b281cc13c9 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -369,6 +369,7 @@ void setPipeline(int index) { visionSource.getSettables().setVideoModeInternal(config.cameraVideoModeIndex); visionSource.getSettables().setBrightness(config.cameraBrightness); visionSource.getSettables().setExposure(config.cameraExposure); + visionSource.getSettables().setGain(config.cameraGain); if (!cameraQuirks.hasQuirk(CameraQuirk.Gain)) { config.cameraGain = -1; diff --git a/photon-server/src/main/java/org/photonvision/server/UIInboundSubscriber.java b/photon-server/src/main/java/org/photonvision/server/UIInboundSubscriber.java index 14d1474854..a8c3e7869d 100644 --- a/photon-server/src/main/java/org/photonvision/server/UIInboundSubscriber.java +++ b/photon-server/src/main/java/org/photonvision/server/UIInboundSubscriber.java @@ -25,6 +25,7 @@ import org.photonvision.common.dataflow.events.DataChangeEvent; import org.photonvision.common.dataflow.events.IncomingWebSocketEvent; import org.photonvision.common.dataflow.events.OutgoingUIEvent; +import org.photonvision.common.dataflow.networktables.NetworkTablesManager; import org.photonvision.common.logging.Logger; public class UIInboundSubscriber extends DataChangeSubscriber { @@ -46,6 +47,7 @@ public void onDataChangeEvent(DataChangeEvent event) { new OutgoingUIEvent<>("fullsettings", settings, incomingWSEvent.originContext); DataChangeService.getInstance().publishEvent(message); Logger.sendConnectedBacklog(); + NetworkTablesManager.getInstance().broadcastConnectedStatus(); } } }