Add alerts for timesync and disconnection (#1799)

photon-lib/src/main/java/org/photonvision/PhotonCamera.java

@@ -41,6 +41,8 @@ import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.PubSubOption;
 import edu.wpi.first.networktables.StringSubscriber;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.util.WPILibVersion;
@@ -59,6 +61,7 @@ import org.photonvision.timesync.TimeSyncSingleton;
 public class PhotonCamera implements AutoCloseable {
     private static int InstanceCount = 0;
     public static final String kTableName = "photonvision";
+    private static final String PHOTON_ALERT_GROUP = "PhotonAlerts";
 
     private final NetworkTable cameraTable;
     PacketSubscriber<PhotonPipelineResult> resultSubscriber;
@@ -68,7 +71,7 @@ public class PhotonCamera implements AutoCloseable {
     IntegerEntry inputSaveImgEntry, outputSaveImgEntry;
     IntegerPublisher pipelineIndexRequest, ledModeRequest;
     IntegerSubscriber pipelineIndexState, ledModeState;
-    IntegerSubscriber heartbeatEntry;
+    IntegerSubscriber heartbeatSubscriber;
     DoubleArraySubscriber cameraIntrinsicsSubscriber;
     DoubleArraySubscriber cameraDistortionSubscriber;
     MultiSubscriber topicNameSubscriber;
@@ -106,6 +109,9 @@ public class PhotonCamera implements AutoCloseable {
     double prevTimeSyncWarnTime = 0;
     private static final double WARN_DEBOUNCE_SEC = 5;
 
+    private final Alert disconnectAlert;
+    private final Alert timesyncAlert;
+
     public static void setVersionCheckEnabled(boolean enabled) {
         VERSION_CHECK_ENABLED = enabled;
     }
@@ -120,6 +126,10 @@ public class PhotonCamera implements AutoCloseable {
      */
     public PhotonCamera(NetworkTableInstance instance, String cameraName) {
         name = cameraName;
+        disconnectAlert =
+                new Alert(
+                        PHOTON_ALERT_GROUP, "PhotonCamera '" + name + "' is disconnected.", AlertType.kWarning);
+        timesyncAlert = new Alert(PHOTON_ALERT_GROUP, "", AlertType.kWarning);
         rootPhotonTable = instance.getTable(kTableName);
         this.cameraTable = rootPhotonTable.getSubTable(cameraName);
         path = cameraTable.getPath();
@@ -139,7 +149,7 @@ public class PhotonCamera implements AutoCloseable {
         outputSaveImgEntry = cameraTable.getIntegerTopic("outputSaveImgCmd").getEntry(0);
         pipelineIndexRequest = cameraTable.getIntegerTopic("pipelineIndexRequest").publish();
         pipelineIndexState = cameraTable.getIntegerTopic("pipelineIndexState").subscribe(0);
-        heartbeatEntry = cameraTable.getIntegerTopic("heartbeat").subscribe(-1);
+        heartbeatSubscriber = cameraTable.getIntegerTopic("heartbeat").subscribe(-1);
         cameraIntrinsicsSubscriber =
                 cameraTable.getDoubleArrayTopic("cameraIntrinsics").subscribe(null);
         cameraDistortionSubscriber =
@@ -249,6 +259,7 @@ public class PhotonCamera implements AutoCloseable {
      */
     public List<PhotonPipelineResult> getAllUnreadResults() {
         verifyVersion();
+        updateDisconnectAlert();
 
         List<PhotonPipelineResult> ret = new ArrayList<>();
 
@@ -274,6 +285,7 @@ public class PhotonCamera implements AutoCloseable {
     @Deprecated(since = "2024", forRemoval = true)
     public PhotonPipelineResult getLatestResult() {
         verifyVersion();
+        updateDisconnectAlert();
 
         // Grab the latest result. We don't care about the timestamp from NT - the metadata header has
         // this, latency compensated by the Time Sync Client
@@ -288,22 +300,34 @@ public class PhotonCamera implements AutoCloseable {
         return result;
     }
 
+    private void updateDisconnectAlert() {
+        disconnectAlert.set(!isConnected());
+    }
+
     private void checkTimeSyncOrWarn(PhotonPipelineResult result) {
         if (result.metadata.timeSinceLastPong > 5L * 1000000L) {
+            String warningText =
+                    "PhotonVision coprocessor at path "
+                            + path
+                            + " is not connected to the TimeSyncServer? It's been "
+                            + String.format("%.2f", result.metadata.timeSinceLastPong / 1e6)
+                            + "s since the coprocessor last heard a pong.";
+
+            timesyncAlert.setText(warningText);
+            timesyncAlert.set(true);
+
             if (Timer.getFPGATimestamp() > (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
                 prevTimeSyncWarnTime = Timer.getFPGATimestamp();
 
                 DriverStation.reportWarning(
-                        "PhotonVision coprocessor at path "
-                                + path
-                                + " is not connected to the TimeSyncServer? It's been "
-                                + String.format("%.2f", result.metadata.timeSinceLastPong / 1e6)
-                                + "s since the coprocessor last heard a pong.\n\nCheck /photonvision/.timesync/{COPROCESSOR_HOSTNAME} for more information.",
+                        warningText
+                                + "\n\nCheck /photonvision/.timesync/{COPROCESSOR_HOSTNAME} for more information.",
                         false);
             }
         } else {
             // Got a valid packet, reset the last time
             prevTimeSyncWarnTime = 0;
+            timesyncAlert.set(false);
         }
     }
 
@@ -404,9 +428,14 @@ public class PhotonCamera implements AutoCloseable {
      * @return True if the camera is actively sending frame data, false otherwise.
      */
     public boolean isConnected() {
-        var curHeartbeat = heartbeatEntry.get();
+        var curHeartbeat = heartbeatSubscriber.get();
         var now = Timer.getFPGATimestamp();
 
+        if (curHeartbeat < 0) {
+            // we have never heard from the camera
+            return false;
+        }
+
         if (curHeartbeat != prevHeartbeatValue) {
             // New heartbeat value from the coprocessor
             prevHeartbeatChangeTime = now;
@@ -455,7 +484,7 @@ public class PhotonCamera implements AutoCloseable {
 
         // Heartbeat entry is assumed to always be present. If it's not present, we
         // assume that a camera with that name was never connected in the first place.
-        if (!heartbeatEntry.exists()) {
+        if (!heartbeatSubscriber.exists()) {
             var cameraNames = getTablesThatLookLikePhotonCameras();
             if (cameraNames.isEmpty()) {
                 DriverStation.reportError(

photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp

@@ -44,6 +44,9 @@
 #include "opencv2/core/utility.hpp"
 #include "photon/dataflow/structures/Packet.h"
 
+static constexpr units::second_t WARN_DEBOUNCE_SEC = 5_s;
+static constexpr units::second_t HEARTBEAT_DEBOUNCE_SEC = 500_ms;
+
 inline void verifyDependencies() {
   if (!(std::string_view{GetWPILibVersion()} ==
         std::string_view{photon::PhotonVersion::wpilibTargetVersion})) {
@@ -137,6 +140,7 @@ namespace photon {
 
 constexpr const units::second_t VERSION_CHECK_INTERVAL = 5_s;
 static const std::vector<std::string_view> PHOTON_PREFIX = {"/photonvision/"};
+static const std::string PHOTON_ALERT_GROUP{"PhotonAlerts"};
 bool PhotonCamera::VERSION_CHECK_ENABLED = true;
 
 void PhotonCamera::SetVersionCheckEnabled(bool enabled) {
@@ -179,9 +183,16 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
           rootTable->GetBooleanTopic("driverMode").Subscribe(false)),
       driverModePublisher(
           rootTable->GetBooleanTopic("driverModeRequest").Publish()),
+      heartbeatSubscriber(
+          rootTable->GetIntegerTopic("heartbeat").Subscribe(-1)),
       topicNameSubscriber(instance, PHOTON_PREFIX, {.topicsOnly = true}),
       path(rootTable->GetPath()),
-      cameraName(cameraName) {
+      cameraName(cameraName),
+      disconnectAlert(PHOTON_ALERT_GROUP,
+                      std::string{"PhotonCamera '"} + std::string{cameraName} +
+                          "' is disconnected.",
+                      frc::Alert::AlertType::kWarning),
+      timesyncAlert(PHOTON_ALERT_GROUP, "", frc::Alert::AlertType::kWarning) {
   verifyDependencies();
   HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount);
   InstanceCount++;
@@ -217,6 +228,8 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
   // Create the new result;
   PhotonPipelineResult result = packet.Unpack<PhotonPipelineResult>();
 
+  CheckTimeSyncOrWarn(result);
+
   result.SetReceiveTimestamp(now);
 
   return result;
@@ -229,6 +242,7 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
 
   // Prints warning if not connected
   VerifyVersion();
+  UpdateDisconnectAlert();
 
   const auto changes = rawBytesEntry.ReadQueue();
 
@@ -247,6 +261,8 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
     photon::Packet packet{value.value};
     auto result = packet.Unpack<PhotonPipelineResult>();
 
+    CheckTimeSyncOrWarn(result);
+
     // TODO: NT4 timestamps are still not to be trusted. But it's the best we
     // can do until we can make time sync more reliable.
     result.SetReceiveTimestamp(units::microsecond_t(value.time) -
@@ -258,6 +274,37 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
   return ret;
 }
 
+void PhotonCamera::UpdateDisconnectAlert() {
+  disconnectAlert.Set(!IsConnected());
+}
+
+void PhotonCamera::CheckTimeSyncOrWarn(photon::PhotonPipelineResult& result) {
+  if (result.metadata.timeSinceLastPong > 5L * 1000000L) {
+    std::string warningText =
+        "PhotonVision coprocessor at path " + path +
+        " is not connected to the TimeSyncServer? It's been " +
+        std::to_string(result.metadata.timeSinceLastPong / 1e6) +
+        "s since the coprocessor last heard a pong.";
+
+    timesyncAlert.SetText(warningText);
+    timesyncAlert.Set(true);
+
+    if (frc::Timer::GetFPGATimestamp() <
+        (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
+      prevTimeSyncWarnTime = frc::Timer::GetFPGATimestamp();
+
+      FRC_ReportWarning(
+          warningText +
+          "\n\nCheck /photonvision/.timesync/{{COPROCESSOR_HOSTNAME}} for more "
+          "information.");
+    }
+  } else {
+    // Got a valid packet, reset the last time
+    prevTimeSyncWarnTime = 0_s;
+    timesyncAlert.Set(false);
+  }
+}
+
 void PhotonCamera::SetDriverMode(bool driverMode) {
   driverModePublisher.Set(driverMode);
 }
@@ -290,6 +337,24 @@ const std::string_view PhotonCamera::GetCameraName() const {
   return cameraName;
 }
 
+bool PhotonCamera::IsConnected() {
+  auto currentHeartbeat = heartbeatSubscriber.Get();
+  auto now = frc::Timer::GetFPGATimestamp();
+
+  if (currentHeartbeat < 0) {
+    // we have never heard from the camera
+    return false;
+  }
+
+  if (currentHeartbeat != prevHeartbeatValue) {
+    // New heartbeat value from the coprocessor
+    prevHeartbeatChangeTime = now;
+    prevHeartbeatValue = currentHeartbeat;
+  }
+
+  return (now - prevHeartbeatChangeTime) < HEARTBEAT_DEBOUNCE_SEC;
+}
+
 std::optional<PhotonCamera::CameraMatrix> PhotonCamera::GetCameraMatrix() {
   auto camCoeffs = cameraIntrinsicsSubscriber.Get();
   if (camCoeffs.size() == 9) {

photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp

@@ -335,7 +335,6 @@ PhotonPipelineResult PhotonCameraSim::Process(
     }
   }
 
-  heartbeatCounter++;
   return PhotonPipelineResult{
       PhotonPipelineMetadata{heartbeatCounter, 0,
                              units::microsecond_t{latency}.to<int64_t>(),
@@ -388,6 +387,7 @@ void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
   ts.cameraDistortionPublisher.Set(distortionView, ReceiveTimestamp);
 
   ts.heartbeatPublisher.Set(heartbeatCounter, ReceiveTimestamp);
+  heartbeatCounter++;
 
   ts.subTable->GetInstance().Flush();
 }

photon-lib/src/main/native/include/photon/PhotonCamera.h

@@ -28,6 +28,7 @@
 #include <string>
 #include <vector>
 
+#include <frc/Alert.h>
 #include <networktables/BooleanTopic.h>
 #include <networktables/DoubleArrayTopic.h>
 #include <networktables/DoubleTopic.h>
@@ -156,6 +157,14 @@ class PhotonCamera {
    */
   const std::string_view GetCameraName() const;
 
+  /**
+   * Returns whether the camera is connected and actively returning new data.
+   * Connection status is debounced.
+   *
+   * @return True if the camera is actively sending frame data, false otherwise.
+   */
+  bool IsConnected();
+
   using CameraMatrix = Eigen::Matrix<double, 3, 3>;
   using DistortionMatrix = Eigen::Matrix<double, 8, 1>;
 
@@ -203,18 +212,31 @@ class PhotonCamera {
   nt::BooleanPublisher driverModePublisher;
   nt::IntegerSubscriber ledModeSubscriber;
 
+  nt::IntegerSubscriber heartbeatSubscriber;
+
   nt::MultiSubscriber topicNameSubscriber;
 
   std::string path;
   std::string cameraName;
 
+  frc::Alert disconnectAlert;
+  frc::Alert timesyncAlert;
+
  private:
   units::second_t lastVersionCheckTime = 0_s;
   static bool VERSION_CHECK_ENABLED;
   inline static int InstanceCount = 0;
 
+  units::second_t prevTimeSyncWarnTime = 0_s;
+
+  int prevHeartbeatValue = -1;
+  units::second_t prevHeartbeatChangeTime = 0_s;
+
   void VerifyVersion();
 
+  void UpdateDisconnectAlert();
+  void CheckTimeSyncOrWarn(photon::PhotonPipelineResult& result);
+
   std::vector<std::string> tablesThatLookLikePhotonCameras();
 };
 

photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h

@@ -280,8 +280,8 @@ class SimCameraProperties {
   int resHeight;
   Eigen::Matrix<double, 3, 3> camIntrinsics;
   Eigen::Matrix<double, 8, 1> distCoeffs;
-  double avgErrorPx;
-  double errorStdDevPx;
+  double avgErrorPx{0};
+  double errorStdDevPx{0};
   units::second_t frameSpeed{0};
   units::second_t exposureTime{0};
   units::second_t avgLatency{0};