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 = 1; 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 \n>>> \s 199 >>> This is neither tested nor supported. \s 200 >>> You MUST update WPILib, PhotonLib, or both. 201 >>> Check `./gradlew dependencies` and ensure\s 202 >>> all mentions of OpenCV match the version \s 203 >>> that PhotonLib was built for. If you find a 204 >>> a mismatched version in a dependency, you\s 205 >>> must take steps to update the version of \s 206 >>> OpenCV used in that dependency. If you do\s 207 >>> not control that dependency and an updated\s 208 >>> version is not available, contact the \s 209 >>> developers of that dependency. \s 210 >>> \s 211 >>> Your code will now crash. \s 212 >>> We hope your day gets better. \s 213 >>> \s 214 >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s 215 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s 216 """; 217 218 DriverStation.reportWarning(bfw, false); 219 DriverStation.reportError(bfw, false); 220 throw new UnsupportedOperationException(bfw); 221 } 222 if (!Core.VERSION.equals(PhotonVersion.opencvTargetVersion)) { 223 String bfw = """ 224 225 226 227 228 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s 229 >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s 230 >>> \s 231 >>> You are running an incompatible version \s 232 >>> of PhotonVision ! \s 233 >>> \s 234 >>> PhotonLib """ 235 + PhotonVersion.versionString 236 + " is built for OpenCV " 237 + PhotonVersion.opencvTargetVersion 238 + "\n" 239 + ">>> but you are using OpenCV " 240 + Core.VERSION 241 + """ 242 \n>>> \s 243 >>> This is neither tested nor supported. \s 244 >>> You MUST update WPILib, PhotonLib, or both. 245 >>> Check `./gradlew dependencies` and ensure\s 246 >>> all mentions of OpenCV match the version \s 247 >>> that PhotonLib was built for. If you find a 248 >>> a mismatched version in a dependency, you\s 249 >>> must take steps to update the version of \s 250 >>> OpenCV used in that dependency. If you do\s 251 >>> not control that dependency and an updated\s 252 >>> version is not available, contact the \s 253 >>> developers of that dependency. \s 254 >>> \s 255 >>> Your code will now crash. \s 256 >>> We hope your day gets better. \s 257 >>> \s 258 >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\s 259 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\s 260 """; 261 // spotless:on 262 263 DriverStation.reportWarning(bfw, false); 264 DriverStation.reportError(bfw, false); 265 throw new UnsupportedOperationException(bfw); 266 } 267 } 268 269 /** 270 * Constructs a PhotonCamera from the name of the camera. 271 * 272 * @param cameraName The nickname of the camera (found in the PhotonVision UI). 273 */ 274 public PhotonCamera(String cameraName) { 275 this(NetworkTableInstance.getDefault(), cameraName); 276 } 277 278 /** 279 * The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults(). 280 * Calling this function clears the internal FIFO queue, and multiple calls to 281 * getAllUnreadResults() will return different (potentially empty) result arrays. Be careful to 282 * call this exactly ONCE per loop of your robot code! FIFO depth is limited to 20 changes, so 283 * make sure to call this frequently enough to avoid old results being discarded, too! 284 */ 285 public List<PhotonPipelineResult> getAllUnreadResults() { 286 verifyVersion(); 287 updateDisconnectAlert(); 288 289 // Grab the latest results. We don't care about the timestamps from NT - the metadata header has 290 // this, latency compensated by the Time Sync Client 291 var changes = resultSubscriber.getAllChanges(); 292 List<PhotonPipelineResult> ret = new ArrayList<>(changes.size()); 293 for (var c : changes) { 294 var result = c.value; 295 checkTimeSyncOrWarn(result); 296 ret.add(result); 297 } 298 299 return ret; 300 } 301 302 /** 303 * Returns the latest pipeline result. This is simply the most recent result Received via NT. 304 * Calling this multiple times will always return the most recent result. 305 * 306 * <p>Replaced by {@link #getAllUnreadResults()} over getLatestResult, as this function can miss 307 * results, or provide duplicate ones! 308 */ 309 @Deprecated(since = "2024", forRemoval = true) 310 public PhotonPipelineResult getLatestResult() { 311 verifyVersion(); 312 updateDisconnectAlert(); 313 314 // Grab the latest result. We don't care about the timestamp from NT - the metadata header has 315 // this, latency compensated by the Time Sync Client 316 var ret = resultSubscriber.get(); 317 318 if (ret.timestamp == 0) return new PhotonPipelineResult(); 319 320 var result = ret.value; 321 322 checkTimeSyncOrWarn(result); 323 324 return result; 325 } 326 327 private void updateDisconnectAlert() { 328 disconnectAlert.set(!isConnected()); 329 } 330 331 private void checkTimeSyncOrWarn(PhotonPipelineResult result) { 332 if (result.metadata.timeSinceLastPong > 5L * 1000000L) { 333 String warningText = 334 "PhotonVision coprocessor at path " 335 + path 336 + " is not connected to the TimeSyncServer? It's been " 337 + String.format("%.2f", result.metadata.timeSinceLastPong / 1e6) 338 + "s since the coprocessor last heard a pong."; 339 340 timesyncAlert.setText(warningText); 341 timesyncAlert.set(true); 342 343 if (Timer.getFPGATimestamp() > (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) { 344 prevTimeSyncWarnTime = Timer.getFPGATimestamp(); 345 346 DriverStation.reportWarning( 347 warningText 348 + "\n\nCheck /photonvision/.timesync/{COPROCESSOR_HOSTNAME} for more information.", 349 false); 350 } 351 } else { 352 // Got a valid packet, reset the last time 353 prevTimeSyncWarnTime = 0; 354 timesyncAlert.set(false); 355 } 356 } 357 358 /** 359 * Returns whether the camera is in driver mode. 360 * 361 * @return Whether the camera is in driver mode. 362 */ 363 public boolean getDriverMode() { 364 return driverModeSubscriber.get(); 365 } 366 367 /** 368 * Toggles driver mode. 369 * 370 * @param driverMode Whether to set driver mode. 371 */ 372 public void setDriverMode(boolean driverMode) { 373 driverModePublisher.set(driverMode); 374 } 375 376 /** 377 * Request the camera to save a new image file from the input camera stream with overlays. Images 378 * take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk 379 * space and eventually cause the system to stop working. Clear out images in 380 * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues. 381 */ 382 public void takeInputSnapshot() { 383 inputSaveImgEntry.set(inputSaveImgEntry.get() + 1); 384 } 385 386 /** 387 * Request the camera to save a new image file from the output stream with overlays. Images take 388 * up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk space 389 * and eventually cause the system to stop working. Clear out images in 390 * /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues. 391 */ 392 public void takeOutputSnapshot() { 393 outputSaveImgEntry.set(outputSaveImgEntry.get() + 1); 394 } 395 396 /** 397 * Returns the active pipeline index. 398 * 399 * @return The active pipeline index. 400 */ 401 public int getPipelineIndex() { 402 return (int) pipelineIndexState.get(0); 403 } 404 405 /** 406 * Allows the user to select the active pipeline index. 407 * 408 * @param index The active pipeline index. 409 */ 410 public void setPipelineIndex(int index) { 411 pipelineIndexRequest.set(index); 412 } 413 414 /** 415 * Returns the current LED mode. 416 * 417 * @return The current LED mode. 418 */ 419 public VisionLEDMode getLEDMode() { 420 int value = (int) ledModeState.get(-1); 421 return switch (value) { 422 case 0 -> VisionLEDMode.kOff; 423 case 1 -> VisionLEDMode.kOn; 424 case 2 -> VisionLEDMode.kBlink; 425 default -> VisionLEDMode.kDefault; 426 }; 427 } 428 429 /** 430 * Sets the LED mode. 431 * 432 * @param led The mode to set to. 433 */ 434 public void setLED(VisionLEDMode led) { 435 ledModeRequest.set(led.value); 436 } 437 438 /** 439 * Returns the name of the camera. This will return the same value that was given to the 440 * constructor as cameraName. 441 * 442 * @return The name of the camera. 443 */ 444 public String getName() { 445 return name; 446 } 447 448 /** 449 * Returns whether the camera is connected and actively returning new data. Connection status is 450 * debounced. 451 * 452 * @return True if the camera is actively sending frame data, false otherwise. 453 */ 454 public boolean isConnected() { 455 var curHeartbeat = heartbeatSubscriber.get(); 456 var now = Timer.getFPGATimestamp(); 457 458 if (curHeartbeat < 0) { 459 // we have never heard from the camera 460 return false; 461 } 462 463 if (curHeartbeat != prevHeartbeatValue) { 464 // New heartbeat value from the coprocessor 465 prevHeartbeatChangeTime = now; 466 prevHeartbeatValue = curHeartbeat; 467 } 468 469 return (now - prevHeartbeatChangeTime) < HEARTBEAT_DEBOUNCE_SEC; 470 } 471 472 public Optional<Matrix<N3, N3>> getCameraMatrix() { 473 var cameraMatrix = cameraIntrinsicsSubscriber.get(); 474 if (cameraMatrix != null && cameraMatrix.length == 9) { 475 return Optional.of(MatBuilder.fill(Nat.N3(), Nat.N3(), cameraMatrix)); 476 } else return Optional.empty(); 477 } 478 479 /** 480 * The camera calibration's distortion coefficients, in OPENCV8 form. Higher-order terms are set 481 * to 0 482 */ 483 public Optional<Matrix<N8, N1>> getDistCoeffs() { 484 var distCoeffs = cameraDistortionSubscriber.get(); 485 if (distCoeffs != null && distCoeffs.length <= 8) { 486 // Copy into array of length 8, and explicitly null higher order terms out 487 double[] data = new double[8]; 488 Arrays.fill(data, 0); 489 System.arraycopy(distCoeffs, 0, data, 0, distCoeffs.length); 490 491 return Optional.of(MatBuilder.fill(Nat.N8(), Nat.N1(), data)); 492 } else return Optional.empty(); 493 } 494 495 /** 496 * Gets the NetworkTable representing this camera's subtable. You probably don't ever need to call 497 * this. 498 */ 499 public final NetworkTable getCameraTable() { 500 return cameraTable; 501 } 502 503 void verifyVersion() { 504 if (!VERSION_CHECK_ENABLED) return; 505 506 if ((Timer.getFPGATimestamp() - lastVersionCheckTime) < VERSION_CHECK_INTERVAL) return; 507 lastVersionCheckTime = Timer.getFPGATimestamp(); 508 509 // Heartbeat entry is assumed to always be present. If it's not present, we 510 // assume that a camera with that name was never connected in the first place. 511 if (!heartbeatSubscriber.exists()) { 512 var cameraNames = getTablesThatLookLikePhotonCameras(); 513 if (cameraNames.isEmpty()) { 514 DriverStation.reportError( 515 "Could not find **any** PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!", 516 false); 517 } else { 518 DriverStation.reportError( 519 "PhotonVision coprocessor at path " 520 + path 521 + " not found on NetworkTables. Double check that your camera names match!", 522 true); 523 524 var cameraNameStr = new StringBuilder(); 525 for (var c : cameraNames) { 526 cameraNameStr.append(" ==> "); 527 cameraNameStr.append(c); 528 cameraNameStr.append("\n"); 529 } 530 531 DriverStation.reportError( 532 "Found the following PhotonVision cameras on NetworkTables:\n" 533 + cameraNameStr.toString(), 534 false); 535 } 536 } 537 // Check for connection status. Warn if disconnected. 538 else if (!isConnected()) { 539 DriverStation.reportWarning( 540 "PhotonVision coprocessor at path " + path + " is not sending new data.", false); 541 } 542 543 String versionString = versionEntry.get(""); 544 545 // Check mdef UUID 546 String local_uuid = PhotonPipelineResult.photonStruct.getInterfaceUUID(); 547 String remote_uuid = resultSubscriber.getInterfaceUUID(); 548 549 if (remote_uuid == null || remote_uuid.isEmpty()) { 550 // not connected yet? 551 DriverStation.reportWarning( 552 "PhotonVision coprocessor at path " 553 + path 554 + " has not reported a message interface UUID - is your coprocessor's camera started?", 555 false); 556 } else if (!local_uuid.equals(remote_uuid)) { 557 // Error on a verified version mismatch 558 // But stay silent otherwise 559 560 // spotless:off 561 String bfw = """ 562 563 564 565 566 567 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 568 >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 569 >>> \s 570 >>> You are running an incompatible version \s 571 >>> of PhotonVision on your coprocessor! \s 572 >>> \s 573 >>> This is neither tested nor supported. \s 574 >>> You MUST update PhotonVision, \s 575 >>> PhotonLib, or both. \s 576 >>> \s 577 >>> Your code will now crash. \s 578 >>> We hope your day gets better. \s 579 >>> \s 580 >>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 581 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 582 """; 583 // spotless:on 584 585 DriverStation.reportWarning(bfw, false); 586 var versionMismatchMessage = 587 "Photon version " 588 + PhotonVersion.versionString 589 + " (message definition version " 590 + local_uuid 591 + ")" 592 + " does not match coprocessor version " 593 + versionString 594 + " (message definition version " 595 + remote_uuid 596 + ")" 597 + "!"; 598 DriverStation.reportError(versionMismatchMessage, false); 599 throw new UnsupportedOperationException(versionMismatchMessage); 600 } 601 } 602 603 private List<String> getTablesThatLookLikePhotonCameras() { 604 return rootPhotonTable.getSubTables().stream() 605 .filter( 606 it -> { 607 return rootPhotonTable.getSubTable(it).getEntry("rawBytes").exists(); 608 }) 609 .toList(); 610 } 611}