Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-25 01:41:40 +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

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