Create timesync JNI for testing client (#1433)

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

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

photon-targeting/src/main/java/org/photonvision/jni/PhotonTargetingJniLoader.java

@@ -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;
+    }
+}

photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java

@@ -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);
+}

photon-targeting/src/main/java/org/photonvision/jni/TimeSyncServer.java

@@ -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);
+}

photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java

@@ -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 */

photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java

@@ -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;
     }
 

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

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

photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp

@@ -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;
+}

photon-targeting/src/main/native/cpp/net/TimeSyncServer.cpp

@@ -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(); }

photon-targeting/src/main/native/include/net/TimeSyncClient.h

@@ -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

photon-targeting/src/main/native/include/net/TimeSyncServer.h

@@ -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

photon-targeting/src/main/native/include/net/TimeSyncStructs.h

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

photon-targeting/src/main/native/jni/TimeSyncClientJNI.cpp

@@ -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"

photon-targeting/src/main/native/jni/TimeSyncServerJNI.cpp

@@ -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"

photon-targeting/src/main/proto/photon.proto

@@ -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;
 }