001/* 002 * MIT License 003 * 004 * Copyright (c) PhotonVision 005 * 006 * Permission is hereby granted, free of charge, to any person obtaining a copy 007 * of this software and associated documentation files (the "Software"), to deal 008 * in the Software without restriction, including without limitation the rights 009 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 010 * copies of the Software, and to permit persons to whom the Software is 011 * furnished to do so, subject to the following conditions: 012 * 013 * The above copyright notice and this permission notice shall be included in all 014 * copies or substantial portions of the Software. 015 * 016 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 017 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 018 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 019 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 020 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 021 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 022 * SOFTWARE. 023 */ 024 025package org.photonvision; 026 027import edu.wpi.first.hal.FRCNetComm.tResourceType; 028import edu.wpi.first.hal.HAL; 029import edu.wpi.first.math.MatBuilder; 030import edu.wpi.first.math.Matrix; 031import edu.wpi.first.math.Nat; 032import edu.wpi.first.math.numbers.*; 033import edu.wpi.first.networktables.BooleanPublisher; 034import edu.wpi.first.networktables.BooleanSubscriber; 035import edu.wpi.first.networktables.DoubleArraySubscriber; 036import edu.wpi.first.networktables.IntegerEntry; 037import edu.wpi.first.networktables.IntegerPublisher; 038import edu.wpi.first.networktables.IntegerSubscriber; 039import edu.wpi.first.networktables.MultiSubscriber; 040import edu.wpi.first.networktables.NetworkTable; 041import edu.wpi.first.networktables.NetworkTableInstance; 042import edu.wpi.first.networktables.PubSubOption; 043import edu.wpi.first.networktables.StringSubscriber; 044import edu.wpi.first.wpilibj.Alert; 045import edu.wpi.first.wpilibj.Alert.AlertType; 046import edu.wpi.first.wpilibj.DriverStation; 047import edu.wpi.first.wpilibj.Timer; 048import edu.wpi.first.wpilibj.util.WPILibVersion; 049import java.util.ArrayList; 050import java.util.Arrays; 051import java.util.List; 052import java.util.Optional; 053import org.opencv.core.Core; 054import org.photonvision.common.hardware.VisionLEDMode; 055import org.photonvision.common.networktables.PacketSubscriber; 056import org.photonvision.targeting.PhotonPipelineResult; 057import org.photonvision.timesync.TimeSyncSingleton; 058 059/** Represents a camera that is connected to PhotonVision. */ 060public class PhotonCamera implements AutoCloseable { 061 private static int InstanceCount = 0; 062 public static final String kTableName = "photonvision"; 063 private static final String PHOTON_ALERT_GROUP = "PhotonAlerts"; 064 065 private final NetworkTable cameraTable; 066 PacketSubscriber<PhotonPipelineResult> resultSubscriber; 067 BooleanPublisher driverModePublisher; 068 BooleanSubscriber driverModeSubscriber; 069 StringSubscriber versionEntry; 070 IntegerEntry inputSaveImgEntry, outputSaveImgEntry; 071 IntegerPublisher pipelineIndexRequest, ledModeRequest; 072 IntegerSubscriber pipelineIndexState, ledModeState; 073 IntegerSubscriber heartbeatSubscriber; 074 DoubleArraySubscriber cameraIntrinsicsSubscriber; 075 DoubleArraySubscriber cameraDistortionSubscriber; 076 MultiSubscriber topicNameSubscriber; 077 NetworkTable rootPhotonTable; 078 079 @Override 080 public void close() { 081 resultSubscriber.close(); 082 driverModePublisher.close(); 083 driverModeSubscriber.close(); 084 versionEntry.close(); 085 inputSaveImgEntry.close(); 086 outputSaveImgEntry.close(); 087 pipelineIndexRequest.close(); 088 pipelineIndexState.close(); 089 ledModeRequest.close(); 090 ledModeState.close(); 091 pipelineIndexRequest.close(); 092 cameraIntrinsicsSubscriber.close(); 093 cameraDistortionSubscriber.close(); 094 topicNameSubscriber.close(); 095 } 096 097 private final String path; 098 private final String name; 099 100 private static boolean VERSION_CHECK_ENABLED = true; 101 private static long VERSION_CHECK_INTERVAL = 5; 102 double lastVersionCheckTime = 0; 103 104 private long prevHeartbeatValue = -1; 105 private double prevHeartbeatChangeTime = 0; 106 private static final double HEARTBEAT_DEBOUNCE_SEC = 0.5; 107 108 double prevTimeSyncWarnTime = 0; 109 private static final double WARN_DEBOUNCE_SEC = 5; 110 111 private final Alert disconnectAlert; 112 private final Alert timesyncAlert; 113 114 public static void setVersionCheckEnabled(boolean enabled) { 115 VERSION_CHECK_ENABLED = enabled; 116 } 117 118 /** 119 * Constructs a PhotonCamera from a root table. 120 * 121 * @param instance The NetworkTableInstance to pull data from. This can be a custom instance in 122 * simulation, but should *usually* be the default NTInstance from 123 * NetworkTableInstance::getDefault 124 * @param cameraName The name of the camera, as seen in the UI. 125 */ 126 public PhotonCamera(NetworkTableInstance instance, String cameraName) { 127 name = cameraName; 128 disconnectAlert = 129 new Alert( 130 PHOTON_ALERT_GROUP, "PhotonCamera '" + name + "' is disconnected.", AlertType.kWarning); 131 timesyncAlert = new Alert(PHOTON_ALERT_GROUP, "", AlertType.kWarning); 132 rootPhotonTable = instance.getTable(kTableName); 133 this.cameraTable = rootPhotonTable.getSubTable(cameraName); 134 path = cameraTable.getPath(); 135 var rawBytesEntry = 136 cameraTable 137 .getRawTopic("rawBytes") 138 .subscribe( 139 PhotonPipelineResult.photonStruct.getTypeString(), 140 new byte[0], 141 PubSubOption.periodic(0.01), 142 PubSubOption.sendAll(true), 143 PubSubOption.pollStorage(20)); 144 resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.photonStruct); 145 driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish(); 146 driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false); 147 inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0); 148 outputSaveImgEntry = cameraTable.getIntegerTopic("outputSaveImgCmd").getEntry(0); 149 pipelineIndexRequest = cameraTable.getIntegerTopic("pipelineIndexRequest").publish(); 150 pipelineIndexState = cameraTable.getIntegerTopic("pipelineIndexState").subscribe(0); 151 heartbeatSubscriber = cameraTable.getIntegerTopic("heartbeat").subscribe(-1); 152 cameraIntrinsicsSubscriber = 153 cameraTable.getDoubleArrayTopic("cameraIntrinsics").subscribe(null); 154 cameraDistortionSubscriber = 155 cameraTable.getDoubleArrayTopic("cameraDistortion").subscribe(null); 156 157 ledModeRequest = rootPhotonTable.getIntegerTopic("ledModeRequest").publish(); 158 ledModeState = rootPhotonTable.getIntegerTopic("ledModeState").subscribe(-1); 159 versionEntry = rootPhotonTable.getStringTopic("version").subscribe(""); 160 161 // Existing is enough to make this multisubscriber do its thing 162 topicNameSubscriber = 163 new MultiSubscriber( 164 instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true)); 165 166 HAL.report(tResourceType.kResourceType_PhotonCamera, InstanceCount); 167 InstanceCount++; 168 169 // HACK - start a TimeSyncServer, if we haven't yet. 170 TimeSyncSingleton.load(); 171 172 // HACK - check if things are compatible 173 verifyDependencies(); 174 } 175 176 public static void verifyDependencies() { 177 // spotless:off 178 if (!WPILibVersion.Version.equals(PhotonVersion.wpilibTargetVersion)) { 179 String bfw = """ 180 181 182 183 184 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s 185 >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s 186 >>> \s 187 >>> You are running an incompatible version \s 188 >>> of PhotonVision ! \s 189 >>> \s 190 >>> PhotonLib """ 191 + PhotonVersion.versionString 192 + " is built for WPILib " 193 + PhotonVersion.wpilibTargetVersion 194 + "\n" 195 + ">>> but you are using WPILib " 196 + WPILibVersion.Version 197 + """ 198 >>> \s 199 >>> This is neither tested nor supported. \s 200 >>> You MUST update PhotonVision, \s 201 >>> PhotonLib, or both. \s 202 >>> Verify the output of `./gradlew dependencies` 203 >>> \s 204 >>> Your code will now crash. \s 205 >>> We hope your day gets better. \s 206 >>> \s 207 >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s 208 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s 209 """; 210 211 DriverStation.reportWarning(bfw, false); 212 DriverStation.reportError(bfw, false); 213 throw new UnsupportedOperationException(bfw); 214 } 215 if (!Core.VERSION.equals(PhotonVersion.opencvTargetVersion)) { 216 String bfw = """ 217 218 219 220 221 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s 222 >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s 223 >>> \s 224 >>> You are running an incompatible version \s 225 >>> of PhotonVision ! \s 226 >>> \s 227 >>> PhotonLib """ 228 + PhotonVersion.versionString 229 + " is built for OpenCV " 230 + PhotonVersion.opencvTargetVersion 231 + "\n" 232 + ">>> but you are using OpenCV " 233 + Core.VERSION 234 + """ 235 >>> \s 236 >>> This is neither tested nor supported. \s 237 >>> You MUST update PhotonVision, \s 238 >>> PhotonLib, or both. \s 239 >>> Verify the output of `./gradlew dependencies` 240 >>> \s 241 >>> Your code will now crash. \s 242 >>> We hope your day gets better. \s 243 >>> \s 244 >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s 245 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s 246 """; 247 // spotless:on 248 249 DriverStation.reportWarning(bfw, false); 250 DriverStation.reportError(bfw, false); 251 throw new UnsupportedOperationException(bfw); 252 } 253 } 254 255 /** 256 * Constructs a PhotonCamera from the name of the camera. 257 * 258 * @param cameraName The nickname of the camera (found in the PhotonVision UI). 259 */ 260 public PhotonCamera(String cameraName) { 261 this(NetworkTableInstance.getDefault(), cameraName); 262 } 263 264 /** 265 * The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults(). 266 * Calling this function clears the internal FIFO queue, and multiple calls to 267 * getAllUnreadResults() will return different (potentially empty) result arrays. Be careful to 268 * call this exactly ONCE per loop of your robot code! FIFO depth is limited to 20 changes, so 269 * make sure to call this frequently enough to avoid old results being discarded, too! 270 */ 271 public List<PhotonPipelineResult> getAllUnreadResults() { 272 verifyVersion(); 273 updateDisconnectAlert(); 274 275 List<PhotonPipelineResult> ret = new ArrayList<>(); 276 277 // Grab the latest results. We don't care about the timestamps from NT - the metadata header has 278 // this, latency compensated by the Time Sync Client 279 var changes = resultSubscriber.getAllChanges(); 280 for (var c : changes) { 281 var result = c.value; 282 checkTimeSyncOrWarn(result); 283 ret.add(result); 284 } 285 286 return ret; 287 } 288 289 /** 290 * Returns the latest pipeline result. This is simply the most recent result Received via NT. 291 * Calling this multiple times will always return the most recent result. 292 * 293 * <p>Replaced by {@link #getAllUnreadResults()} over getLatestResult, as this function can miss 294 * results, or provide duplicate ones! 295 */ 296 @Deprecated(since = "2024", forRemoval = true) 297 public PhotonPipelineResult getLatestResult() { 298 verifyVersion(); 299 updateDisconnectAlert(); 300 301 // Grab the latest result. We don't care about the timestamp from NT - the metadata header has 302 // this, latency compensated by the Time Sync Client 303 var ret = resultSubscriber.get(); 304 305 if (ret.timestamp == 0) return new PhotonPipelineResult(); 306 307 var result = ret.value; 308 309 checkTimeSyncOrWarn(result); 310 311 return result; 312 } 313 314 private void updateDisconnectAlert() { 315 disconnectAlert.set(!isConnected()); 316 } 317 318 private void checkTimeSyncOrWarn(PhotonPipelineResult result) { 319 if (result.metadata.timeSinceLastPong > 5L * 1000000L) { 320 String warningText = 321 "PhotonVision coprocessor at path " 322 + path 323 + " is not connected to the TimeSyncServer? It's been " 324 + String.format("%.2f", result.metadata.timeSinceLastPong / 1e6) 325 + "s since the coprocessor last heard a pong."; 326 327 timesyncAlert.setText(warningText); 328 timesyncAlert.set(true); 329 330 if (Timer.getFPGATimestamp() > (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) { 331 prevTimeSyncWarnTime = Timer.getFPGATimestamp(); 332 333 DriverStation.reportWarning( 334 warningText 335 + "\n\nCheck /photonvision/.timesync/{COPROCESSOR_HOSTNAME} for more information.", 336 false); 337 } 338 } else { 339 // Got a valid packet, reset the last time 340 prevTimeSyncWarnTime = 0; 341 timesyncAlert.set(false); 342 } 343 } 344 345 /** 346 * Returns whether the camera is in driver mode. 347 * 348 * @return Whether the camera is in driver mode. 349 */ 350 public boolean getDriverMode() { 351 return driverModeSubscriber.get(); 352 } 353 354 /** 355 * Toggles driver mode. 356 * 357 * @param driverMode Whether to set driver mode. 358 */ 359 public void setDriverMode(boolean driverMode) { 360 driverModePublisher.set(driverMode); 361 } 362 363 /** 364 * Request the camera to save a new image file from the input camera stream with overlays. Images 365 * take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk 366 * space and eventually cause the system to stop working. Clear out images in 367 * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues. 368 */ 369 public void takeInputSnapshot() { 370 inputSaveImgEntry.set(inputSaveImgEntry.get() + 1); 371 } 372 373 /** 374 * Request the camera to save a new image file from the output stream with overlays. Images take 375 * up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk space 376 * and eventually cause the system to stop working. Clear out images in 377 * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues. 378 */ 379 public void takeOutputSnapshot() { 380 outputSaveImgEntry.set(outputSaveImgEntry.get() + 1); 381 } 382 383 /** 384 * Returns the active pipeline index. 385 * 386 * @return The active pipeline index. 387 */ 388 public int getPipelineIndex() { 389 return (int) pipelineIndexState.get(0); 390 } 391 392 /** 393 * Allows the user to select the active pipeline index. 394 * 395 * @param index The active pipeline index. 396 */ 397 public void setPipelineIndex(int index) { 398 pipelineIndexRequest.set(index); 399 } 400 401 /** 402 * Returns the current LED mode. 403 * 404 * @return The current LED mode. 405 */ 406 public VisionLEDMode getLEDMode() { 407 int value = (int) ledModeState.get(-1); 408 return switch (value) { 409 case 0 -> VisionLEDMode.kOff; 410 case 1 -> VisionLEDMode.kOn; 411 case 2 -> VisionLEDMode.kBlink; 412 default -> VisionLEDMode.kDefault; 413 }; 414 } 415 416 /** 417 * Sets the LED mode. 418 * 419 * @param led The mode to set to. 420 */ 421 public void setLED(VisionLEDMode led) { 422 ledModeRequest.set(led.value); 423 } 424 425 /** 426 * Returns the name of the camera. This will return the same value that was given to the 427 * constructor as cameraName. 428 * 429 * @return The name of the camera. 430 */ 431 public String getName() { 432 return name; 433 } 434 435 /** 436 * Returns whether the camera is connected and actively returning new data. Connection status is 437 * debounced. 438 * 439 * @return True if the camera is actively sending frame data, false otherwise. 440 */ 441 public boolean isConnected() { 442 var curHeartbeat = heartbeatSubscriber.get(); 443 var now = Timer.getFPGATimestamp(); 444 445 if (curHeartbeat < 0) { 446 // we have never heard from the camera 447 return false; 448 } 449 450 if (curHeartbeat != prevHeartbeatValue) { 451 // New heartbeat value from the coprocessor 452 prevHeartbeatChangeTime = now; 453 prevHeartbeatValue = curHeartbeat; 454 } 455 456 return (now - prevHeartbeatChangeTime) < HEARTBEAT_DEBOUNCE_SEC; 457 } 458 459 public Optional<Matrix<N3, N3>> getCameraMatrix() { 460 var cameraMatrix = cameraIntrinsicsSubscriber.get(); 461 if (cameraMatrix != null && cameraMatrix.length == 9) { 462 return Optional.of(MatBuilder.fill(Nat.N3(), Nat.N3(), cameraMatrix)); 463 } else return Optional.empty(); 464 } 465 466 /** 467 * The camera calibration's distortion coefficients, in OPENCV8 form. Higher-order terms are set 468 * to 0 469 */ 470 public Optional<Matrix<N8, N1>> getDistCoeffs() { 471 var distCoeffs = cameraDistortionSubscriber.get(); 472 if (distCoeffs != null && distCoeffs.length <= 8) { 473 // Copy into array of length 8, and explicitly null higher order terms out 474 double[] data = new double[8]; 475 Arrays.fill(data, 0); 476 System.arraycopy(distCoeffs, 0, data, 0, distCoeffs.length); 477 478 return Optional.of(MatBuilder.fill(Nat.N8(), Nat.N1(), data)); 479 } else return Optional.empty(); 480 } 481 482 /** 483 * Gets the NetworkTable representing this camera's subtable. You probably don't ever need to call 484 * this. 485 */ 486 public final NetworkTable getCameraTable() { 487 return cameraTable; 488 } 489 490 void verifyVersion() { 491 if (!VERSION_CHECK_ENABLED) return; 492 493 if ((Timer.getFPGATimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return; 494 lastVersionCheckTime = Timer.getFPGATimestamp(); 495 496 // Heartbeat entry is assumed to always be present. If it's not present, we 497 // assume that a camera with that name was never connected in the first place. 498 if (!heartbeatSubscriber.exists()) { 499 var cameraNames = getTablesThatLookLikePhotonCameras(); 500 if (cameraNames.isEmpty()) { 501 DriverStation.reportError( 502 "Could not find **any** PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!", 503 false); 504 } else { 505 DriverStation.reportError( 506 "PhotonVision coprocessor at path " 507 + path 508 + " not found on NetworkTables. Double check that your camera names match!", 509 true); 510 511 var cameraNameStr = new StringBuilder(); 512 for (var c : cameraNames) { 513 cameraNameStr.append(" ==> "); 514 cameraNameStr.append(c); 515 cameraNameStr.append("\n"); 516 } 517 518 DriverStation.reportError( 519 "Found the following PhotonVision cameras on NetworkTables:\n" 520 + cameraNameStr.toString(), 521 false); 522 } 523 } 524 // Check for connection status. Warn if disconnected. 525 else if (!isConnected()) { 526 DriverStation.reportWarning( 527 "PhotonVision coprocessor at path " + path + " is not sending new data.", false); 528 } 529 530 String versionString = versionEntry.get(""); 531 532 // Check mdef UUID 533 String local_uuid = PhotonPipelineResult.photonStruct.getInterfaceUUID(); 534 String remote_uuid = resultSubscriber.getInterfaceUUID(); 535 536 if (remote_uuid == null || remote_uuid.isEmpty()) { 537 // not connected yet? 538 DriverStation.reportWarning( 539 "PhotonVision coprocessor at path " 540 + path 541 + " has not reported a message interface UUID - is your coprocessor's camera started?", 542 false); 543 } else if (!local_uuid.equals(remote_uuid)) { 544 // Error on a verified version mismatch 545 // But stay silent otherwise 546 547 // spotless:off 548 String bfw = """ 549 550 551 552 553 554 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 555 >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 556 >>> \s 557 >>> You are running an incompatible version \s 558 >>> of PhotonVision on your coprocessor! \s 559 >>> \s 560 >>> This is neither tested nor supported. \s 561 >>> You MUST update PhotonVision, \s 562 >>> PhotonLib, or both. \s 563 >>> \s 564 >>> Your code will now crash. \s 565 >>> We hope your day gets better. \s 566 >>> \s 567 >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 568 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 569 """; 570 // spotless:on 571 572 DriverStation.reportWarning(bfw, false); 573 var versionMismatchMessage = 574 "Photon version " 575 + PhotonVersion.versionString 576 + " (message definition version " 577 + local_uuid 578 + ")" 579 + " does not match coprocessor version " 580 + versionString 581 + " (message definition version " 582 + remote_uuid 583 + ")" 584 + "!"; 585 DriverStation.reportError(versionMismatchMessage, false); 586 throw new UnsupportedOperationException(versionMismatchMessage); 587 } 588 } 589 590 private List<String> getTablesThatLookLikePhotonCameras() { 591 return rootPhotonTable.getSubTables().stream() 592 .filter( 593 it -> { 594 return rootPhotonTable.getSubTable(it).getEntry("rawBytes").exists(); 595 }) 596 .toList(); 597 } 598}