19
19
20
20
import edu .wpi .first .apriltag .AprilTagFieldLayout ;
21
21
import edu .wpi .first .networktables .LogMessage ;
22
+ import edu .wpi .first .networktables .MultiSubscriber ;
22
23
import edu .wpi .first .networktables .NetworkTable ;
23
24
import edu .wpi .first .networktables .NetworkTableEvent ;
24
25
import edu .wpi .first .networktables .NetworkTableEvent .Kind ;
25
26
import edu .wpi .first .networktables .NetworkTableInstance ;
26
27
import edu .wpi .first .networktables .StringSubscriber ;
28
+ import edu .wpi .first .wpilibj .Alert ;
29
+ import edu .wpi .first .wpilibj .Alert .AlertType ;
30
+ import edu .wpi .first .wpilibj .smartdashboard .SmartDashboard ;
27
31
import java .io .IOException ;
32
+ import java .util .Arrays ;
28
33
import java .util .EnumSet ;
29
34
import java .util .HashMap ;
30
35
import org .photonvision .PhotonVersion ;
36
+ import org .photonvision .common .configuration .CameraConfiguration ;
31
37
import org .photonvision .common .configuration .ConfigManager ;
32
38
import org .photonvision .common .configuration .NetworkConfig ;
33
39
import org .photonvision .common .dataflow .DataChangeService ;
37
43
import org .photonvision .common .logging .LogGroup ;
38
44
import org .photonvision .common .logging .LogLevel ;
39
45
import org .photonvision .common .logging .Logger ;
46
+ import org .photonvision .common .networking .NetworkManager ;
40
47
import org .photonvision .common .scripting .ScriptEventType ;
41
48
import org .photonvision .common .scripting .ScriptManager ;
42
49
import org .photonvision .common .util .TimedTaskManager ;
@@ -48,9 +55,19 @@ public class NetworkTablesManager {
48
55
49
56
private final NetworkTableInstance ntInstance = NetworkTableInstance .getDefault ();
50
57
private final String kRootTableName = "/photonvision" ;
58
+ private final String kCoprocTableName = "coprocessors" ;
51
59
private final String kFieldLayoutName = "apriltag_field_layout" ;
52
60
public final NetworkTable kRootTable = ntInstance .getTable (kRootTableName );
53
61
62
+ private final MultiSubscriber sub =
63
+ new MultiSubscriber (ntInstance , new String [] {kRootTableName + "/" + kCoprocTableName + "/" });
64
+
65
+ // Creating the alert up here since it should be persistent
66
+ private final Alert conflictAlert = new Alert ("PhotonAlerts" , "" , AlertType .kWarning );
67
+
68
+ public boolean conflictingHostname = false ;
69
+ public String conflictingCameras = "" ;
70
+
54
71
private boolean m_isRetryingConnection = false ;
55
72
56
73
private StringSubscriber m_fieldLayoutSubscriber =
@@ -70,14 +87,19 @@ private NetworkTablesManager() {
70
87
71
88
ntDriverStation = new NTDriverStation (this .getNTInst ());
72
89
73
- // Get the UI state in sync with the backend. NT should fire a callback when it first connects
74
- // to the robot
90
+ // This should start as false, since we don't know if there's a conflict yet
91
+ conflictAlert .set (false );
92
+
93
+ // Get the UI state in sync with the backend. NT should fire a callback when it
94
+ // first connects to the robot
75
95
broadcastConnectedStatus ();
76
96
}
77
97
78
98
public void registerTimedTasks () {
79
99
m_timeSync .start ();
80
100
TimedTaskManager .getInstance ().addTask ("NTManager" , this ::ntTick , 5000 );
101
+ TimedTaskManager .getInstance ()
102
+ .addTask ("CheckHostnameAndCameraNames" , this ::checkHostnameAndCameraNames , 10000 );
81
103
}
82
104
83
105
private static NetworkTablesManager INSTANCE ;
@@ -205,6 +227,101 @@ private void broadcastVersion() {
205
227
kRootTable .getEntry ("buildDate" ).setString (PhotonVersion .buildDate );
206
228
}
207
229
230
+ /**
231
+ * Publishes the hostname and camera names to a table using the MAC address as a key. Then checks
232
+ * for conflicts of hostname or camera names across other coprocessors that are also publishing to
233
+ * this table.
234
+ */
235
+ private void checkHostnameAndCameraNames () {
236
+ String MAC = NetworkManager .getInstance ().getMACAddress ();
237
+ if (MAC == null || MAC .isEmpty ()) {
238
+ logger .error ("Cannot check hostname and camera names, MAC address is not set!" );
239
+ return ;
240
+ }
241
+
242
+ String hostname = ConfigManager .getInstance ().getConfig ().getNetworkConfig ().hostname ;
243
+ if (hostname == null || hostname .isEmpty ()) {
244
+ logger .error ("Cannot check hostname and camera names, hostname is not set!" );
245
+ return ;
246
+ }
247
+
248
+ HashMap <String , CameraConfiguration > cameraConfigs =
249
+ ConfigManager .getInstance ().getConfig ().getCameraConfigurations ();
250
+ String [] cameraNames =
251
+ cameraConfigs .entrySet ().stream ()
252
+ .map (entry -> entry .getValue ().nickname )
253
+ .toArray (String []::new );
254
+
255
+ // Create a subtable under the photonvision root table
256
+ NetworkTable coprocTable = kRootTable .getSubTable (kCoprocTableName );
257
+
258
+ // Create a subtable for this coprocessor using its MAC address
259
+ NetworkTable macTable = coprocTable .getSubTable (MAC );
260
+
261
+ // Publish the hostname and camera names
262
+ macTable .getEntry ("hostname" ).setString (hostname );
263
+ macTable .getEntry ("cameraNames" ).setStringArray (cameraNames );
264
+ logger .debug ("Published hostname and camera names to NT under MAC: " + MAC );
265
+
266
+ boolean conflictingHostname = false ;
267
+ StringBuilder conflictingCameras = new StringBuilder ();
268
+
269
+ // Check for conflicts with other coprocessors
270
+ for (String key : coprocTable .getSubTables ()) {
271
+ // Check that key is formatted like a MAC address
272
+ if (!key .matches ("([0-9A-F]{2}-){5}[0-9A-F]{2}" )) {
273
+ logger .warn ("Skipping non-MAC key in conflict detection: " + key );
274
+ continue ;
275
+ }
276
+
277
+ if (!key .equals (MAC )) { // Skip our own entry
278
+ NetworkTable otherCoprocTable = coprocTable .getSubTable (key );
279
+ String otherHostname = otherCoprocTable .getEntry ("hostname" ).getString ("" );
280
+ String [] otherCameraNames =
281
+ otherCoprocTable .getEntry ("cameraNames" ).getStringArray (new String [0 ]);
282
+ // Check for hostname conflicts
283
+ if (otherHostname .equals (hostname )) {
284
+ logger .warn ("Hostname conflict detected with coprocessor " + key + ": " + hostname );
285
+ conflictingHostname = true ;
286
+ }
287
+
288
+ // Check for camera name conflicts
289
+ for (String cameraName : cameraNames ) {
290
+ if (Arrays .stream (otherCameraNames ).anyMatch (otherName -> otherName .equals (cameraName ))) {
291
+ logger .warn ("Camera name conflict detected: " + cameraName );
292
+ conflictingCameras .append (
293
+ conflictingCameras .isEmpty () ? cameraName : ", " + cameraName );
294
+ }
295
+ }
296
+ }
297
+ }
298
+
299
+ boolean hasChanged =
300
+ this .conflictingHostname != conflictingHostname
301
+ || !this .conflictingCameras .equals (conflictingCameras .toString ());
302
+
303
+ // Publish the conflict status
304
+ if (hasChanged ) {
305
+ DataChangeService .getInstance ()
306
+ .publishEvent (
307
+ new OutgoingUIEvent <>(
308
+ "fullsettings" ,
309
+ UIPhotonConfiguration .programStateToUi (ConfigManager .getInstance ().getConfig ())));
310
+ }
311
+
312
+ conflictAlert .setText (
313
+ conflictingHostname
314
+ ? "Hostname conflict detected for " + hostname + "!"
315
+ : ""
316
+ + (conflictingCameras .isEmpty ()
317
+ ? ""
318
+ : " Camera name conflict detected: " + conflictingCameras .toString () + "!" ));
319
+ conflictAlert .set (conflictingHostname || !conflictingCameras .isEmpty ());
320
+ SmartDashboard .updateValues ();
321
+ this .conflictingHostname = conflictingHostname ;
322
+ this .conflictingCameras = conflictingCameras .toString ();
323
+ }
324
+
208
325
public void setConfig (NetworkConfig config ) {
209
326
if (config .runNTServer ) {
210
327
setServerMode ();
@@ -246,9 +363,8 @@ private void setServerMode() {
246
363
247
364
// So it seems like if Photon starts before the robot NT server does, and both aren't static IP,
248
365
// it'll never connect. This hack works around it by restarting the client/server while the nt
249
- // instance
250
- // isn't connected, same as clicking the save button in the settings menu (or restarting the
251
- // service)
366
+ // instance isn't connected, same as clicking the save button in the settings menu (or restarting
367
+ // the service)
252
368
private void ntTick () {
253
369
if (!ntInstance .isConnected ()
254
370
&& !ConfigManager .getInstance ().getConfig ().getNetworkConfig ().runNTServer ) {
0 commit comments