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

Create timesync JNI for testing client (#1433)

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files

2
photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java
View File

@@ -87,11 +87,9 @@ public class PacketPublisher<T> implements AutoCloseable {
instance.addSchema(typeString, "photonstructschema", struct.getSchema());
for (var inner : struct.getNestedPhotonMessages()) {
System.out.println(inner.getTypeString());
addSchemaImpl(inner, seen);
}
for (var inner : struct.getNestedWpilibMessages()) {
System.out.println(inner.getTypeString());
instance.addSchema(inner);
}
seen.remove(typeString);

88
photon-targeting/src/main/java/org/photonvision/jni/PhotonTargetingJniLoader.java Normal file
View File

@@ -0,0 +1,88 @@
/*
* 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.jni;
import edu.wpi.first.util.RuntimeDetector;
import edu.wpi.first.util.RuntimeLoader;
import java.io.File;
import java.io.FileOutputStream;
import java.io.IOException;
import java.nio.file.Files;
import java.util.List;
import org.photonvision.common.hardware.Platform;
public class PhotonTargetingJniLoader {
public static boolean isWorking = false;
public static boolean load() throws IOException, UnsatisfiedLinkError {
if (isWorking) return true;
isWorking = load_();
return isWorking;
}
public static boolean load_() throws IOException, UnsatisfiedLinkError {
// We always extract the shared object (we could hash each so, but that's a lot
// of work)
String arch_name = Platform.getNativeLibraryFolderName();
var clazz = PhotonTargetingJniLoader.class;
for (var libraryName : List.of("photontargeting", "photontargetingJNI")) {
if (RuntimeDetector.isAthena()) {
System.out.println("Detected rio - loading directly");
RuntimeLoader.loadLibrary(libraryName);
continue;
}
var nativeLibName = System.mapLibraryName(libraryName);
var path = "/nativelibraries/" + arch_name + "/" + nativeLibName;
var in = clazz.getResourceAsStream(path);
if (in == null) {
System.err.println("Could not get resource at path " + path);
return false;
}
// It's important that we don't mangle the names of these files on Windows at
// least
var tempfolder = Files.createTempDirectory("nativeextract");
File temp = new File(tempfolder.toAbsolutePath().toString(), nativeLibName);
System.out.println(temp.getAbsolutePath().toString());
FileOutputStream fos = new FileOutputStream(temp);
int read = -1;
byte[] buffer = new byte[1024];
while ((read = in.read(buffer)) != -1) {
fos.write(buffer, 0, read);
}
fos.close();
in.close();
try {
System.load(temp.getAbsolutePath());
} catch (Throwable t) {
System.err.println("Unable to System.load " + temp.getName() + " : " + t.getMessage());
t.printStackTrace();
return false;
}
System.out.println("Successfully loaded shared object " + temp.getName());
}
return true;
}
}

156
photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java Normal file
View File

@@ -0,0 +1,156 @@
/*
* 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.jni;
import edu.wpi.first.networktables.NetworkTablesJNI;
/**
* Send ping-pongs to estimate server time, relative to nt::Now. The underlying implementation does
* technically allow us to provide a different source, but all photon code assumes nt::Now is used
*/
public class TimeSyncClient {
public static class PingMetadata {
// offset, us
public long offset;
// outgoing count
public long pingsSent;
// incoming count
public long pongsReceived;
// when we last heard back from the server
public long lastPongTime;
// RTT2, time from ping send to pong recieve at the client
public long rtt2;
public PingMetadata(
long offset, long pingsSent, long pongsReceived, long lastPongTime, long rtt2) {
this.offset = offset;
this.pingsSent = pingsSent;
this.pongsReceived = pongsReceived;
this.lastPongTime = lastPongTime;
this.rtt2 = rtt2;
}
@Override
public String toString() {
return "PingMetadata [offset="
+ offset
+ ", pingsSent="
+ pingsSent
+ ", pongsReceived="
+ pongsReceived
+ ", lastPongTime="
+ lastPongTime
+ ", rtt2="
+ rtt2
+ "]";
}
/**
* How long, in us, since we last heard back from the server
*
* @return Time between last pong RX and now, or Long.MAX_VALUE if we have heard zero pongs
*/
public long timeSinceLastPong() {
// If no pongs, it's been forever
if (pongsReceived < 1) {
return Long.MAX_VALUE;
}
return NetworkTablesJNI.now() - lastPongTime;
}
}
private final Object mutex = new Object();
private long handle;
private String server;
private int port;
private double interval;
public TimeSyncClient(String server, int port, double interval) {
this.server = server;
this.port = port;
this.interval = interval;
synchronized (mutex) {
this.handle = TimeSyncClient.create(server, port, interval);
TimeSyncClient.start(handle);
}
}
public void setServer(String newServer) {
if (!server.equals(newServer)) {
synchronized (mutex) {
stop();
this.handle = TimeSyncClient.create(newServer, port, interval);
TimeSyncClient.start(handle);
this.server = newServer;
}
}
}
public void stop() {
synchronized (mutex) {
if (handle > 0) {
TimeSyncClient.stop(handle);
handle = 0;
}
}
}
/**
* This offset, when added to the current value of nt::now(), yields the timestamp in the timebase
* of the TSP Server
*
* @return
*/
public long getOffset() {
synchronized (mutex) {
return TimeSyncClient.getOffset(handle);
}
}
/**
* Best estimate of the current timestamp at the TSP server
*
* @return The current time estimate, in microseconds, at the TSP server
*/
public long currentServerTimestamp() {
return NetworkTablesJNI.now() + getOffset();
}
public PingMetadata getPingMetadata() {
synchronized (mutex) {
return TimeSyncClient.getLatestMetadata(handle);
}
}
public String getServer() {
return server;
}
private static native long create(String serverIP, int serverPort, double pingIntervalSeconds);
private static native void start(long handle);
private static native void stop(long handle);
private static native long getOffset(long handle);
private static native PingMetadata getLatestMetadata(long handle);
}

43
photon-targeting/src/main/java/org/photonvision/jni/TimeSyncServer.java Normal file
View File

@@ -0,0 +1,43 @@
/*
* 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.jni;
public class TimeSyncServer {
private long handle;
public TimeSyncServer(int port) {
this.handle = TimeSyncServer.create(port);
}
public void start() {
TimeSyncServer.start(handle);
}
public void stop() {
if (handle > 0) {
TimeSyncServer.stop(handle);
handle = 0;
}
}
private static native long create(int port);
private static native void start(long handle);
private static native void stop(long handle);
}

22
photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java
View File

@@ -22,24 +22,30 @@ import org.photonvision.struct.PhotonPipelineMetadataSerde;
import org.photonvision.targeting.serde.PhotonStructSerializable;
public class PhotonPipelineMetadata implements PhotonStructSerializable<PhotonPipelineMetadata> {
// Mirror of the heartbeat entry -- monotonically increasing
public long sequenceID;
// Image capture and NT publish timestamp, in microseconds and in the
// coprocessor timebase. As
// reported by WPIUtilJNI::now.
// Image capture and NT publish timestamp, in microseconds
// The timebase is nt::Now on the time sync server
public long captureTimestampMicros;
public long publishTimestampMicros;
// Mirror of the heartbeat entry -- monotonically increasing
public long sequenceID;
// Time from last Time Sync Pong received and the construction of this metadata
public long timeSinceLastPong;
public PhotonPipelineMetadata(
long captureTimestampMicros, long publishTimestampMicros, long sequenceID) {
long captureTimestampMicros,
long publishTimestampMicros,
long sequenceID,
long timeSinceLastPong) {
this.captureTimestampMicros = captureTimestampMicros;
this.publishTimestampMicros = publishTimestampMicros;
this.sequenceID = sequenceID;
this.timeSinceLastPong = timeSinceLastPong;
}
public PhotonPipelineMetadata() {
this(-1, -1, -1);
this(-1, -1, -1, Long.MAX_VALUE);
}
/** Returns the time between image capture and publish to NT */

47
photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
View File

@@ -40,9 +40,6 @@ public class PhotonPipelineResult
// Multi-tag result
public Optional<MultiTargetPNPResult> multitagResult;
// HACK: Since we don't trust NT time sync, keep track of when we got this packet into robot code
public long ntReceiveTimestampMicros = -1;
/** Constructs an empty pipeline result. */
public PhotonPipelineResult() {
this(new PhotonPipelineMetadata(), List.of(), Optional.empty());
@@ -52,19 +49,21 @@ public class PhotonPipelineResult
* Constructs a pipeline result.
*
* @param sequenceID The number of frames processed by this camera since boot
* @param captureTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor
* captured the image this result contains the targeting info of
* @param publishTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor
* published targeting info
* @param captureTimestampMicros The time, in uS in the coprocessor's timebase, that the
* coprocessor captured the image this result contains the targeting info of
* @param publishTimestampMicros The time, in uS in the coprocessor's timebase, that the
* coprocessor published targeting info
* @param targets The list of targets identified by the pipeline.
*/
public PhotonPipelineResult(
long sequenceID,
long captureTimestamp,
long publishTimestamp,
long captureTimestampMicros,
long publishTimestampMicros,
long timeSinceLastPong,
List<PhotonTrackedTarget> targets) {
this(
new PhotonPipelineMetadata(captureTimestamp, publishTimestamp, sequenceID),
new PhotonPipelineMetadata(
captureTimestampMicros, publishTimestampMicros, sequenceID, timeSinceLastPong),
targets,
Optional.empty());
}
@@ -84,10 +83,12 @@ public class PhotonPipelineResult
long sequenceID,
long captureTimestamp,
long publishTimestamp,
long timeSinceLastPong,
List<PhotonTrackedTarget> targets,
Optional<MultiTargetPNPResult> result) {
this(
new PhotonPipelineMetadata(captureTimestamp, publishTimestamp, sequenceID),
new PhotonPipelineMetadata(
captureTimestamp, publishTimestamp, sequenceID, timeSinceLastPong),
targets,
result);
}
@@ -162,26 +163,14 @@ public class PhotonPipelineResult
}
/**
* Returns the estimated time the frame was taken, in the Received system's time base. This is
* calculated as (NT Receive time (robot base) - (publish timestamp, coproc timebase - capture
* timestamp, coproc timebase))
* Returns the estimated time the frame was taken, in the Time Sync Server's time base (nt::Now).
* This is calculated using the estiamted offset between Time Sync Server time and local time. The
* robot shall run a server, so the offset shall be 0.
*
* @return The timestamp in seconds
*/
public double getTimestampSeconds() {
return (ntReceiveTimestampMicros
- (metadata.publishTimestampMicros - metadata.captureTimestampMicros))
/ 1e6;
}
/** The time that the robot Received this result, in the FPGA timebase. */
public long getNtReceiveTimestampMicros() {
return ntReceiveTimestampMicros;
}
/** Sets the FPGA timestamp this result was Received by robot code */
public void setReceiveTimestampMicros(long timestampMicros) {
this.ntReceiveTimestampMicros = timestampMicros;
return metadata.captureTimestampMicros / 1e6;
}
@Override
@@ -192,8 +181,6 @@ public class PhotonPipelineResult
+ targets
+ ", multitagResult="
+ multitagResult
+ ", ntReceiveTimestampMicros="
+ ntReceiveTimestampMicros
+ "]";
}
@@ -204,7 +191,6 @@ public class PhotonPipelineResult
result = prime * result + ((metadata == null) ? 0 : metadata.hashCode());
result = prime * result + ((targets == null) ? 0 : targets.hashCode());
result = prime * result + ((multitagResult == null) ? 0 : multitagResult.hashCode());
result = prime * result + (int) (ntReceiveTimestampMicros ^ (ntReceiveTimestampMicros >>> 32));
return result;
}
@@ -223,7 +209,6 @@ public class PhotonPipelineResult
if (multitagResult == null) {
if (other.multitagResult != null) return false;
} else if (!multitagResult.equals(other.multitagResult)) return false;
if (ntReceiveTimestampMicros != other.ntReceiveTimestampMicros) return false;
return true;
}

1
photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java
View File

@@ -48,6 +48,7 @@ public class PhotonPipelineResultProto
msg.getSequenceId(),
msg.getCaptureTimestampMicros(),
msg.getNtPublishTimestampMicros(),
msg.getTimeSinceLastPongMicros(),
PhotonTrackedTarget.proto.unpack(msg.getTargets()),
msg.hasMultiTargetResult()
? Optional.of(MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult()))