diff --git a/.gitignore b/.gitignore index 6fcd86932..27e2c667d 100644 --- a/.gitignore +++ b/.gitignore @@ -150,3 +150,4 @@ photon-server/src/main/resources/web/* venv .venv/* .venv +networktables.json diff --git a/README.md b/README.md index 5db8ed8e5..060010aed 100644 --- a/README.md +++ b/README.md @@ -42,6 +42,7 @@ Note that these are case sensitive! * linuxathena - `-PtgtIP`: Specifies where `./gradlew deploy` should try to copy the fat JAR to - `-Pprofile`: enables JVM profiling +- `-PwithSanitizers`: On Linux, enables `-fsanitize=address,undefined,leak` If you're cross-compiling, you'll need the wpilib toolchain installed. This can be done via Gradle: for example `./gradlew installArm64Toolchain` or `./gradlew installRoboRioToolchain` diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 3d399c029..a60ec848f 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/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 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 getAllUnreadResults() { verifyVersion(); + updateDisconnectAlert(); List 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( diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 59bfc613a..c0c29462e 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/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 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(); + CheckTimeSyncOrWarn(result); + result.SetReceiveTimestamp(now); return result; @@ -229,6 +242,7 @@ std::vector PhotonCamera::GetAllUnreadResults() { // Prints warning if not connected VerifyVersion(); + UpdateDisconnectAlert(); const auto changes = rawBytesEntry.ReadQueue(); @@ -247,6 +261,8 @@ std::vector PhotonCamera::GetAllUnreadResults() { photon::Packet packet{value.value}; auto result = packet.Unpack(); + 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 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::GetCameraMatrix() { auto camCoeffs = cameraIntrinsicsSubscriber.Get(); if (camCoeffs.size() == 9) { diff --git a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp index 3ee6aaddf..ed6d2bd35 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/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(), @@ -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(); } diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index df2c05d92..66f6e1276 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -28,6 +28,7 @@ #include #include +#include #include #include #include @@ -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; using DistortionMatrix = Eigen::Matrix; @@ -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 tablesThatLookLikePhotonCameras(); }; diff --git a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h index 701d46645..050aa7038 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h @@ -280,8 +280,8 @@ class SimCameraProperties { int resHeight; Eigen::Matrix camIntrinsics; Eigen::Matrix 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}; diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java index 17c6f54e3..c25955480 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java @@ -25,6 +25,9 @@ package org.photonvision; import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertNull; +import static org.junit.jupiter.api.Assertions.assertTrue; import static org.junit.jupiter.api.Assumptions.assumeTrue; import static org.photonvision.UnitTestUtils.waitForCondition; import static org.photonvision.UnitTestUtils.waitForSequenceNumber; @@ -33,8 +36,14 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.NetworkTablesJNI; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.SimHooks; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.io.IOException; +import java.util.Arrays; +import java.util.List; +import java.util.Optional; import java.util.stream.Stream; import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.Assertions; @@ -49,24 +58,38 @@ import org.photonvision.jni.PhotonTargetingJniLoader; import org.photonvision.jni.TimeSyncClient; import org.photonvision.jni.WpilibLoader; import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.targeting.PhotonPipelineMetadata; import org.photonvision.targeting.PhotonPipelineResult; class PhotonCameraTest { + // A test-scoped, local-only NT instance + NetworkTableInstance inst = null; + @BeforeAll public static void load_wpilib() { WpilibLoader.loadLibraries(); - - // See #1574 - test flakey, disabled until we address this - assumeTrue(false); } @BeforeEach public void setup() { + assertNull(inst); + HAL.initialize(500, 0); + + inst = NetworkTableInstance.create(); + inst.stopClient(); + inst.stopServer(); + inst.startLocal(); + SmartDashboard.setNetworkTableInstance(inst); } @AfterEach public void teardown() { + inst.close(); + inst = null; + + SimHooks.resumeTiming(); + HAL.shutdown(); } @@ -87,12 +110,10 @@ class PhotonCameraTest { load_wpilib(); PhotonTargetingJniLoader.load(); - HAL.initialize(500, 0); + inst.stopClient(); + inst.startServer(); - NetworkTableInstance.getDefault().stopClient(); - NetworkTableInstance.getDefault().startServer(); - - var camera = new PhotonCamera("Arducam_OV2311_USB_Camera"); + var camera = new PhotonCamera(inst, "Arducam_OV2311_USB_Camera"); PhotonCamera.setVersionCheckEnabled(false); for (int i = 0; i < 5; i++) { @@ -132,6 +153,38 @@ class PhotonCameraTest { Arguments.of(1, 1, 30, 10)); } + private void configureInstanceDataLoggers(NetworkTableInstance inst, String name) { + // Consumer printEvent = (NetworkTableEvent e) -> { + // if (e.is(Kind.kConnected)) { + // System.out.println(name + ": Connected to " + e.connInfo); + // } + // if (e.is(Kind.kDisconnected)) { + // System.out.println(name + ": Disconnected from " + e.connInfo); + // } + // if (e.is(Kind.kPublish)) { + // System.out.println(name + ": Topic published: " + e.topicInfo); + // } + // if (e.is(Kind.kUnpublish)) { + // System.out.println(name + ": Topic removed " + e.topicInfo); + // } + // if (e.is(Kind.kValueAll)) { + // System.out.println(name + ": Value changed " + e.valueData); + // } + // if (e.is(Kind.kLogMessage)) { + // System.out.println(name + ": LOG: " + e.logMessage); + // } + // }; + + // inst.addConnectionListener(true, printEvent); + // inst.addListener(new String[]{""}, EnumSet.of(NetworkTableEvent.Kind.kTopic, + // NetworkTableEvent.Kind.kValueAll), printEvent); + + var log = DataLogManager.getLog(); + inst.startEntryDataLog(log, "", name + "_NT:"); + inst.startConnectionDataLog(log, name + "_NTCONNECTION:"); + System.out.println(name + ": Started logging to " + DataLogManager.getLogDir()); + } + /** * Try starting client before server and vice-versa, making sure that we never fail the version * check @@ -140,9 +193,21 @@ class PhotonCameraTest { @MethodSource("testNtOffsets") public void testRestartingRobotAndCoproc( int robotStart, int coprocStart, int robotRestart, int coprocRestart) throws Throwable { + // See #1574 - test flakey, disabled until we address this + assumeTrue(false); + var robotNt = NetworkTableInstance.create(); var coprocNt = NetworkTableInstance.create(); + configureInstanceDataLoggers(robotNt, "ROBOT 1"); + configureInstanceDataLoggers(coprocNt, "COPROC 1"); + + // Don't need inst + inst.close(); + + // rename log + DataLogManager.start("logs", "testRestartingRobotAndCoproc.wpilog"); + robotNt.addLogger(10, 255, (it) -> System.out.println("ROBOT: " + it.logMessage.message)); coprocNt.addLogger(10, 255, (it) -> System.out.println("CLIENT: " + it.logMessage.message)); @@ -166,6 +231,7 @@ class PhotonCameraTest { fakePhotonCoprocCam.close(); coprocNt.close(); coprocNt = NetworkTableInstance.create(); + configureInstanceDataLoggers(coprocNt, "COPROC 2"); coprocNt.addLogger(10, 255, (it) -> System.out.println("CLIENT: " + it.logMessage.message)); @@ -180,6 +246,7 @@ class PhotonCameraTest { robotNt.close(); robotNt = NetworkTableInstance.create(); + configureInstanceDataLoggers(robotNt, "ROBOT 2"); robotNt.addLogger(10, 255, (it) -> System.out.println("ROBOT: " + it.logMessage.message)); robotCamera = new PhotonCamera(robotNt, "MY_CAMERA"); } @@ -233,5 +300,94 @@ class PhotonCameraTest { coprocNt.close(); robotNt.close(); tspClient.stop(); + + DataLogManager.stop(); + } + + @Test + public void testAlerts() throws InterruptedException { + // GIVEN a fresh NT instance + + var cameraName = "foobar"; + + // AND a photoncamera that is disconnected + var camera = new PhotonCamera(inst, cameraName); + assertFalse(camera.isConnected()); + + String disconnectedCameraString = "PhotonCamera '" + cameraName + "' is disconnected."; + + // Loop to hit cases past first iteration + for (int i = 0; i < 10; i++) { + // WHEN we update the camera + camera.getAllUnreadResults(); + + // AND we tick SmartDashboard + SmartDashboard.updateValues(); + + // The alert state will be set (hard-coded here) + assertTrue( + Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0])) + .anyMatch(it -> it.equals(disconnectedCameraString))); + + Thread.sleep(20); + } + + // GIVEN a simulated camera + var sim = new PhotonCameraSim(camera); + // AND a result with a timeSinceLastPong in the past + PhotonPipelineResult noPongResult = + new PhotonPipelineResult( + new PhotonPipelineMetadata( + 1, 2, 3, 10 * 1000000 // 10 seconds -> us since last pong + ), + List.of(), + Optional.empty()); + + // Loop to hit cases past first iteration + for (int i = 0; i < 10; i++) { + // AND a PhotonCamera with a "new" result + sim.submitProcessedFrame(noPongResult); + + // WHEN we update the camera + camera.getAllUnreadResults(); + + // AND we tick SmartDashboard + SmartDashboard.updateValues(); + + // THEN the camera isn't disconnected + assertTrue( + Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0])) + .noneMatch(it -> it.equals(disconnectedCameraString))); + // AND the alert string looks like a timesync warning + assertTrue( + Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0])) + .filter(it -> it.contains("is not connected to the TimeSyncServer")) + .count() + == 1); + + Thread.sleep(20); + } + + final double HEARTBEAT_TIMEOUT = 0.5; + + // GIVEN a PhotonCamera provided new results + SimHooks.pauseTiming(); + sim.submitProcessedFrame(noPongResult); + camera.getAllUnreadResults(); + // AND in a connected state + assertTrue(camera.isConnected()); + + // WHEN we wait the timeout + SimHooks.stepTiming(HEARTBEAT_TIMEOUT * 1.5); + + // THEN the camera will not be connected + assertFalse(camera.isConnected()); + + // WHEN we then provide new results + SimHooks.stepTiming(0.02); + sim.submitProcessedFrame(noPongResult); + camera.getAllUnreadResults(); + // THEN the camera will not be connected + assertTrue(camera.isConnected()); } } diff --git a/photon-lib/src/test/java/org/photonvision/UnitTestUtils.java b/photon-lib/src/test/java/org/photonvision/UnitTestUtils.java index 99af2e0f2..d4485d9c1 100644 --- a/photon-lib/src/test/java/org/photonvision/UnitTestUtils.java +++ b/photon-lib/src/test/java/org/photonvision/UnitTestUtils.java @@ -50,10 +50,10 @@ public class UnitTestUtils { } static PhotonPipelineResult waitForSequenceNumber(PhotonCamera camera, int seq) { - assertTrue(camera.heartbeatEntry.getTopic().getHandle() != 0); + assertTrue(camera.heartbeatSubscriber.getTopic().getHandle() != 0); System.out.println( - "Waiting for seq=" + seq + " on " + camera.heartbeatEntry.getTopic().getName()); + "Waiting for seq=" + seq + " on " + camera.heartbeatSubscriber.getTopic().getName()); // wait up to 1 second for a new result for (int i = 0; i < 100; i++) { var res = camera.getLatestResult(); diff --git a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp index d795b653d..74876eda1 100644 --- a/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonCameraTest.cpp @@ -22,12 +22,19 @@ * SOFTWARE. */ +#include #include #include #include #include +#include +#include -#include "photon/PhotonCamera.h" +#include +#include + +#include +#include TEST(TimeSyncProtocolTest, Smoketest) { using namespace wpi::tsp; @@ -52,3 +59,77 @@ TEST(TimeSyncProtocolTest, Smoketest) { client.Stop(); } + +TEST(PhotonCameraTest, Alerts) { + using frc::SmartDashboard; + + // GIVEN a local-only NT instance + auto inst = nt::NetworkTableInstance::GetDefault(); + inst.StopClient(); + inst.StopServer(); + inst.StartLocal(); + // (We can't create our own instance, SmartDashboard will always use the + // default) + + const std::string cameraName = "foobar"; + + // AND a PhotonCamera that is disconnected + photon::PhotonCamera camera(inst, cameraName); + EXPECT_FALSE(camera.IsConnected()); + std::string disconnectedCameraString = + "PhotonCamera '" + cameraName + "' is disconnected."; + + // Loop to hit cases past first iteration + for (int i = 0; i < 10; i++) { + // WHEN we update the camera + camera.GetAllUnreadResults(); + // AND we tick SmartDashboard + SmartDashboard::UpdateValues(); + + // The alert state will be set (hard-coded here) + auto alerts = SmartDashboard::GetStringArray("PhotonAlerts/warnings", {}); + EXPECT_TRUE( + std::any_of(alerts.begin(), alerts.end(), + [&disconnectedCameraString](const std::string& alert) { + return alert == disconnectedCameraString; + })); + + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + + // GIVEN a simulated camera + photon::PhotonCameraSim sim(&camera); + // AND a result with a timeSinceLastPong in the past + photon::PhotonPipelineMetadata metadata{3, 1, 2, 10 * 1000000}; + photon::PhotonPipelineResult noPongResult{ + metadata, std::vector{}, std::nullopt}; + + // Loop to hit cases past first iteration + for (int i = 0; i < 10; i++) { + // AND a PhotonCamera with a "new" result + sim.SubmitProcessedFrame(noPongResult); + // WHEN we update the camera + camera.GetAllUnreadResults(); + // AND we tick SmartDashboard + SmartDashboard::UpdateValues(); + + // THEN the camera isn't disconnected + auto alerts = SmartDashboard::GetStringArray("PhotonAlerts/warnings", {}); + fmt::println("{}:{}: saw alerts: {}", __FILE__, __LINE__, alerts); + EXPECT_TRUE( + std::none_of(alerts.begin(), alerts.end(), + [&disconnectedCameraString](const std::string& alert) { + return alert == disconnectedCameraString; + })); + + // AND the alert string looks like a timesync warning + EXPECT_EQ( + 1, std::count_if( + alerts.begin(), alerts.end(), [](const std::string& alert) { + return alert.find("is not connected to the TimeSyncServer") != + std::string::npos; + })); + + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } +} diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp index 88696414a..e9a26b024 100644 --- a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp +++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp @@ -509,6 +509,10 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) { auto camResults = camera.GetLatestResult(); auto targetSpan = camResults.GetTargets(); + + // We need to see at least one target + ASSERT_GT(targetSpan.size(), static_cast(0)); + std::vector targets; for (photon::PhotonTrackedTarget tar : targetSpan) { targets.push_back(tar); diff --git a/shared/config.gradle b/shared/config.gradle index 26bcdce76..1178f8372 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -31,6 +31,12 @@ model { nativeUtils.usePlatformArguments(it) if (it.toolChain instanceof GccCompatibleToolChain) { it.cppCompiler.args << "-Wno-deprecated-enum-enum-conversion" + + if (project.hasProperty('withSanitizers')) { + println("Adding asan/usan/lsan to " + it) + it.cppCompiler.args << "-fsanitize=address,undefined,leak" << "-g" + it.linker.args << "-fsanitize=address,undefined,leak" + } } } }