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

86
photon-targeting/src/generated/main/java/org/photonvision/jni/WpilibLoader.java Normal file
View File

@@ -0,0 +1,86 @@
/*
* 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 java.io.IOException;
import org.opencv.core.Core;
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.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.net.WPINetJNI;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.util.CombinedRuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
public class WpilibLoader {
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(
WpilibLoader.class,
"wpiutiljni",
"wpilibc",
"wpimathjni",
"ntcorejni",
"wpinetjni",
"wpiHaljni",
"wpi",
"cscorejni",
"apriltagjni");
CombinedRuntimeLoader.loadLibraries(WpilibLoader.class, Core.NATIVE_LIBRARY_NAME);
has_loaded = true;
} catch (IOException e) {
e.printStackTrace();
has_loaded = false;
}
return has_loaded;
}
}

10
photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java
View File

@@ -35,9 +35,9 @@ import edu.wpi.first.util.struct.Struct;
*/
public class PhotonPipelineMetadataSerde implements PacketSerde<PhotonPipelineMetadata> {
@Override
public final String getInterfaceUUID() { return "626e70461cbdb274fb43ead09c255f4e"; }
public final String getInterfaceUUID() { return "ac0a45f686457856fb30af77699ea356"; }
@Override
public final String getSchema() { return "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;"; }
public final String getSchema() { return "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;int64 timeSinceLastPong;"; }
@Override
public final String getTypeName() { return "PhotonPipelineMetadata"; }
@@ -57,6 +57,9 @@ public class PhotonPipelineMetadataSerde implements PacketSerde<PhotonPipelineMe
// field publishTimestampMicros is of intrinsic type int64
packet.encode((long) value.publishTimestampMicros);
// field timeSinceLastPong is of intrinsic type int64
packet.encode((long) value.timeSinceLastPong);
}
@Override
@@ -72,6 +75,9 @@ public class PhotonPipelineMetadataSerde implements PacketSerde<PhotonPipelineMe
// publishTimestampMicros is of intrinsic type int64
ret.publishTimestampMicros = packet.decodeLong();
// timeSinceLastPong is of intrinsic type int64
ret.timeSinceLastPong = packet.decodeLong();
return ret;
}

6
photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java
View File

@@ -35,9 +35,9 @@ import edu.wpi.first.util.struct.Struct;
*/
public class PhotonPipelineResultSerde implements PacketSerde<PhotonPipelineResult> {
@Override
public final String getInterfaceUUID() { return "5eeaa293d0c69aea90eaddea786a2b3b"; }
public final String getInterfaceUUID() { return "4b2ff16a964b5e2bf04be0c1454d91c4"; }
@Override
public final String getSchema() { return "PhotonPipelineMetadata:626e70461cbdb274fb43ead09c255f4e metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"; }
public final String getSchema() { return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"; }
@Override
public final String getTypeName() { return "PhotonPipelineResult"; }
@@ -78,7 +78,7 @@ public class PhotonPipelineResultSerde implements PacketSerde<PhotonPipelineResu
@Override
public PacketSerde<?>[] getNestedPhotonMessages() {
return new PacketSerde<?>[] {
PhotonPipelineMetadata.photonStruct,MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct
MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct,PhotonPipelineMetadata.photonStruct
};
}

2
photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineMetadataSerde.cpp
View File

@@ -28,6 +28,7 @@ void StructType::Pack(Packet& packet, const PhotonPipelineMetadata& value) {
packet.Pack<int64_t>(value.sequenceID);
packet.Pack<int64_t>(value.captureTimestampMicros);
packet.Pack<int64_t>(value.publishTimestampMicros);
packet.Pack<int64_t>(value.timeSinceLastPong);
}
PhotonPipelineMetadata StructType::Unpack(Packet& packet) {
@@ -35,6 +36,7 @@ PhotonPipelineMetadata StructType::Unpack(Packet& packet) {
.sequenceID = packet.Unpack<int64_t>(),
.captureTimestampMicros = packet.Unpack<int64_t>(),
.publishTimestampMicros = packet.Unpack<int64_t>(),
.timeSinceLastPong = packet.Unpack<int64_t>(),
}};
}

4
photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h
View File

@@ -34,12 +34,12 @@ namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<PhotonPipelineMetadata> {
static constexpr std::string_view GetSchemaHash() {
return "626e70461cbdb274fb43ead09c255f4e";
return "ac0a45f686457856fb30af77699ea356";
}
static constexpr std::string_view GetSchema() {
return "int64 sequenceID;int64 captureTimestampMicros;int64 "
"publishTimestampMicros;";
"publishTimestampMicros;int64 timeSinceLastPong;";
}
static photon::PhotonPipelineMetadata Unpack(photon::Packet& packet);

4
photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h
View File

@@ -39,11 +39,11 @@ namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<PhotonPipelineResult> {
static constexpr std::string_view GetSchemaHash() {
return "5eeaa293d0c69aea90eaddea786a2b3b";
return "4b2ff16a964b5e2bf04be0c1454d91c4";
}
static constexpr std::string_view GetSchema() {
return "PhotonPipelineMetadata:626e70461cbdb274fb43ead09c255f4e "
return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 "
"metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 "
"targets[?];optional "
"MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b "

1
photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineMetadataStruct.h
View File

@@ -29,6 +29,7 @@ struct PhotonPipelineMetadata_PhotonStruct {
int64_t sequenceID;
int64_t captureTimestampMicros;
int64_t publishTimestampMicros;
int64_t timeSinceLastPong;
friend bool operator==(PhotonPipelineMetadata_PhotonStruct const&,
PhotonPipelineMetadata_PhotonStruct const&) = default;

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

200
photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp Normal file
View File

@@ -0,0 +1,200 @@
/*
* 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/>.
*/
#include "net/TimeSyncClient.h"
#include <fmt/core.h>
#include <wpinet/UDPClient.h>
#include <wpinet/uv/util.h>
#include <atomic>
#include <chrono>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <iostream>
#include <mutex>
#include <thread>
#include <Eigen/Core>
#include <wpi/Logger.h>
#include <wpi/struct/Struct.h>
#include "ntcore_cpp.h"
static void ClientLoggerFunc(unsigned int level, const char* file,
unsigned int line, const char* msg) {
if (level == 20) {
fmt::print(stderr, "TimeSyncClient: {}\n", msg);
return;
}
std::string_view levelmsg;
if (level >= 50) {
levelmsg = "CRITICAL";
} else if (level >= 40) {
levelmsg = "ERROR";
} else if (level >= 30) {
levelmsg = "WARNING";
} else {
return;
}
fmt::print(stderr, "TimeSyncClient: {}: {} ({}:{})\n", levelmsg, msg, file,
line);
}
void wpi::tsp::TimeSyncClient::Tick() {
// fmt::println("wpi::tsp::TimeSyncClient::Tick");
// Regardless of if we've gotten a pong back yet, we'll ping again. this is
// pretty naive but should be "fine" for now?
uint64_t ping_local_time{m_timeProvider()};
TspPing ping{.version = 1, .message_id = 1, .client_time = ping_local_time};
wpi::SmallVector<uint8_t, wpi::Struct<TspPing>::GetSize()> pingData(
wpi::Struct<TspPing>::GetSize());
wpi::PackStruct(pingData, ping);
// Wrap our buffer - pingData should free itself
wpi::uv::Buffer pingBuf{pingData};
int sent = m_udp->TrySend(wpi::SmallVector<wpi::uv::Buffer, 1>{pingBuf});
if (static_cast<size_t>(sent) != wpi::Struct<TspPing>::GetSize()) {
WPI_ERROR(m_logger, "Didn't send the whole ping out? sent {} bytes", sent);
return;
}
{
std::lock_guard lock{m_offsetMutex};
m_metadata.pingsSent++;
}
m_lastPing = ping;
}
void wpi::tsp::TimeSyncClient::UdpCallback(uv::Buffer& buf, size_t nbytes,
const sockaddr& sender,
unsigned flags) {
uint64_t pong_local_time = m_timeProvider();
if (static_cast<size_t>(nbytes) != wpi::Struct<TspPong>::GetSize()) {
WPI_ERROR(m_logger, "Got {} bytes for pong?", nbytes);
return;
}
TspPong pong{
wpi::UnpackStruct<TspPong>(buf.bytes()),
};
// fmt::println("->[client] Got pong: {} {} {} {}", pong.version,
// pong.message_id, pong.client_time, pong.server_time);
if (pong.version != 1) {
fmt::println("Bad version from server?");
return;
}
if (pong.message_id != 2) {
fmt::println("Bad message id from server?");
return;
}
TspPing ping = m_lastPing;
if (pong.client_time != ping.client_time) {
WPI_WARNING(m_logger,
"Pong was not a reply to our ping? Got ping {} vs pong {}",
ping.client_time, pong.client_time);
return;
}
// when time = send_time+rtt2/2, server time = server time
// server time = local time + offset
// offset = (server time - local time) = (server time) - (send_time +
// rtt2/2)
auto rtt2 = pong_local_time - ping.client_time;
int64_t serverTimeOffsetUs = pong.server_time - rtt2 / 2 - ping.client_time;
auto filtered = m_lastOffsets.Calculate(serverTimeOffsetUs);
// fmt::println("Ping-ponged! RTT2 {} uS, offset {}/filtered offset {} uS",
// rtt2,
// serverTimeOffsetUs, filtered);
{
std::lock_guard lock{m_offsetMutex};
m_metadata.offset = filtered;
m_metadata.rtt2 = rtt2;
m_metadata.pongsReceived++;
m_metadata.lastPongTime = pong_local_time;
}
using std::cout;
// fmt::println("Ping-ponged! RTT2 {} uS, offset {} uS", rtt2,
// serverTimeOffsetUs);
// fmt::println("Estimated server time {} s",
// (m_timeProvider() + serverTimeOffsetUs) / 1000000.0);
}
wpi::tsp::TimeSyncClient::TimeSyncClient(std::string_view server,
int remote_port,
std::chrono::milliseconds ping_delay,
std::function<uint64_t()> timeProvider)
: m_logger(::ClientLoggerFunc),
m_timeProvider(timeProvider),
m_udp{wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)},
m_pingTimer{wpi::uv::Timer::Create(m_loopRunner.GetLoop())},
m_serverIP{server},
m_serverPort{remote_port},
m_loopDelay(ping_delay) {
struct sockaddr_in serverAddr;
uv::NameToAddr(m_serverIP, m_serverPort, &serverAddr);
m_loopRunner.ExecSync(
[this, serverAddr](uv::Loop&) { m_udp->Connect(serverAddr); });
// fmt::println("Starting client (with server address {}:{})", server,
// remote_port);
}
void wpi::tsp::TimeSyncClient::Start() {
// fmt::println("Connecting received");
m_loopRunner.ExecSync([this](uv::Loop&) {
m_udp->received.connect(&wpi::tsp::TimeSyncClient::UdpCallback, this);
m_udp->StartRecv();
});
// fmt::println("Starting pinger");
using namespace std::chrono_literals;
m_pingTimer->timeout.connect(&wpi::tsp::TimeSyncClient::Tick, this);
m_loopRunner.ExecSync(
[this](uv::Loop&) { m_pingTimer->Start(m_loopDelay, m_loopDelay); });
}
void wpi::tsp::TimeSyncClient::Stop() { m_loopRunner.Stop(); }
int64_t wpi::tsp::TimeSyncClient::GetOffset() {
std::lock_guard lock{m_offsetMutex};
return m_metadata.offset;
}
wpi::tsp::TimeSyncClient::Metadata wpi::tsp::TimeSyncClient::GetMetadata() {
std::lock_guard lock{m_offsetMutex};
return m_metadata;
}

116
photon-targeting/src/main/native/cpp/net/TimeSyncServer.cpp Normal file
View File

@@ -0,0 +1,116 @@
/*
* 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/>.
*/
#include "net/TimeSyncServer.h"
#include <fmt/core.h>
#include <wpinet/UDPClient.h>
#include <wpinet/uv/util.h>
#include <atomic>
#include <chrono>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <iostream>
#include <mutex>
#include <thread>
#include <wpi/Logger.h>
#include <wpi/struct/Struct.h>
#include "ntcore_cpp.h"
static void ServerLoggerFunc(unsigned int level, const char* file,
unsigned int line, const char* msg) {
if (level == 20) {
fmt::print(stderr, "TimeSyncServer: {}\n", msg);
return;
}
std::string_view levelmsg;
if (level >= 50) {
levelmsg = "CRITICAL";
} else if (level >= 40) {
levelmsg = "ERROR";
} else if (level >= 30) {
levelmsg = "WARNING";
} else {
return;
}
fmt::print(stderr, "TimeSyncServer: {}: {} ({}:{})\n", levelmsg, msg, file,
line);
}
void wpi::tsp::TimeSyncServer::UdpCallback(uv::Buffer& data, size_t n,
const sockaddr& sender,
unsigned flags) {
// fmt::println("TimeSyncServer got ping!");
TspPing ping{wpi::UnpackStruct<TspPing>(data.bytes())};
if (ping.version != 1) {
WPI_ERROR(m_logger, "Bad version from client?");
return;
}
if (ping.message_id != 1) {
WPI_ERROR(m_logger, "Bad message id from client?");
return;
}
uint64_t current_time = m_timeProvider();
TspPong pong{ping, current_time};
pong.message_id = 2;
wpi::SmallVector<uint8_t, wpi::Struct<TspPong>::GetSize()> pongData(
wpi::Struct<TspPong>::GetSize());
wpi::PackStruct(pongData, pong);
// Wrap our buffer - pongData should free itself for free
wpi::uv::Buffer pongBuf{pongData};
int sent =
m_udp->TrySend(sender, wpi::SmallVector<wpi::uv::Buffer, 1>{pongBuf});
// fmt::println("Pong ret: {}", sent);
if (static_cast<size_t>(sent) != wpi::Struct<TspPong>::GetSize()) {
WPI_ERROR(m_logger, "Didn't send the whole pong back?");
return;
}
// WPI_INFO(m_logger, "Got ping: {} {} {}", ping.version, ping.message_id,
// ping.client_time);
// WPI_INFO(m_logger, "Sent pong: {} {} {} {}", pong.version, pong.message_id,
// pong.client_time, pong.server_time);
}
wpi::tsp::TimeSyncServer::TimeSyncServer(int port,
std::function<uint64_t()> timeProvider)
: m_logger{::ServerLoggerFunc},
m_timeProvider{timeProvider},
m_udp{wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)} {
m_loopRunner.ExecSync(
[this, port](uv::Loop&) { m_udp->Bind("0.0.0.0", port); });
}
void wpi::tsp::TimeSyncServer::Start() {
m_loopRunner.ExecSync([this](uv::Loop&) {
m_udp->received.connect(&wpi::tsp::TimeSyncServer::UdpCallback, this);
m_udp->StartRecv();
});
}
void wpi::tsp::TimeSyncServer::Stop() { m_loopRunner.Stop(); }

103
photon-targeting/src/main/native/include/net/TimeSyncClient.h Normal file
View File

@@ -0,0 +1,103 @@
/*
* 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/>.
*/
#pragma once
#include <fmt/core.h>
#include <wpinet/EventLoopRunner.h>
#include <wpinet/UDPClient.h>
#include <wpinet/uv/Buffer.h>
#include <wpinet/uv/Timer.h>
#include <wpinet/uv/Udp.h>
#include <atomic>
#include <chrono>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <functional>
#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <frc/filter/MedianFilter.h>
#include <wpi/Logger.h>
#include <wpi/static_circular_buffer.h>
#include <wpi/struct/Struct.h>
#include "TimeSyncStructs.h"
#include "ntcore_cpp.h"
namespace wpi {
namespace tsp {
class TimeSyncClient {
public:
struct Metadata {
int64_t offset{0};
int64_t rtt2{0};
size_t pingsSent{0};
size_t pongsReceived{0};
uint64_t lastPongTime{0};
};
private:
using SharedUdpPtr = std::shared_ptr<uv::Udp>;
using SharedTimerPtr = std::shared_ptr<uv::Timer>;
EventLoopRunner m_loopRunner{};
wpi::Logger m_logger;
std::function<uint64_t()> m_timeProvider;
SharedUdpPtr m_udp;
SharedTimerPtr m_pingTimer;
std::string m_serverIP;
int m_serverPort;
std::chrono::milliseconds m_loopDelay;
std::mutex m_offsetMutex;
Metadata m_metadata;
// We only allow the most recent ping to stay alive, so only keep track of it
TspPing m_lastPing;
// 30s is a reasonable guess
frc::MedianFilter<int64_t> m_lastOffsets{30};
void Tick();
void UdpCallback(uv::Buffer& buf, size_t nbytes, const sockaddr& sender,
unsigned flags);
public:
TimeSyncClient(std::string_view server, int remote_port,
std::chrono::milliseconds ping_delay,
std::function<uint64_t()> timeProvider = nt::Now);
void Start();
void Stop();
int64_t GetOffset();
Metadata GetMetadata();
};
} // namespace tsp
} // namespace wpi

78
photon-targeting/src/main/native/include/net/TimeSyncServer.h Normal file
View File

@@ -0,0 +1,78 @@
/*
* 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/>.
*/
#pragma once
#include <fmt/core.h>
#include <wpinet/EventLoopRunner.h>
#include <wpinet/UDPClient.h>
#include <wpinet/uv/Buffer.h>
#include <wpinet/uv/Timer.h>
#include <wpinet/uv/Udp.h>
#include <atomic>
#include <chrono>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <functional>
#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <wpi/Logger.h>
#include <wpi/struct/Struct.h>
#include "TimeSyncStructs.h"
#include "ntcore_cpp.h"
namespace wpi {
namespace tsp {
class TimeSyncServer {
using SharedUdpPtr = std::shared_ptr<uv::Udp>;
EventLoopRunner m_loopRunner{};
wpi::Logger m_logger;
std::function<uint64_t()> m_timeProvider;
SharedUdpPtr m_udp;
std::thread m_listener;
private:
void UdpCallback(uv::Buffer& buf, size_t nbytes, const sockaddr& sender,
unsigned flags);
public:
explicit TimeSyncServer(int port = 5810,
std::function<uint64_t()> timeProvider = nt::Now);
/**
* Start listening for pings
*/
void Start();
/**
* Stop our loop runner. After stopping, we cannot restart.
*/
void Stop();
};
} // namespace tsp
} // namespace wpi

91
photon-targeting/src/main/native/include/net/TimeSyncStructs.h Normal file
View File

@@ -0,0 +1,91 @@
/*
* 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/>.
*/
#pragma once
#include <stdint.h>
#include <wpi/struct/Struct.h>
namespace wpi {
namespace tsp {
struct TspPing {
uint8_t version;
uint8_t message_id;
uint64_t client_time;
};
struct TspPong : public TspPing {
TspPong(TspPing ping, uint64_t servertime)
: TspPing{ping}, server_time{servertime} {}
uint64_t server_time;
};
} // namespace tsp
} // namespace wpi
template <>
struct wpi::Struct<wpi::tsp::TspPing> {
static constexpr std::string_view GetTypeName() { return "TspPing"; }
static constexpr size_t GetSize() { return 10; }
static constexpr std::string_view GetSchema() {
return "uint8 version;uint8 message_id;uint64 client_time";
}
static wpi::tsp::TspPing Unpack(std::span<const uint8_t> data) {
return wpi::tsp::TspPing{
wpi::UnpackStruct<uint8_t, 0>(data),
wpi::UnpackStruct<uint8_t, 1>(data),
wpi::UnpackStruct<uint64_t, 2>(data),
};
}
static void Pack(std::span<uint8_t> data, const wpi::tsp::TspPing& value) {
wpi::PackStruct<0>(data, value.version);
wpi::PackStruct<1>(data, value.message_id);
wpi::PackStruct<2>(data, value.client_time);
}
};
template <>
struct wpi::Struct<wpi::tsp::TspPong> {
static constexpr std::string_view GetTypeName() { return "TspPong"; }
static constexpr size_t GetSize() { return 18; }
static constexpr std::string_view GetSchema() {
return "uint8 version;uint8 message_id;uint64 client_time;uint64_t "
"server_time";
}
static wpi::tsp::TspPong Unpack(std::span<const uint8_t> data) {
return wpi::tsp::TspPong{
wpi::tsp::TspPing{
wpi::UnpackStruct<uint8_t, 0>(data),
wpi::UnpackStruct<uint8_t, 1>(data),
wpi::UnpackStruct<uint64_t, 2>(data),
},
wpi::UnpackStruct<uint64_t, 10>(data),
};
}
static void Pack(std::span<uint8_t> data, const wpi::tsp::TspPong& value) {
wpi::PackStruct<0>(data, value.version);
wpi::PackStruct<1>(data, value.message_id);
wpi::PackStruct<2>(data, value.client_time);
wpi::PackStruct<10>(data, value.server_time);
}
};
static_assert(wpi::StructSerializable<wpi::tsp::TspPong>);
static_assert(wpi::StructSerializable<wpi::tsp::TspPing>);

168
photon-targeting/src/main/native/jni/TimeSyncClientJNI.cpp Normal file
View File

@@ -0,0 +1,168 @@
/*
* 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/>.
*/
#include <org_photonvision_jni_TimeSyncClient.h>
#include <cstdio>
#include <string>
#include "net/TimeSyncClient.h"
using namespace wpi::tsp;
/**
* Finds a class and keeps it as a global reference.
*
* Use with caution, as the destructor does NOT call DeleteGlobalRef due to
* potential shutdown issues with doing so.
*/
class JClass {
public:
JClass() = default;
JClass(JNIEnv* env, const char* name) {
jclass local = env->FindClass(name);
if (!local) {
return;
}
m_cls = static_cast<jclass>(env->NewGlobalRef(local));
env->DeleteLocalRef(local);
}
void free(JNIEnv* env) {
if (m_cls) {
env->DeleteGlobalRef(m_cls);
}
m_cls = nullptr;
}
explicit operator bool() const { return m_cls; }
operator jclass() const { return m_cls; }
protected:
jclass m_cls = nullptr;
};
static JClass metadataClass;
static jmethodID metadataCtor;
// TODO - only one onload allowed
JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
JNIEnv* env;
if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
return JNI_ERR;
}
metadataClass =
JClass(env, "org/photonvision/jni/TimeSyncClient$PingMetadata");
if (!metadataClass) {
std::printf("Couldn't find class!");
return JNI_ERR;
}
metadataCtor = env->GetMethodID(metadataClass, "<init>", "(JJJJJ)V");
if (!metadataCtor) {
std::printf("Couldn't find constructor!");
return JNI_ERR;
}
return JNI_VERSION_1_6;
}
extern "C" {
/*
* Class: org_photonvision_jni_TimeSyncClient
* Method: create
* Signature: (Ljava/lang/String;ID)J
*/
JNIEXPORT jlong JNICALL
Java_org_photonvision_jni_TimeSyncClient_create
(JNIEnv* env, jclass, jstring name, jint port, jdouble interval)
{
using namespace std::chrono_literals;
const char* c_name{env->GetStringUTFChars(name, 0)};
std::string cpp_name{c_name};
jlong ret{reinterpret_cast<jlong>(
new TimeSyncClient(cpp_name, static_cast<int>(port),
std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::duration<double>(interval))))};
env->ReleaseStringUTFChars(name, c_name);
return ret;
}
/*
* Class: org_photonvision_jni_TimeSyncClient
* Method: start
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_photonvision_jni_TimeSyncClient_start
(JNIEnv*, jclass, jlong ptr)
{
TimeSyncClient* client = reinterpret_cast<TimeSyncClient*>(ptr);
client->Start();
}
/*
* Class: org_photonvision_jni_TimeSyncClient
* Method: stop
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_photonvision_jni_TimeSyncClient_stop
(JNIEnv*, jclass, jlong ptr)
{
TimeSyncClient* client = reinterpret_cast<TimeSyncClient*>(ptr);
client->Stop();
delete client;
}
/*
* Class: org_photonvision_jni_TimeSyncClient
* Method: getOffset
* Signature: (J)J
*/
JNIEXPORT jlong JNICALL
Java_org_photonvision_jni_TimeSyncClient_getOffset
(JNIEnv*, jclass, jlong ptr)
{
TimeSyncClient* client = reinterpret_cast<TimeSyncClient*>(ptr);
return client->GetOffset();
}
/*
* Class: org_photonvision_jni_TimeSyncClient
* Method: getLatestMetadata
* Signature: (J)Ljava/lang/Object;
*/
JNIEXPORT jobject JNICALL
Java_org_photonvision_jni_TimeSyncClient_getLatestMetadata
(JNIEnv* env, jclass, jlong ptr)
{
TimeSyncClient* client = reinterpret_cast<TimeSyncClient*>(ptr);
auto m{client->GetMetadata()};
auto ret = env->NewObject(metadataClass, metadataCtor, m.offset, m.pingsSent,
m.pongsReceived, m.lastPongTime, m.rtt2);
return ret;
}
} // extern "C"

68
photon-targeting/src/main/native/jni/TimeSyncServerJNI.cpp Normal file
View File

@@ -0,0 +1,68 @@
/*
* 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/>.
*/
#include <org_photonvision_jni_TimeSyncClient.h>
#include <org_photonvision_jni_TimeSyncServer.h>
#include <cstdio>
#include "net/TimeSyncServer.h"
using namespace wpi::tsp;
extern "C" {
/*
* Class: org_photonvision_jni_TimeSyncServer
* Method: create
* Signature: (I)J
*/
JNIEXPORT jlong JNICALL
Java_org_photonvision_jni_TimeSyncServer_create
(JNIEnv*, jclass, jint port)
{
return reinterpret_cast<jlong>(new TimeSyncServer(port));
}
/*
* Class: org_photonvision_jni_TimeSyncServer
* Method: start
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_photonvision_jni_TimeSyncServer_start
(JNIEnv*, jclass, jlong ptr)
{
TimeSyncServer* server = reinterpret_cast<TimeSyncServer*>(ptr);
server->Start();
}
/*
* Class: org_photonvision_jni_TimeSyncServer
* Method: stop
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_photonvision_jni_TimeSyncServer_stop
(JNIEnv*, jclass, jlong ptr)
{
TimeSyncServer* server = reinterpret_cast<TimeSyncServer*>(ptr);
server->Stop();
delete server;
}
} // extern "C"

1
photon-targeting/src/main/proto/photon.proto
View File

@@ -66,4 +66,5 @@ message ProtobufPhotonPipelineResult {
int64 sequence_id = 4;
int64 capture_timestamp_micros = 5;
int64 nt_publish_timestamp_micros = 6;
int64 time_since_last_pong_micros = 7;
}

69
photon-targeting/src/test/java/net/TimeSyncTest.java Normal file
View File

@@ -0,0 +1,69 @@
/*
* 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 net;
import static org.junit.jupiter.api.Assertions.fail;
import edu.wpi.first.hal.HAL;
import java.io.IOException;
import org.junit.jupiter.api.AfterAll;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.photonvision.jni.PhotonTargetingJniLoader;
import org.photonvision.jni.TimeSyncClient;
import org.photonvision.jni.TimeSyncServer;
import org.photonvision.jni.WpilibLoader;
public class TimeSyncTest {
@BeforeAll
public static void load_wpilib() throws UnsatisfiedLinkError, IOException {
WpilibLoader.loadLibraries();
if (!PhotonTargetingJniLoader.load()) {
fail();
}
}
@AfterAll
public static void teardown() {
HAL.shutdown();
}
@Test
public void smoketest() throws InterruptedException {
HAL.initialize(1000, 0);
// NetworkTableInstance.getDefault().stopClient();
// NetworkTableInstance.getDefault().startServer();
var server = new TimeSyncServer(5810);
System.err.println("Waiting: PID=" + ProcessHandle.current().pid());
server.start();
var client = new TimeSyncClient("127.0.0.1", 5810, 0.5);
for (int i = 0; i < 5; i++) {
Thread.sleep(100);
System.out.println(client.getPingMetadata());
}
server.stop();
client.stop();
}
}

4
photon-targeting/src/test/java/org/photonvision/PacketTest.java
View File

@@ -42,7 +42,7 @@ class PacketTest {
@Test
void pipelineResultSerde() {
var ret1 = new PhotonPipelineResult(1, 2, 3, List.of());
var ret1 = new PhotonPipelineResult(1, 2, 3, 1024, List.of());
var p1 = new Packet(10);
PhotonPipelineResult.photonStruct.pack(p1, ret1);
var unpackedRet1 = PhotonPipelineResult.photonStruct.unpack(p1);
@@ -53,6 +53,7 @@ class PacketTest {
1,
2,
3,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -106,6 +107,7 @@ class PacketTest {
3,
4,
5,
1024,
List.of(
new PhotonTrackedTarget(
3.0,

8
photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java
View File

@@ -39,6 +39,7 @@ public class PhotonPipelineResultTest {
3,
4,
5,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -87,6 +88,7 @@ public class PhotonPipelineResultTest {
3,
4,
5,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -137,6 +139,7 @@ public class PhotonPipelineResultTest {
3,
4,
5,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -190,6 +193,7 @@ public class PhotonPipelineResultTest {
3,
4,
5,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -248,6 +252,7 @@ public class PhotonPipelineResultTest {
3,
4,
5,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -296,6 +301,7 @@ public class PhotonPipelineResultTest {
3,
4,
5,
1024,
List.of(
new PhotonTrackedTarget(
7.0,
@@ -346,6 +352,7 @@ public class PhotonPipelineResultTest {
3,
4,
5,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -399,6 +406,7 @@ public class PhotonPipelineResultTest {
3,
4,
5,
1024,
List.of(
new PhotonTrackedTarget(
3.0,

2
photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java
View File

@@ -43,6 +43,7 @@ public class PhotonPipelineResultProtoTest {
3,
4,
5,
1024,
List.of(
new PhotonTrackedTarget(
3.0,
@@ -97,6 +98,7 @@ public class PhotonPipelineResultProtoTest {
3,
4,
5,
1024,
List.of(
new PhotonTrackedTarget(
3.0,

4
photon-targeting/src/test/native/cpp/PacketTest.cpp
View File

@@ -85,7 +85,7 @@ TEST(PacketTest, PnpResult) {
// }
TEST(PacketTest, PhotonPipelineResult) {
PhotonPipelineResult result(PhotonPipelineMetadata(0, 0, 1),
PhotonPipelineResult result(PhotonPipelineMetadata(0, 0, 1, 2),
std::vector<PhotonTrackedTarget>{}, std::nullopt);
Packet p;
@@ -130,7 +130,7 @@ TEST(PacketTest, PhotonPipelineResult) {
17.0, 22.33, 2.54},
std::vector<int16_t>{8, 7, 11, 22, 59, 40}};
PhotonPipelineResult result2(PhotonPipelineMetadata{0, 0, 1}, targets,
PhotonPipelineResult result2(PhotonPipelineMetadata{0, 0, 1, 1}, targets,
mtResult);
Packet p2;

42
photon-targeting/src/test/native/cpp/net/TimeSyncTest.cpp Normal file
View File

@@ -0,0 +1,42 @@
/*
* 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/>.
*/
#include <gtest/gtest.h>
#include <hal/HAL.h>
#include <net/TimeSyncClient.h>
#include <net/TimeSyncServer.h>
TEST(TimeSyncProtocolTest, Smoketest) {
using namespace wpi::tsp;
using namespace std::chrono_literals;
HAL_Initialize(500, 0);
TimeSyncServer server{5812};
TimeSyncClient client{"127.0.0.1", 5812, 100ms};
server.Start();
client.Start();
for (int i = 0; i < 10; i++) {
std::this_thread::sleep_for(100ms);
TimeSyncClient::Metadata m = client.GetMetadata();
fmt::println("Offset={} rtt={}", m.offset, m.rtt2);
}
server.Stop();
}