Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add alerts for timesync and disconnection (#1799)

Browse Source
This commit is contained in:
Gold87
2025-03-14 02:13:51 -04:00
committed by GitHub
parent f6736fc730
commit 8d4024b8c8
12 changed files with 389 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -150,3 +150,4 @@ photon-server/src/main/resources/web/*
venv
.venv/*
.venv
networktables.json

1
README.md
View File

@@ -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`

47
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View File

@@ -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(

67
photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
View File

@@ -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) {

2
photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp
View File

@@ -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();
}

22
photon-lib/src/main/native/include/photon/PhotonCamera.h
View File

@@ -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();
};

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

@@ -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};

172
photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java
View File

@@ -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<NetworkTableEvent> 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());
}
}

4
photon-lib/src/test/java/org/photonvision/UnitTestUtils.java
View File

@@ -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();

83
photon-lib/src/test/native/cpp/PhotonCameraTest.cpp
View File

@@ -22,12 +22,19 @@
* SOFTWARE.
*/
#include <fmt/ranges.h>
#include <gtest/gtest.h>
#include <hal/HAL.h>
#include <net/TimeSyncClient.h>
#include <net/TimeSyncServer.h>
#include <photon/PhotonCamera.h>
#include <photon/simulation/PhotonCameraSim.h>
#include "photon/PhotonCamera.h"
#include <string>
#include <vector>
#include <frc/smartdashboard/SmartDashboard.h>
#include <networktables/NetworkTableInstance.h>
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<photon::PhotonTrackedTarget>{}, 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));
}
}

4
photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
View File

@@ -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<size_t>(0));
std::vector<photon::PhotonTrackedTarget> targets;
for (photon::PhotonTrackedTarget tar : targetSpan) {
targets.push_back(tar);

6
shared/config.gradle
View File

@@ -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"
}
}
}
}