Create timesync JNI for testing client (#1433)

This commit is contained in:
Matt
2024-10-31 08:27:19 -07:00
committed by GitHub
parent 937bafa8e2
commit 37aaa49b32
69 changed files with 2252 additions and 368 deletions

View File

@@ -265,7 +265,7 @@ public class SqlConfigProvider extends ConfigProvider {
JacksonUtils.deserialize(
getOneConfigFile(conn, GlobalKeys.HARDWARE_CONFIG), HardwareConfig.class);
} catch (IOException e) {
logger.error("Could not deserialize hardware config! Loading defaults");
logger.error("Could not deserialize hardware config! Loading defaults", e);
hardwareConfig = new HardwareConfig();
}
@@ -274,7 +274,7 @@ public class SqlConfigProvider extends ConfigProvider {
JacksonUtils.deserialize(
getOneConfigFile(conn, GlobalKeys.HARDWARE_SETTINGS), HardwareSettings.class);
} catch (IOException e) {
logger.error("Could not deserialize hardware settings! Loading defaults");
logger.error("Could not deserialize hardware settings! Loading defaults", e);
hardwareSettings = new HardwareSettings();
}
@@ -283,7 +283,7 @@ public class SqlConfigProvider extends ConfigProvider {
JacksonUtils.deserialize(
getOneConfigFile(conn, GlobalKeys.NETWORK_CONFIG), NetworkConfig.class);
} catch (IOException e) {
logger.error("Could not deserialize network config! Loading defaults");
logger.error("Could not deserialize network config! Loading defaults", e);
networkConfig = new NetworkConfig();
}
@@ -292,7 +292,7 @@ public class SqlConfigProvider extends ConfigProvider {
JacksonUtils.deserialize(
getOneConfigFile(conn, GlobalKeys.ATFL_CONFIG_FILE), AprilTagFieldLayout.class);
} catch (IOException e) {
logger.error("Could not deserialize apriltag layout! Loading defaults");
logger.error("Could not deserialize apriltag layout! Loading defaults", e);
try {
atfl = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
} catch (UncheckedIOException e2) {

View File

@@ -20,7 +20,7 @@ package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEvent;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.networktables.NetworkTablesJNI;
import java.util.List;
import java.util.function.BooleanSupplier;
import java.util.function.Consumer;
@@ -146,13 +146,19 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
List.of(),
result.inputAndOutputFrame);
else acceptedResult = result;
var now = WPIUtilJNI.now();
var captureMicros = MathUtils.nanosToMicros(acceptedResult.getImageCaptureTimestampNanos());
var now = NetworkTablesJNI.now();
var captureMicros = MathUtils.nanosToMicros(result.getImageCaptureTimestampNanos());
var offset = NetworkTablesManager.getInstance().getOffset();
// Transform the metadata timestamps from the local nt::Now timebase to the Time Sync Server's
// timebase
var simplified =
new PhotonPipelineResult(
acceptedResult.sequenceID,
captureMicros,
now,
captureMicros + offset,
now + offset,
NetworkTablesManager.getInstance().getTimeSinceLastPong(),
TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets),
acceptedResult.multiTagResult);

View File

@@ -18,6 +18,7 @@
package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.networktables.LogMessage;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEvent;
import edu.wpi.first.networktables.NetworkTableEvent.Kind;
@@ -26,7 +27,6 @@ import edu.wpi.first.networktables.StringSubscriber;
import java.io.IOException;
import java.util.EnumSet;
import java.util.HashMap;
import java.util.function.Consumer;
import org.photonvision.PhotonVersion;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.configuration.NetworkConfig;
@@ -34,6 +34,7 @@ import org.photonvision.common.dataflow.DataChangeService;
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
import org.photonvision.common.hardware.HardwareManager;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.LogLevel;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.scripting.ScriptEventType;
import org.photonvision.common.scripting.ScriptManager;
@@ -41,32 +42,39 @@ import org.photonvision.common.util.TimedTaskManager;
import org.photonvision.common.util.file.JacksonUtils;
public class NetworkTablesManager {
private static final Logger logger =
new Logger(NetworkTablesManager.class, LogGroup.NetworkTables);
private final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
private final String kRootTableName = "/photonvision";
private final String kFieldLayoutName = "apriltag_field_layout";
public final NetworkTable kRootTable = ntInstance.getTable(kRootTableName);
private final NTLogger m_ntLogger = new NTLogger();
private boolean m_isRetryingConnection = false;
private StringSubscriber m_fieldLayoutSubscriber =
kRootTable.getStringTopic(kFieldLayoutName).subscribe("");
private final TimeSyncManager m_timeSync = new TimeSyncManager(kRootTable);
private NetworkTablesManager() {
ntInstance.addLogger(255, 255, (event) -> {}); // to hide error messages
ntInstance.addConnectionListener(true, m_ntLogger); // to hide error messages
ntInstance.addLogger(
LogMessage.kInfo, LogMessage.kCritical, this::logNtMessage); // to hide error messages
ntInstance.addConnectionListener(true, this::checkNtConnectState); // to hide error messages
ntInstance.addListener(
m_fieldLayoutSubscriber, EnumSet.of(Kind.kValueAll), this::onFieldLayoutChanged);
TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000);
// Get the UI state in sync with the backend. NT should fire a callback when it first connects
// to the robot
broadcastConnectedStatus();
}
public void registerTimedTasks() {
m_timeSync.start();
TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000);
}
private static NetworkTablesManager INSTANCE;
public static NetworkTablesManager getInstance() {
@@ -74,43 +82,72 @@ public class NetworkTablesManager {
return INSTANCE;
}
private static final Logger logger = new Logger(NetworkTablesManager.class, LogGroup.General);
private void logNtMessage(NetworkTableEvent event) {
String levelmsg = "DEBUG";
LogLevel pvlevel = LogLevel.DEBUG;
if (event.logMessage.level >= LogMessage.kCritical) {
pvlevel = LogLevel.ERROR;
levelmsg = "CRITICAL";
} else if (event.logMessage.level >= LogMessage.kError) {
pvlevel = LogLevel.ERROR;
levelmsg = "ERROR";
} else if (event.logMessage.level >= LogMessage.kWarning) {
pvlevel = LogLevel.WARN;
levelmsg = "WARNING";
} else if (event.logMessage.level >= LogMessage.kInfo) {
pvlevel = LogLevel.INFO;
levelmsg = "INFO";
}
private static class NTLogger implements Consumer<NetworkTableEvent> {
private boolean hasReportedConnectionFailure = false;
logger.log(
"NT: "
+ levelmsg
+ " "
+ event.logMessage.level
+ ": "
+ event.logMessage.message
+ " ("
+ event.logMessage.filename
+ ":"
+ event.logMessage.line
+ ")",
pvlevel);
}
@Override
public void accept(NetworkTableEvent event) {
var isConnEvent = event.is(Kind.kConnected);
var isDisconnEvent = event.is(Kind.kDisconnected);
public void checkNtConnectState(NetworkTableEvent event) {
var isConnEvent = event.is(Kind.kConnected);
var isDisconnEvent = event.is(Kind.kDisconnected);
if (!hasReportedConnectionFailure && isDisconnEvent) {
var msg =
String.format(
"NT lost connection to %s:%d! (NT version %d). Will retry in background.",
event.connInfo.remote_ip,
event.connInfo.remote_port,
event.connInfo.protocol_version);
logger.error(msg);
HardwareManager.getInstance().setNTConnected(false);
if (isDisconnEvent) {
var msg =
String.format(
"NT lost connection to %s:%d! (NT version %d). Will retry in background.",
event.connInfo.remote_ip,
event.connInfo.remote_port,
event.connInfo.protocol_version);
logger.error(msg);
HardwareManager.getInstance().setNTConnected(false);
hasReportedConnectionFailure = true;
getInstance().broadcastConnectedStatus();
} else if (isConnEvent && event.connInfo != null) {
var msg =
String.format(
"NT connected to %s:%d! (NT version %d)",
event.connInfo.remote_ip,
event.connInfo.remote_port,
event.connInfo.protocol_version);
logger.info(msg);
HardwareManager.getInstance().setNTConnected(true);
getInstance().broadcastConnectedStatus();
} else if (isConnEvent && event.connInfo != null) {
var msg =
String.format(
"NT connected to %s:%d! (NT version %d)",
event.connInfo.remote_ip,
event.connInfo.remote_port,
event.connInfo.protocol_version);
logger.info(msg);
HardwareManager.getInstance().setNTConnected(true);
hasReportedConnectionFailure = false;
ScriptManager.queueEvent(ScriptEventType.kNTConnected);
getInstance().broadcastVersion();
getInstance().broadcastConnectedStatus();
}
ScriptManager.queueEvent(ScriptEventType.kNTConnected);
getInstance().broadcastVersion();
getInstance().broadcastConnectedStatus();
m_timeSync.reportNtConnected();
} else if (isConnEvent) {
logger.warn("Got connection event with no connection info??");
} else {
logger.warn("Got a non-sensical connection message that is neither connect nor disconnect?");
}
}
@@ -168,9 +205,16 @@ public class NetworkTablesManager {
} else {
setClientMode(config.ntServerAddress);
}
m_timeSync.setConfig(config);
broadcastVersion();
}
public long getOffset() {
return m_timeSync.getOffset();
}
private void setClientMode(String ntServerAddress) {
ntInstance.stopServer();
ntInstance.startClient4("photonvision");
@@ -211,4 +255,8 @@ public class NetworkTablesManager {
"[NetworkTablesManager] Could not connect to the robot! Will retry in the background...");
}
}
public long getTimeSinceLastPong() {
return m_timeSync.getTimeSinceLastPong();
}
}

View File

@@ -0,0 +1,169 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.cscore.CameraServerJNI;
import edu.wpi.first.networktables.IntegerPublisher;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import org.photonvision.common.configuration.NetworkConfig;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.TimedTaskManager;
import org.photonvision.jni.PhotonTargetingJniLoader;
import org.photonvision.jni.TimeSyncClient;
import org.photonvision.jni.TimeSyncServer;
public class TimeSyncManager {
private static final Logger logger = new Logger(TimeSyncManager.class, LogGroup.NetworkTables);
private TimeSyncClient m_client = null;
private TimeSyncServer m_server = null;
private NetworkTableInstance ntInstance;
IntegerPublisher m_offsetPub;
IntegerPublisher m_rtt2Pub;
IntegerPublisher m_pingsPub;
IntegerPublisher m_pongsPub;
IntegerPublisher m_lastPongTimePub;
public TimeSyncManager(NetworkTable kRootTable) {
if (!PhotonTargetingJniLoader.isWorking) {
logger.error("PhotonTargetingJNI was not loaded! Cannot do time-sync");
}
this.ntInstance = kRootTable.getInstance();
// Need this subtable to be unique per coprocessor. TODO: consider using MAC address or
// something similar for metrics?
var timeTable = kRootTable.getSubTable(".timesync").getSubTable(CameraServerJNI.getHostname());
m_offsetPub = timeTable.getIntegerTopic("offset_us").publish();
m_rtt2Pub = timeTable.getIntegerTopic("rtt2_us").publish();
m_pingsPub = timeTable.getIntegerTopic("ping_tx_count").publish();
m_pongsPub = timeTable.getIntegerTopic("pong_rx_count").publish();
m_lastPongTimePub = timeTable.getIntegerTopic("pong_rx_time_us").publish();
// default to being a client
logger.debug("Starting TimeSyncClient on localhost (for now)");
m_client = new TimeSyncClient("127.0.0.1", 5810, 1.0);
}
// Since we're spinning off tasks in a new thread, be careful and start it seperately
public void start() {
if (!PhotonTargetingJniLoader.isWorking) {
logger.error("PhotonTargetingJNI was not loaded! Cannot start");
}
TimedTaskManager.getInstance().addTask("TimeSyncManager::tick", this::tick, 1000);
}
public synchronized long getOffset() {
if (!PhotonTargetingJniLoader.isWorking) {
return 0;
}
// if we're a client, return the offset to server time
if (m_client != null) return m_client.getOffset();
// if we're a server, our time (nt::Now) is the same as network time
if (m_server != null) return 0;
// ????? should never hit
logger.error("Client and server and null?");
return 0;
}
synchronized void setConfig(NetworkConfig config) {
if (!PhotonTargetingJniLoader.isWorking) {
return;
}
if (m_client == null && m_server == null) {
throw new RuntimeException("Neither client nor server are null?");
}
// if not already running a server, set it up
if (config.runNTServer && m_server == null) {
// tear down anything old
if (m_client != null) {
logger.debug("Tearing down old client");
m_client.stop();
m_client = null;
}
logger.debug("Starting TimeSyncServer");
m_server = new TimeSyncServer(5810);
m_server.start();
} else
// if not already running a client, set it up
if (m_client == null) {
// tear down anything old
if (m_server != null) {
logger.debug("Tearing down old server");
m_server.stop();
m_server = null;
}
// Guess at IP -- tick will take care of changing this (may take up to 1 second)
logger.debug("Starting TimeSyncClient on localhost (for now)");
m_client = new TimeSyncClient("127.0.0.1", 5810, 1.0);
}
}
synchronized void tick() {
if (m_client != null) {
var conns = ntInstance.getConnections();
if (conns.length > 0) {
logger.debug("Changing TimeSyncClient server to " + conns[0].remote_ip);
m_client.setServer(conns[0].remote_ip);
}
if (m_client != null) {
var m = m_client.getPingMetadata();
m_offsetPub.set(m.offset);
m_rtt2Pub.set(m.rtt2);
m_pingsPub.set(m.pingsSent);
m_pongsPub.set(m.pongsReceived);
m_lastPongTimePub.set(m.lastPongTime);
}
}
}
public synchronized long getTimeSinceLastPong() {
if (m_client != null) {
return m_client.getPingMetadata().timeSinceLastPong();
} else if (m_server != null) {
return 0;
} else {
// ????
return 0;
}
}
/** Restart our timesync client if NT just connected */
public synchronized void reportNtConnected() {
if (m_client != null) {
// restart (in java code; we could just add a reset metrics function...)
logger.debug(
"NT (re)connected -- restarting Time Sync Client at " + m_client.getServer() + ":5810");
m_client.stop();
m_client = new TimeSyncClient(m_client.getServer(), 5810, 1.0);
}
}
}

View File

@@ -25,4 +25,5 @@ public enum LogGroup {
General,
Config,
CSCore,
NetworkTables,
}

View File

@@ -100,6 +100,7 @@ public class Logger {
levelMap.put(LogGroup.VisionModule, LogLevel.INFO);
levelMap.put(LogGroup.Config, LogLevel.INFO);
levelMap.put(LogGroup.CSCore, LogLevel.TRACE);
levelMap.put(LogGroup.NetworkTables, LogLevel.DEBUG);
}
static {
@@ -200,7 +201,7 @@ public class Logger {
return logLevel.code <= levelMap.get(group).code;
}
void log(String message, LogLevel level) {
public void log(String message, LogLevel level) {
if (shouldLog(level)) {
log(message, level, group, className);
}

View File

@@ -18,76 +18,20 @@
package org.photonvision.common.util;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.apriltag.jni.AprilTagJNI;
import edu.wpi.first.cscore.CameraServerJNI;
import edu.wpi.first.cscore.OpenCvLoader;
import edu.wpi.first.hal.JNIWrapper;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.jni.ArmFeedforwardJNI;
import edu.wpi.first.math.jni.DAREJNI;
import edu.wpi.first.math.jni.EigenJNI;
import edu.wpi.first.math.jni.Ellipse2dJNI;
import edu.wpi.first.math.jni.Pose3dJNI;
import edu.wpi.first.math.jni.StateSpaceUtilJNI;
import edu.wpi.first.math.jni.TrajectoryUtilJNI;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.net.WPINetJNI;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.util.CombinedRuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
import java.awt.HeadlessException;
import java.io.File;
import java.io.IOException;
import java.nio.file.Path;
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.highgui.HighGui;
import org.photonvision.jni.WpilibLoader;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
public class TestUtils {
private static boolean has_loaded = false;
public static boolean loadLibraries() {
if (has_loaded) return true;
NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
CameraServerJNI.Helper.setExtractOnStaticLoad(false);
OpenCvLoader.Helper.setExtractOnStaticLoad(false);
JNIWrapper.Helper.setExtractOnStaticLoad(false);
WPINetJNI.Helper.setExtractOnStaticLoad(false);
AprilTagJNI.Helper.setExtractOnStaticLoad(false);
// wpimathjni is a bit odd, it's all in the wpimathjni shared lib, but the java side stuff has
// been split.
ArmFeedforwardJNI.Helper.setExtractOnStaticLoad(false);
DAREJNI.Helper.setExtractOnStaticLoad(false);
EigenJNI.Helper.setExtractOnStaticLoad(false);
Ellipse2dJNI.Helper.setExtractOnStaticLoad(false);
Pose3dJNI.Helper.setExtractOnStaticLoad(false);
StateSpaceUtilJNI.Helper.setExtractOnStaticLoad(false);
TrajectoryUtilJNI.Helper.setExtractOnStaticLoad(false);
try {
CombinedRuntimeLoader.loadLibraries(
TestUtils.class,
"wpiutiljni",
"wpimathjni",
"ntcorejni",
"wpinetjni",
"wpiHaljni",
"cscorejni",
"apriltagjni");
CombinedRuntimeLoader.loadLibraries(TestUtils.class, Core.NATIVE_LIBRARY_NAME);
has_loaded = true;
} catch (IOException e) {
e.printStackTrace();
has_loaded = false;
}
return has_loaded;
return WpilibLoader.loadLibraries();
}
@SuppressWarnings("unused")

View File

@@ -33,6 +33,7 @@ import java.io.IOException;
import java.nio.file.Path;
import java.util.HashMap;
import java.util.Map;
import org.eclipse.jetty.io.EofException;
public class JacksonUtils {
public static class UIMap extends HashMap<String, Object> {}
@@ -76,6 +77,10 @@ public class JacksonUtils {
}
public static <T> T deserialize(String s, Class<T> ref) throws IOException {
if (s.length() == 0) {
throw new EofException("Provided empty string for class " + ref.getName());
}
PolymorphicTypeValidator ptv =
BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build();
ObjectMapper objectMapper =

View File

@@ -25,7 +25,7 @@ import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.networktables.NetworkTablesJNI;
import java.util.Arrays;
import java.util.List;
import org.opencv.core.Core;
@@ -98,7 +98,7 @@ public class MathUtils {
}
public static long wpiNanoTime() {
return microsToNanos(WPIUtilJNI.now());
return microsToNanos(NetworkTablesJNI.now());
}
/**

View File

@@ -42,18 +42,18 @@ public class USBFrameProvider extends CpuImageProcessor {
@Override
public CapturedFrame getInputMat() {
var mat = new CVMat(); // We do this so that we don't fill a Mat in use by another thread
// This is from wpi::Now, or WPIUtilJNI.now()
long time =
cvSink.grabFrame(mat.getMat())
* 1000; // Units are microseconds, epoch is the same as the Unix epoch
// We allocate memory so we don't fill a Mat in use by another thread (memory model is easier)
var mat = new CVMat();
// This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since
// Hal::initialize was called
long captureTimeNs = cvSink.grabFrame(mat.getMat()) * 1000;
if (time == 0) {
if (captureTimeNs == 0) {
var error = cvSink.getError();
logger.error("Error grabbing image: " + error);
}
return new CapturedFrame(mat, settables.getFrameStaticProperties(), time);
return new CapturedFrame(mat, settables.getFrameStaticProperties(), captureTimeNs);
}
@Override

View File

@@ -18,8 +18,10 @@
package org.photonvision.vision.processes;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.Assertions.fail;
import edu.wpi.first.cscore.VideoMode;
import java.io.IOException;
import java.util.Arrays;
import java.util.HashMap;
import java.util.List;
@@ -31,6 +33,7 @@ import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.dataflow.CVPipelineResultConsumer;
import org.photonvision.common.util.TestUtils;
import org.photonvision.jni.PhotonTargetingJniLoader;
import org.photonvision.vision.camera.QuirkyCamera;
import org.photonvision.vision.camera.USBCameras.USBCameraSource;
import org.photonvision.vision.frame.FrameProvider;
@@ -41,7 +44,16 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult;
public class VisionModuleManagerTest {
@BeforeAll
public static void init() {
String classpathStr = System.getProperty("java.class.path");
System.out.print(classpathStr);
TestUtils.loadLibraries();
try {
if (!PhotonTargetingJniLoader.load()) fail();
} catch (UnsatisfiedLinkError | IOException e) {
e.printStackTrace();
fail(e);
}
}
private static class TestSource extends VisionSource {