mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-24 01:31:44 +00:00
Create timesync JNI for testing client (#1433)
This commit is contained in:
@@ -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) {
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -25,4 +25,5 @@ public enum LogGroup {
|
||||
General,
|
||||
Config,
|
||||
CSCore,
|
||||
NetworkTables,
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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")
|
||||
|
||||
@@ -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 =
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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 {
|
||||
|
||||
Reference in New Issue
Block a user