diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index cf264b70b..72b480545 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -115,6 +115,10 @@ jobs: - uses: actions/setup-python@v5 with: python-version: '3.11' + - name: Install graphviz + run: | + sudo apt-get update + sudo apt-get -y install graphviz - name: Install dependencies working-directory: docs run: | @@ -283,6 +287,9 @@ jobs: java-version: 17 distribution: temurin architecture: ${{ matrix.architecture }} + - name: Install Arm64 Toolchain + run: ./gradlew installArm64Toolchain + if: ${{ (matrix.artifact-name) == 'LinuxArm64' }} - run: | rm -rf photon-server/src/main/resources/web/* mkdir -p photon-server/src/main/resources/web/docs @@ -301,7 +308,7 @@ jobs: path: photon-server/src/main/resources/web/docs - run: | chmod +x gradlew - ./gradlew photon-server:shadowJar -PArchOverride=${{ matrix.arch-override }} + ./gradlew photon-targeting:jar photon-server:shadowJar -PArchOverride=${{ matrix.arch-override }} if: ${{ (matrix.arch-override != 'none') }} - run: | chmod +x gradlew @@ -311,6 +318,10 @@ jobs: with: name: jar-${{ matrix.artifact-name }} path: photon-server/build/libs + - uses: actions/upload-artifact@v4 + with: + name: photon-targeting_jar-${{ matrix.artifact-name }} + path: photon-targeting/build/libs run-smoketest-native: needs: [build-package] diff --git a/.github/workflows/photonvision-docs.yml b/.github/workflows/photonvision-docs.yml index f0809b115..3b0790cdf 100644 --- a/.github/workflows/photonvision-docs.yml +++ b/.github/workflows/photonvision-docs.yml @@ -26,6 +26,11 @@ jobs: - name: Install and upgrade pip run: python -m pip install --upgrade pip + - name: Install graphviz + run: | + sudo apt-get update + sudo apt-get -y install graphviz + - name: Install Python dependencies working-directory: docs run: | diff --git a/.readthedocs.yaml b/.readthedocs.yaml index 8b3ef1aca..4cf9063ec 100644 --- a/.readthedocs.yaml +++ b/.readthedocs.yaml @@ -9,6 +9,8 @@ build: os: ubuntu-22.04 tools: python: "3.11" + apt_packages: + - graphviz jobs: post_checkout: # Cancel building pull requests when there aren't changed in the docs directory or YAML file. diff --git a/docs/source/conf.py b/docs/source/conf.py index dca2301a4..ee4054b71 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -37,6 +37,7 @@ extensions = [ "sphinx_design", "myst_parser", "sphinx.ext.mathjax", + "sphinx.ext.graphviz", ] # Configure OpenGraph support diff --git a/docs/source/docs/contributing/design-descriptions/index.md b/docs/source/docs/contributing/design-descriptions/index.md index f1a598f89..a436f4807 100644 --- a/docs/source/docs/contributing/design-descriptions/index.md +++ b/docs/source/docs/contributing/design-descriptions/index.md @@ -3,4 +3,5 @@ ```{toctree} :maxdepth: 1 image-rotation +time-sync ``` diff --git a/docs/source/docs/contributing/design-descriptions/time-sync.md b/docs/source/docs/contributing/design-descriptions/time-sync.md new file mode 100644 index 000000000..3d98152a8 --- /dev/null +++ b/docs/source/docs/contributing/design-descriptions/time-sync.md @@ -0,0 +1,111 @@ +# Time Synchronization Protocol Specification, Version 1.0 + +Protocol Revision 1.0, 08/25/2024 + +## Background + +In a distributed compute environment like robots, time synchronization between computers is increasingly important. Currently, [NetworkTables Version 4.1](https://github.com/wpilibsuite/allwpilib/blob/main/ntcore/doc/networktables4.adoc) provides support for time synchronization of clients with the NetworkTables server using binary PING/PONG messages sent over WebSockets. This approach, while fundamentally the same as is described in this memo, has demonstrated some opportunities for improvement: + +- PING/PONG messages are processed in the same queue as other NetworkTables messages. Depending on the underlying implementation and processor speed, this can incur message processing delays and increase client-calculated Round-Trip Time (RTT), and cause messages to arrive at the server timestamped in the future. +- Messages use WebSockets over TCP for their transport layer. We don't need the robustness guarantees of TCP as our connection is stateless. + +For these reasons, a time synchronization solution separate from NetworkTables communication was desired. Architecture decisions made to address these issues are: + +- Use the User Datagram Protocol (UDP) transport layer, as we don't need the robustness guarantees afforded by TCP. As a Client, if a PING isn't replied to, we'll just try again at the start of the next PING window. As a bonus, we are free to use UDP port 5810 as NetworkTables only uses TCP Port 5810/5811 as of Version 4.1. +- Use a separate thread from the current NetworkTables libUV runner. + + +## Prior Art + +The [NetworkTables 4.1 timestamp synchronization](https://github.com/wpilibsuite/allwpilib/blob/main/ntcore/doc/networktables4.adoc#timestamps) approach, an implementation of [Cristian's Algorithm](https://en.wikipedia.org/wiki/Cristian%27s_algorithm). We also implement Cristian’s Algorithm. + +The [Precision Time Protocol](https://en.wikipedia.org/wiki/Precision_Time_Protocol#Synchronization) at it's core does something similar with Sync/Delay_Req/Delay_Resp. We do not have (guaranteed) access to hardware timestamping, but we utilize this PING/PONG pattern to estimate total round-trip time. + + +## Roles + +```{graphviz} +digraph CristianAlgorithm { + ratio=0.5; + bgcolor="transparent"; + + node [ + fontcolor = "#e6e6e6", + style = filled, + color = "#e6e6e6", + fillcolor = "#333333" + fontsize=10; + ] + + edge [ + color = "#e6e6e6", + fontcolor = "#e6e6e6" + fontsize=10; + ] + + rankdir=LR; + node [shape=box, style=filled, color=lightblue]; + + user_send [label="User Sends T1"]; + server_receive [label="Server Receives T1"]; + server_send [label="Server Sends T2"]; + user_receive [label="User Receives T2"]; + user_compute [label="User Computes Time"]; + + user_send -> server_receive [label="T1 (Request)"]; + server_receive -> server_send [label="T1 received by server"]; + server_send -> user_receive [label="T2 sent by server"]; + user_receive -> user_compute [label="T2 received by user"]; + user_compute -> user_send [label="Computed Time: T3 = T2 + (deltaT2 - deltaT1)/2"]; +} +``` + +Time Synchronization Protocol (TSP) participants can assume either a server role or a client role. The server role is responsible for listening for incoming time synchronization requests from clients and replying appropriately. The client role is responsible for sending "Ping" messages to the server and listening for "Pong" replies to estimate the offset between the server and client time bases. + +All time values shall use units of microseconds. The epoch of the time base this is measured against is unspecified. + +Clients shall periodically (e.g. every few seconds) send, in a manner that minimizes transmission delays, a **TSP Ping Message** that contains the client's current local time. + +When the server receives a **TSP Ping Message** from any client, it shall respond to the client, in a manner that minimizes transmission delays, with a **TSP Pong message** encoding a timestamp of its (the server's) current local time (in microseconds), and the client-provided data value. + +When the client receives a **TSP Pong Message** from the server, it shall verify that the `Client Local Time` corresponds to the currently in-flight TSP Ping message; if not, it shall drop this packet. The round trip time (RTT) shall be computed from the delta between the message's data value and the current local time. If the RTT is less than that from previous measurements, the client shall use the timestamp in the message plus ½ the RTT as the server time equivalent to the current local time, and use this equivalence to compute server time base timestamps from local time for future messages. + +## Transport + +Communication between server and clients shall occur over the User Datagram Protocol (UDP) Port 5810. + +## Message Format + +The message format forgoes CRCs (as these are provided by the Ethernet physical layer) or packet delimination (as our packetsa are assumed be under the network MTU). **TSP Ping** and **TSP Pong** messages shall be encoded in a manor compatible with a WPILib packed struct with respect to byte alignment and endienness. + +### TSP Ping + +| Offset | Format | Data | Notes | +| ------ | ------ | ---- | ----- | +| 0 | uint8 | Protocol version | This field shall always set to 1 (0b1) for TSP Version 1. | +| 1 | uint8 | Message ID | This field shall always be set to 1 (0b1). | +| 2 | uint64 | Client Local Time | The client's local time value, at the time this Ping message was sent. | + +### TSP Pong + +| Offset | Format | Data | Notes | +| ------ | ------ | ---- | ----- | +| 0 | uint8 | Protocol version | This field shall always set to 1 (0b1) for TSP Version 1. +| 1 | uint8 | Message ID | This field shall always be set to 2 (0b2). +| 2 | uint64 | Client Local Time | The client's local time value from the Ping message that this Pong is generated in response to. +| 10 | uint64 | Server Local Time | The current time at the server, at the time this Pong message was sent. + + +## Optional Protocol Extensions + +Clients may publish statistics to NetworkTables. If they do, they shall publish to a key that is globally unique per participant in the Time Synronization network. If a client implements this, it shall provide the following publishers: + +| Key | Type | Notes | +| ------ | ------ | ---- | ----- | +| offset_us | Integer | The time offset that, when added to the client's local clock, provides server time | +| ping_tx_count | Integer | The total number of TSP Ping packets transmitted | +| ping_rx_count | Integer | The total number of TSP Ping packets received | +| pong_rx_time_us | Integer | The time, in client local time, that the last pong was received | +| rtt2_us | Integer | The time in us from last complete (ping transmission to pong reception) | + +PhotonVision has chosen to publish to the sub-table `/photonvision/.timesync/{DEVICE_HOSTNAME}`. Future implementations of this protocol may decide to implement this as a structured data type. diff --git a/photon-core/build.gradle b/photon-core/build.gradle index ee9ea9160..5cf4a2bb4 100644 --- a/photon-core/build.gradle +++ b/photon-core/build.gradle @@ -17,6 +17,7 @@ def nativeTasks = wpilibTools.createExtractionTasks { nativeTasks.addToSourceSetResources(sourceSets.main) +nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpilibc") nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath") nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil") diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java index 8b3c380a3..994cda74d 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java @@ -265,7 +265,7 @@ public class SqlConfigProvider extends ConfigProvider { JacksonUtils.deserialize( getOneConfigFile(conn, GlobalKeys.HARDWARE_CONFIG), HardwareConfig.class); } catch (IOException e) { - logger.error("Could not deserialize hardware config! Loading defaults"); + logger.error("Could not deserialize hardware config! Loading defaults", e); hardwareConfig = new HardwareConfig(); } @@ -274,7 +274,7 @@ public class SqlConfigProvider extends ConfigProvider { JacksonUtils.deserialize( getOneConfigFile(conn, GlobalKeys.HARDWARE_SETTINGS), HardwareSettings.class); } catch (IOException e) { - logger.error("Could not deserialize hardware settings! Loading defaults"); + logger.error("Could not deserialize hardware settings! Loading defaults", e); hardwareSettings = new HardwareSettings(); } @@ -283,7 +283,7 @@ public class SqlConfigProvider extends ConfigProvider { JacksonUtils.deserialize( getOneConfigFile(conn, GlobalKeys.NETWORK_CONFIG), NetworkConfig.class); } catch (IOException e) { - logger.error("Could not deserialize network config! Loading defaults"); + logger.error("Could not deserialize network config! Loading defaults", e); networkConfig = new NetworkConfig(); } @@ -292,7 +292,7 @@ public class SqlConfigProvider extends ConfigProvider { JacksonUtils.deserialize( getOneConfigFile(conn, GlobalKeys.ATFL_CONFIG_FILE), AprilTagFieldLayout.class); } catch (IOException e) { - logger.error("Could not deserialize apriltag layout! Loading defaults"); + logger.error("Could not deserialize apriltag layout! Loading defaults", e); try { atfl = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); } catch (UncheckedIOException e2) { diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 719b0f296..a3a8971f9 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -20,7 +20,7 @@ package org.photonvision.common.dataflow.networktables; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEvent; -import edu.wpi.first.util.WPIUtilJNI; +import edu.wpi.first.networktables.NetworkTablesJNI; import java.util.List; import java.util.function.BooleanSupplier; import java.util.function.Consumer; @@ -146,13 +146,19 @@ public class NTDataPublisher implements CVPipelineResultConsumer { List.of(), result.inputAndOutputFrame); else acceptedResult = result; - var now = WPIUtilJNI.now(); - var captureMicros = MathUtils.nanosToMicros(acceptedResult.getImageCaptureTimestampNanos()); + var now = NetworkTablesJNI.now(); + var captureMicros = MathUtils.nanosToMicros(result.getImageCaptureTimestampNanos()); + + var offset = NetworkTablesManager.getInstance().getOffset(); + + // Transform the metadata timestamps from the local nt::Now timebase to the Time Sync Server's + // timebase var simplified = new PhotonPipelineResult( acceptedResult.sequenceID, - captureMicros, - now, + captureMicros + offset, + now + offset, + NetworkTablesManager.getInstance().getTimeSinceLastPong(), TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets), acceptedResult.multiTagResult); diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java index bb59d8f46..21c22ad22 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java @@ -18,6 +18,7 @@ package org.photonvision.common.dataflow.networktables; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.networktables.LogMessage; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTableEvent.Kind; @@ -26,7 +27,6 @@ import edu.wpi.first.networktables.StringSubscriber; import java.io.IOException; import java.util.EnumSet; import java.util.HashMap; -import java.util.function.Consumer; import org.photonvision.PhotonVersion; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.configuration.NetworkConfig; @@ -34,6 +34,7 @@ import org.photonvision.common.dataflow.DataChangeService; import org.photonvision.common.dataflow.events.OutgoingUIEvent; import org.photonvision.common.hardware.HardwareManager; import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.LogLevel; import org.photonvision.common.logging.Logger; import org.photonvision.common.scripting.ScriptEventType; import org.photonvision.common.scripting.ScriptManager; @@ -41,32 +42,39 @@ import org.photonvision.common.util.TimedTaskManager; import org.photonvision.common.util.file.JacksonUtils; public class NetworkTablesManager { + private static final Logger logger = + new Logger(NetworkTablesManager.class, LogGroup.NetworkTables); + private final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault(); private final String kRootTableName = "/photonvision"; private final String kFieldLayoutName = "apriltag_field_layout"; public final NetworkTable kRootTable = ntInstance.getTable(kRootTableName); - private final NTLogger m_ntLogger = new NTLogger(); - private boolean m_isRetryingConnection = false; private StringSubscriber m_fieldLayoutSubscriber = kRootTable.getStringTopic(kFieldLayoutName).subscribe(""); + private final TimeSyncManager m_timeSync = new TimeSyncManager(kRootTable); + private NetworkTablesManager() { - ntInstance.addLogger(255, 255, (event) -> {}); // to hide error messages - ntInstance.addConnectionListener(true, m_ntLogger); // to hide error messages + ntInstance.addLogger( + LogMessage.kInfo, LogMessage.kCritical, this::logNtMessage); // to hide error messages + ntInstance.addConnectionListener(true, this::checkNtConnectState); // to hide error messages ntInstance.addListener( m_fieldLayoutSubscriber, EnumSet.of(Kind.kValueAll), this::onFieldLayoutChanged); - TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000); - // Get the UI state in sync with the backend. NT should fire a callback when it first connects // to the robot broadcastConnectedStatus(); } + public void registerTimedTasks() { + m_timeSync.start(); + TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000); + } + private static NetworkTablesManager INSTANCE; public static NetworkTablesManager getInstance() { @@ -74,43 +82,72 @@ public class NetworkTablesManager { return INSTANCE; } - private static final Logger logger = new Logger(NetworkTablesManager.class, LogGroup.General); + private void logNtMessage(NetworkTableEvent event) { + String levelmsg = "DEBUG"; + LogLevel pvlevel = LogLevel.DEBUG; + if (event.logMessage.level >= LogMessage.kCritical) { + pvlevel = LogLevel.ERROR; + levelmsg = "CRITICAL"; + } else if (event.logMessage.level >= LogMessage.kError) { + pvlevel = LogLevel.ERROR; + levelmsg = "ERROR"; + } else if (event.logMessage.level >= LogMessage.kWarning) { + pvlevel = LogLevel.WARN; + levelmsg = "WARNING"; + } else if (event.logMessage.level >= LogMessage.kInfo) { + pvlevel = LogLevel.INFO; + levelmsg = "INFO"; + } - private static class NTLogger implements Consumer { - private boolean hasReportedConnectionFailure = false; + logger.log( + "NT: " + + levelmsg + + " " + + event.logMessage.level + + ": " + + event.logMessage.message + + " (" + + event.logMessage.filename + + ":" + + event.logMessage.line + + ")", + pvlevel); + } - @Override - public void accept(NetworkTableEvent event) { - var isConnEvent = event.is(Kind.kConnected); - var isDisconnEvent = event.is(Kind.kDisconnected); + public void checkNtConnectState(NetworkTableEvent event) { + var isConnEvent = event.is(Kind.kConnected); + var isDisconnEvent = event.is(Kind.kDisconnected); - if (!hasReportedConnectionFailure && isDisconnEvent) { - var msg = - String.format( - "NT lost connection to %s:%d! (NT version %d). Will retry in background.", - event.connInfo.remote_ip, - event.connInfo.remote_port, - event.connInfo.protocol_version); - logger.error(msg); - HardwareManager.getInstance().setNTConnected(false); + if (isDisconnEvent) { + var msg = + String.format( + "NT lost connection to %s:%d! (NT version %d). Will retry in background.", + event.connInfo.remote_ip, + event.connInfo.remote_port, + event.connInfo.protocol_version); + logger.error(msg); + HardwareManager.getInstance().setNTConnected(false); - hasReportedConnectionFailure = true; - getInstance().broadcastConnectedStatus(); - } else if (isConnEvent && event.connInfo != null) { - var msg = - String.format( - "NT connected to %s:%d! (NT version %d)", - event.connInfo.remote_ip, - event.connInfo.remote_port, - event.connInfo.protocol_version); - logger.info(msg); - HardwareManager.getInstance().setNTConnected(true); + getInstance().broadcastConnectedStatus(); + } else if (isConnEvent && event.connInfo != null) { + var msg = + String.format( + "NT connected to %s:%d! (NT version %d)", + event.connInfo.remote_ip, + event.connInfo.remote_port, + event.connInfo.protocol_version); + logger.info(msg); + HardwareManager.getInstance().setNTConnected(true); - hasReportedConnectionFailure = false; - ScriptManager.queueEvent(ScriptEventType.kNTConnected); - getInstance().broadcastVersion(); - getInstance().broadcastConnectedStatus(); - } + ScriptManager.queueEvent(ScriptEventType.kNTConnected); + getInstance().broadcastVersion(); + getInstance().broadcastConnectedStatus(); + + m_timeSync.reportNtConnected(); + } else if (isConnEvent) { + logger.warn("Got connection event with no connection info??"); + } else { + logger.warn("Got a non-sensical connection message that is neither connect nor disconnect?"); } } @@ -168,9 +205,16 @@ public class NetworkTablesManager { } else { setClientMode(config.ntServerAddress); } + + m_timeSync.setConfig(config); + broadcastVersion(); } + public long getOffset() { + return m_timeSync.getOffset(); + } + private void setClientMode(String ntServerAddress) { ntInstance.stopServer(); ntInstance.startClient4("photonvision"); @@ -211,4 +255,8 @@ public class NetworkTablesManager { "[NetworkTablesManager] Could not connect to the robot! Will retry in the background..."); } } + + public long getTimeSinceLastPong() { + return m_timeSync.getTimeSinceLastPong(); + } } diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/TimeSyncManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/TimeSyncManager.java new file mode 100644 index 000000000..83431a3f3 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/TimeSyncManager.java @@ -0,0 +1,169 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.common.dataflow.networktables; + +import edu.wpi.first.cscore.CameraServerJNI; +import edu.wpi.first.networktables.IntegerPublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import org.photonvision.common.configuration.NetworkConfig; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.TimedTaskManager; +import org.photonvision.jni.PhotonTargetingJniLoader; +import org.photonvision.jni.TimeSyncClient; +import org.photonvision.jni.TimeSyncServer; + +public class TimeSyncManager { + private static final Logger logger = new Logger(TimeSyncManager.class, LogGroup.NetworkTables); + + private TimeSyncClient m_client = null; + private TimeSyncServer m_server = null; + + private NetworkTableInstance ntInstance; + IntegerPublisher m_offsetPub; + IntegerPublisher m_rtt2Pub; + IntegerPublisher m_pingsPub; + IntegerPublisher m_pongsPub; + IntegerPublisher m_lastPongTimePub; + + public TimeSyncManager(NetworkTable kRootTable) { + if (!PhotonTargetingJniLoader.isWorking) { + logger.error("PhotonTargetingJNI was not loaded! Cannot do time-sync"); + } + + this.ntInstance = kRootTable.getInstance(); + + // Need this subtable to be unique per coprocessor. TODO: consider using MAC address or + // something similar for metrics? + var timeTable = kRootTable.getSubTable(".timesync").getSubTable(CameraServerJNI.getHostname()); + m_offsetPub = timeTable.getIntegerTopic("offset_us").publish(); + m_rtt2Pub = timeTable.getIntegerTopic("rtt2_us").publish(); + m_pingsPub = timeTable.getIntegerTopic("ping_tx_count").publish(); + m_pongsPub = timeTable.getIntegerTopic("pong_rx_count").publish(); + m_lastPongTimePub = timeTable.getIntegerTopic("pong_rx_time_us").publish(); + + // default to being a client + logger.debug("Starting TimeSyncClient on localhost (for now)"); + m_client = new TimeSyncClient("127.0.0.1", 5810, 1.0); + } + + // Since we're spinning off tasks in a new thread, be careful and start it seperately + public void start() { + if (!PhotonTargetingJniLoader.isWorking) { + logger.error("PhotonTargetingJNI was not loaded! Cannot start"); + } + + TimedTaskManager.getInstance().addTask("TimeSyncManager::tick", this::tick, 1000); + } + + public synchronized long getOffset() { + if (!PhotonTargetingJniLoader.isWorking) { + return 0; + } + + // if we're a client, return the offset to server time + if (m_client != null) return m_client.getOffset(); + // if we're a server, our time (nt::Now) is the same as network time + if (m_server != null) return 0; + + // ????? should never hit + logger.error("Client and server and null?"); + return 0; + } + + synchronized void setConfig(NetworkConfig config) { + if (!PhotonTargetingJniLoader.isWorking) { + return; + } + + if (m_client == null && m_server == null) { + throw new RuntimeException("Neither client nor server are null?"); + } + + // if not already running a server, set it up + if (config.runNTServer && m_server == null) { + // tear down anything old + if (m_client != null) { + logger.debug("Tearing down old client"); + m_client.stop(); + m_client = null; + } + + logger.debug("Starting TimeSyncServer"); + m_server = new TimeSyncServer(5810); + m_server.start(); + } else + // if not already running a client, set it up + if (m_client == null) { + // tear down anything old + if (m_server != null) { + logger.debug("Tearing down old server"); + m_server.stop(); + m_server = null; + } + + // Guess at IP -- tick will take care of changing this (may take up to 1 second) + logger.debug("Starting TimeSyncClient on localhost (for now)"); + m_client = new TimeSyncClient("127.0.0.1", 5810, 1.0); + } + } + + synchronized void tick() { + if (m_client != null) { + var conns = ntInstance.getConnections(); + + if (conns.length > 0) { + logger.debug("Changing TimeSyncClient server to " + conns[0].remote_ip); + m_client.setServer(conns[0].remote_ip); + } + + if (m_client != null) { + var m = m_client.getPingMetadata(); + + m_offsetPub.set(m.offset); + m_rtt2Pub.set(m.rtt2); + m_pingsPub.set(m.pingsSent); + m_pongsPub.set(m.pongsReceived); + m_lastPongTimePub.set(m.lastPongTime); + } + } + } + + public synchronized long getTimeSinceLastPong() { + if (m_client != null) { + return m_client.getPingMetadata().timeSinceLastPong(); + } else if (m_server != null) { + return 0; + } else { + // ???? + return 0; + } + } + + /** Restart our timesync client if NT just connected */ + public synchronized void reportNtConnected() { + if (m_client != null) { + // restart (in java code; we could just add a reset metrics function...) + logger.debug( + "NT (re)connected -- restarting Time Sync Client at " + m_client.getServer() + ":5810"); + m_client.stop(); + m_client = new TimeSyncClient(m_client.getServer(), 5810, 1.0); + } + } +} diff --git a/photon-core/src/main/java/org/photonvision/common/logging/LogGroup.java b/photon-core/src/main/java/org/photonvision/common/logging/LogGroup.java index 0cdd9e92a..0e2a1d882 100644 --- a/photon-core/src/main/java/org/photonvision/common/logging/LogGroup.java +++ b/photon-core/src/main/java/org/photonvision/common/logging/LogGroup.java @@ -25,4 +25,5 @@ public enum LogGroup { General, Config, CSCore, + NetworkTables, } diff --git a/photon-core/src/main/java/org/photonvision/common/logging/Logger.java b/photon-core/src/main/java/org/photonvision/common/logging/Logger.java index 49a9bfb7a..dc0ebefe7 100644 --- a/photon-core/src/main/java/org/photonvision/common/logging/Logger.java +++ b/photon-core/src/main/java/org/photonvision/common/logging/Logger.java @@ -100,6 +100,7 @@ public class Logger { levelMap.put(LogGroup.VisionModule, LogLevel.INFO); levelMap.put(LogGroup.Config, LogLevel.INFO); levelMap.put(LogGroup.CSCore, LogLevel.TRACE); + levelMap.put(LogGroup.NetworkTables, LogLevel.DEBUG); } static { @@ -200,7 +201,7 @@ public class Logger { return logLevel.code <= levelMap.get(group).code; } - void log(String message, LogLevel level) { + public void log(String message, LogLevel level) { if (shouldLog(level)) { log(message, level, group, className); } diff --git a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java index 218bd50dc..ca1298905 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/TestUtils.java @@ -18,76 +18,20 @@ package org.photonvision.common.util; import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.apriltag.jni.AprilTagJNI; -import edu.wpi.first.cscore.CameraServerJNI; -import edu.wpi.first.cscore.OpenCvLoader; -import edu.wpi.first.hal.JNIWrapper; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.jni.ArmFeedforwardJNI; -import edu.wpi.first.math.jni.DAREJNI; -import edu.wpi.first.math.jni.EigenJNI; -import edu.wpi.first.math.jni.Ellipse2dJNI; -import edu.wpi.first.math.jni.Pose3dJNI; -import edu.wpi.first.math.jni.StateSpaceUtilJNI; -import edu.wpi.first.math.jni.TrajectoryUtilJNI; import edu.wpi.first.math.util.Units; -import edu.wpi.first.net.WPINetJNI; -import edu.wpi.first.networktables.NetworkTablesJNI; -import edu.wpi.first.util.CombinedRuntimeLoader; -import edu.wpi.first.util.WPIUtilJNI; import java.awt.HeadlessException; import java.io.File; import java.io.IOException; import java.nio.file.Path; -import org.opencv.core.Core; import org.opencv.core.Mat; import org.opencv.highgui.HighGui; +import org.photonvision.jni.WpilibLoader; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; public class TestUtils { - private static boolean has_loaded = false; - public static boolean loadLibraries() { - if (has_loaded) return true; - - NetworkTablesJNI.Helper.setExtractOnStaticLoad(false); - WPIUtilJNI.Helper.setExtractOnStaticLoad(false); - CameraServerJNI.Helper.setExtractOnStaticLoad(false); - OpenCvLoader.Helper.setExtractOnStaticLoad(false); - JNIWrapper.Helper.setExtractOnStaticLoad(false); - WPINetJNI.Helper.setExtractOnStaticLoad(false); - AprilTagJNI.Helper.setExtractOnStaticLoad(false); - - // wpimathjni is a bit odd, it's all in the wpimathjni shared lib, but the java side stuff has - // been split. - ArmFeedforwardJNI.Helper.setExtractOnStaticLoad(false); - DAREJNI.Helper.setExtractOnStaticLoad(false); - EigenJNI.Helper.setExtractOnStaticLoad(false); - Ellipse2dJNI.Helper.setExtractOnStaticLoad(false); - Pose3dJNI.Helper.setExtractOnStaticLoad(false); - StateSpaceUtilJNI.Helper.setExtractOnStaticLoad(false); - TrajectoryUtilJNI.Helper.setExtractOnStaticLoad(false); - - try { - CombinedRuntimeLoader.loadLibraries( - TestUtils.class, - "wpiutiljni", - "wpimathjni", - "ntcorejni", - "wpinetjni", - "wpiHaljni", - "cscorejni", - "apriltagjni"); - - CombinedRuntimeLoader.loadLibraries(TestUtils.class, Core.NATIVE_LIBRARY_NAME); - - has_loaded = true; - } catch (IOException e) { - e.printStackTrace(); - has_loaded = false; - } - - return has_loaded; + return WpilibLoader.loadLibraries(); } @SuppressWarnings("unused") diff --git a/photon-core/src/main/java/org/photonvision/common/util/file/JacksonUtils.java b/photon-core/src/main/java/org/photonvision/common/util/file/JacksonUtils.java index 6fc3e29a9..40d8e6a4f 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/file/JacksonUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/file/JacksonUtils.java @@ -33,6 +33,7 @@ import java.io.IOException; import java.nio.file.Path; import java.util.HashMap; import java.util.Map; +import org.eclipse.jetty.io.EofException; public class JacksonUtils { public static class UIMap extends HashMap {} @@ -76,6 +77,10 @@ public class JacksonUtils { } public static T deserialize(String s, Class ref) throws IOException { + if (s.length() == 0) { + throw new EofException("Provided empty string for class " + ref.getName()); + } + PolymorphicTypeValidator ptv = BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build(); ObjectMapper objectMapper = diff --git a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java index 5efb81ef2..167effaea 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java @@ -25,7 +25,7 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.WPIUtilJNI; +import edu.wpi.first.networktables.NetworkTablesJNI; import java.util.Arrays; import java.util.List; import org.opencv.core.Core; @@ -98,7 +98,7 @@ public class MathUtils { } public static long wpiNanoTime() { - return microsToNanos(WPIUtilJNI.now()); + return microsToNanos(NetworkTablesJNI.now()); } /** diff --git a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java index 4a25e72a5..f28cabdd9 100644 --- a/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java +++ b/photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java @@ -42,18 +42,18 @@ public class USBFrameProvider extends CpuImageProcessor { @Override public CapturedFrame getInputMat() { - var mat = new CVMat(); // We do this so that we don't fill a Mat in use by another thread - // This is from wpi::Now, or WPIUtilJNI.now() - long time = - cvSink.grabFrame(mat.getMat()) - * 1000; // Units are microseconds, epoch is the same as the Unix epoch + // We allocate memory so we don't fill a Mat in use by another thread (memory model is easier) + var mat = new CVMat(); + // This is from wpi::Now, or WPIUtilJNI.now(). The epoch from grabFrame is uS since + // Hal::initialize was called + long captureTimeNs = cvSink.grabFrame(mat.getMat()) * 1000; - if (time == 0) { + if (captureTimeNs == 0) { var error = cvSink.getError(); logger.error("Error grabbing image: " + error); } - return new CapturedFrame(mat, settables.getFrameStaticProperties(), time); + return new CapturedFrame(mat, settables.getFrameStaticProperties(), captureTimeNs); } @Override diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java index b964f5723..4eb4adf6f 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java @@ -18,8 +18,10 @@ package org.photonvision.vision.processes; import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.fail; import edu.wpi.first.cscore.VideoMode; +import java.io.IOException; import java.util.Arrays; import java.util.HashMap; import java.util.List; @@ -31,6 +33,7 @@ import org.photonvision.common.configuration.CameraConfiguration; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.CVPipelineResultConsumer; import org.photonvision.common.util.TestUtils; +import org.photonvision.jni.PhotonTargetingJniLoader; import org.photonvision.vision.camera.QuirkyCamera; import org.photonvision.vision.camera.USBCameras.USBCameraSource; import org.photonvision.vision.frame.FrameProvider; @@ -41,7 +44,16 @@ import org.photonvision.vision.pipeline.result.CVPipelineResult; public class VisionModuleManagerTest { @BeforeAll public static void init() { + String classpathStr = System.getProperty("java.class.path"); + System.out.print(classpathStr); + TestUtils.loadLibraries(); + try { + if (!PhotonTargetingJniLoader.load()) fail(); + } catch (UnsatisfiedLinkError | IOException e) { + e.printStackTrace(); + fail(e); + } } private static class TestSource extends VisionSource { diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 6b0a3c228..5aa403c95 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -177,6 +177,7 @@ task generateVendorJson() { def read = photonlibFileInput.text .replace('${photon_version}', pubVersion) .replace('${frc_year}', frcYear) + .replace('${wpilib_version}', wpilibVersion) photonlibFileOutput.text = read outputs.upToDateWhen { false } @@ -331,6 +332,7 @@ def nativeTasks = wpilibTools.createExtractionTasks { nativeTasks.addToSourceSetResources(sourceSets.test) +nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpilibc") nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath") nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil") diff --git a/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py b/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py index a1bf4c409..080e1ba19 100644 --- a/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py +++ b/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py @@ -24,6 +24,7 @@ from ..targeting import * class MultiTargetPNPResultSerde: + # Message definition md5sum. See photon_packet.adoc for details MESSAGE_VERSION = "541096947e9f3ca2d3f425ff7b04aa7b" MESSAGE_FORMAT = "PnpResult:ae4d655c0a3104d88df4f5db144c1e86 estimatedPose;int16 fiducialIDsUsed[?];" diff --git a/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py b/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py index f0e8731f3..f8e68f123 100644 --- a/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py +++ b/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py @@ -24,11 +24,10 @@ from ..targeting import * class PhotonPipelineMetadataSerde: + # Message definition md5sum. See photon_packet.adoc for details - MESSAGE_VERSION = "626e70461cbdb274fb43ead09c255f4e" - MESSAGE_FORMAT = ( - "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;" - ) + MESSAGE_VERSION = "ac0a45f686457856fb30af77699ea356" + MESSAGE_FORMAT = "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;int64 timeSinceLastPong;" @staticmethod def unpack(packet: "Packet") -> "PhotonPipelineMetadata": @@ -43,6 +42,9 @@ class PhotonPipelineMetadataSerde: # publishTimestampMicros is of intrinsic type int64 ret.publishTimestampMicros = packet.decodeLong() + # timeSinceLastPong is of intrinsic type int64 + ret.timeSinceLastPong = packet.decodeLong() + return ret diff --git a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py index fa32c9bdd..d54917cf8 100644 --- a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py +++ b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py @@ -24,9 +24,10 @@ from ..targeting import * class PhotonPipelineResultSerde: + # Message definition md5sum. See photon_packet.adoc for details - MESSAGE_VERSION = "5eeaa293d0c69aea90eaddea786a2b3b" - MESSAGE_FORMAT = "PhotonPipelineMetadata:626e70461cbdb274fb43ead09c255f4e metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;" + MESSAGE_VERSION = "4b2ff16a964b5e2bf04be0c1454d91c4" + MESSAGE_FORMAT = "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;" @staticmethod def unpack(packet: "Packet") -> "PhotonPipelineResult": diff --git a/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py b/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py index 4765fa5e3..1a54e7b53 100644 --- a/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py +++ b/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py @@ -24,6 +24,7 @@ from ..targeting import * class PhotonTrackedTargetSerde: + # Message definition md5sum. See photon_packet.adoc for details MESSAGE_VERSION = "cc6dbb5c5c1e0fa808108019b20863f1" MESSAGE_FORMAT = "float64 yaw;float64 pitch;float64 area;float64 skew;int32 fiducialId;int32 objDetectId;float32 objDetectConf;Transform3d bestCameraToTarget;Transform3d altCameraToTarget;float64 poseAmbiguity;TargetCorner:16f6ac0dedc8eaccb951f4895d9e18b6 minAreaRectCorners[?];TargetCorner:16f6ac0dedc8eaccb951f4895d9e18b6 detectedCorners[?];" diff --git a/photon-lib/py/photonlibpy/generated/PnpResultSerde.py b/photon-lib/py/photonlibpy/generated/PnpResultSerde.py index e4c1f261e..a9d4cb81e 100644 --- a/photon-lib/py/photonlibpy/generated/PnpResultSerde.py +++ b/photon-lib/py/photonlibpy/generated/PnpResultSerde.py @@ -24,6 +24,7 @@ from ..targeting import * class PnpResultSerde: + # Message definition md5sum. See photon_packet.adoc for details MESSAGE_VERSION = "ae4d655c0a3104d88df4f5db144c1e86" MESSAGE_FORMAT = "Transform3d best;Transform3d alt;float64 bestReprojErr;float64 altReprojErr;float64 ambiguity;" diff --git a/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py b/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py index f55207f9b..ef1814a35 100644 --- a/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py +++ b/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py @@ -24,6 +24,7 @@ from ..targeting import * class TargetCornerSerde: + # Message definition md5sum. See photon_packet.adoc for details MESSAGE_VERSION = "16f6ac0dedc8eaccb951f4895d9e18b6" MESSAGE_FORMAT = "float64 x;float64 y;" diff --git a/photon-lib/src/generate/photonlib.json.in b/photon-lib/src/generate/photonlib.json.in index cbb183671..88d4e002d 100644 --- a/photon-lib/src/generate/photonlib.json.in +++ b/photon-lib/src/generate/photonlib.json.in @@ -9,7 +9,47 @@ "https://maven.photonvision.org/repository/snapshots" ], "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [], + "jniDependencies": [ + { + "groupId": "edu.wpi.first.wpilibc", + "artifactId": "wpilibc-cpp", + "version": "${wpilib_version}", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "${photon_version}", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-jni", + "version": "${photon_version}", + "skipInvalidPlatforms": true, + "isJar": true, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], "cppDependencies": [ { "groupId": "org.photonvision", diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 8a6035d52..c70cd505f 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -53,6 +53,7 @@ import java.util.stream.Collectors; import org.photonvision.common.hardware.VisionLEDMode; import org.photonvision.common.networktables.PacketSubscriber; import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.timesync.TimeSyncSingleton; /** Represents a camera that is connected to PhotonVision. */ public class PhotonCamera implements AutoCloseable { @@ -116,6 +117,9 @@ public class PhotonCamera implements AutoCloseable { private double prevHeartbeatChangeTime = 0; private static final double HEARTBEAT_DEBOUNCE_SEC = 0.5; + private double prevTimeSyncWarnTime = 0; + private static final double WARN_DEBOUNCE_SEC = 5; + public static void setVersionCheckEnabled(boolean enabled) { VERSION_CHECK_ENABLED = enabled; } @@ -166,6 +170,9 @@ public class PhotonCamera implements AutoCloseable { HAL.report(tResourceType.kResourceType_PhotonCamera, InstanceCount); InstanceCount++; + + // HACK - start a TimeSyncServer, if we haven't yet. + TimeSyncSingleton.load(); } /** @@ -189,13 +196,12 @@ public class PhotonCamera implements AutoCloseable { List ret = new ArrayList<>(); + // Grab the latest results. We don't care about the timestamps from NT - the metadata header has + // this, latency compensated by the Time Sync Client var changes = resultSubscriber.getAllChanges(); - - // TODO: NT4 timestamps are still not to be trusted. But it's the best we can do until we can - // make time sync more reliable. for (var c : changes) { var result = c.value; - result.setReceiveTimestampMicros(c.timestamp); + checkTimeSyncOrWarn(result); ret.add(result); } @@ -213,21 +219,38 @@ public class PhotonCamera implements AutoCloseable { public PhotonPipelineResult getLatestResult() { verifyVersion(); + // Grab the latest result. We don't care about the timestamp from NT - the metadata header has + // this, latency compensated by the Time Sync Client var ret = resultSubscriber.get(); if (ret.timestamp == 0) return new PhotonPipelineResult(); var result = ret.value; - // Set the timestamp of the result. Since PacketSubscriber doesn't realize that the result - // contains a thing with time knowledge, set it here. - // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds. - // TODO: NT4 time sync is Not To Be Trusted, we should do something else? - result.setReceiveTimestampMicros(ret.timestamp); + checkTimeSyncOrWarn(result); return result; } + private void checkTimeSyncOrWarn(PhotonPipelineResult result) { + if (result.metadata.timeSinceLastPong > 5L * 1000000L) { + if (Timer.getFPGATimestamp() > (prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) { + prevTimeSyncWarnTime = Timer.getFPGATimestamp(); + + DriverStation.reportWarning( + "PhotonVision coprocessor at path " + + path + + " is not connected to the TimeSyncServer? It's been " + + String.format("%.2f", result.metadata.timeSinceLastPong / 1e6) + + "s since the coprocessor last heard a pong.\n\nCheck /photonvision/.timesync/{COPROCESSOR_HOSTNAME} for more information.", + false); + } + } else { + // Got a valid packet, reset the last time + prevTimeSyncWarnTime = 0; + } + } + /** * Returns whether the camera is in driver mode. * diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index 757921859..76931a581 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -69,7 +69,7 @@ public class PhotonCameraSim implements AutoCloseable { private final PhotonCamera cam; NTTopicSet ts = new NTTopicSet(); - private long heartbeatCounter = 0; + private long heartbeatCounter = 1; /** This simulated camera's {@link SimCameraProperties} */ public final SimCameraProperties prop; @@ -553,9 +553,10 @@ public class PhotonCameraSim implements AutoCloseable { heartbeatCounter, now - (long) (latencyMillis * 1000), now, + // Pretend like we heard a pong recently + 1000L + (long) ((Math.random() - 0.5) * 50), detectableTgts, multitagResult); - ret.setReceiveTimestampMicros(now); return ret; } @@ -605,6 +606,8 @@ public class PhotonCameraSim implements AutoCloseable { ts.cameraIntrinsicsPublisher.set(prop.getIntrinsics().getData(), receiveTimestamp); ts.cameraDistortionPublisher.set(prop.getDistCoeffs().getData(), receiveTimestamp); - ts.heartbeatPublisher.set(heartbeatCounter++, receiveTimestamp); + + ts.heartbeatPublisher.set(heartbeatCounter, receiveTimestamp); + heartbeatCounter += 1; } } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java index 92bcf4c1c..da725eb8f 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java @@ -38,6 +38,7 @@ import java.util.Map; import org.opencv.core.Core; import org.opencv.core.CvType; import org.opencv.core.Mat; +import org.opencv.core.MatOfByte; import org.opencv.core.MatOfPoint; import org.opencv.core.MatOfPoint2f; import org.opencv.core.Point; @@ -99,11 +100,18 @@ public class VideoSimUtil { * * @param id The fiducial id of the desired tag */ - public static Mat get36h11TagImage(int id) { + private static Mat get36h11TagImage(int id) { RawFrame frame = AprilTag.generate36h11AprilTagImage(id); - Mat result = new Mat(10, 10, CvType.CV_8UC1, frame.getData(), frame.getStride()).clone(); - frame.close(); - return result; + + var buf = frame.getData(); + byte[] arr = new byte[buf.remaining()]; + buf.get(arr); + // frame.close(); + + var mat = new MatOfByte(arr).reshape(1, 10).submat(new Rect(0, 0, 10, 10)); + mat.dump(); + + return mat; } /** Gets the points representing the marker(black square) corners. */ diff --git a/photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java b/photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java new file mode 100644 index 000000000..c1d7e5826 --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java @@ -0,0 +1,52 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.photonvision.timesync; + +import java.io.IOException; +import org.photonvision.jni.PhotonTargetingJniLoader; +import org.photonvision.jni.TimeSyncServer; + +/** Helper to hold a single TimeSyncServer instance with some default config */ +public class TimeSyncSingleton { + private static TimeSyncServer INSTANCE = null; + + public static boolean load() { + if (INSTANCE == null) { + try { + if (!PhotonTargetingJniLoader.load()) { + return false; + } + } catch (UnsatisfiedLinkError | IOException e) { + e.printStackTrace(); + return false; + } + + INSTANCE = new TimeSyncServer(5810); + INSTANCE.start(); + } + + return INSTANCE != null; + } +} diff --git a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp index c8da8306b..46c13ffad 100644 --- a/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp @@ -330,7 +330,8 @@ PhotonPipelineResult PhotonCameraSim::Process( heartbeatCounter++; return PhotonPipelineResult{ PhotonPipelineMetadata{heartbeatCounter, 0, - units::microsecond_t{latency}.to()}, + units::microsecond_t{latency}.to(), + 1000000}, detectableTgts, multiTagResults}; } void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) { diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java index 4b02a8d76..95cc2736f 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java @@ -24,12 +24,25 @@ package org.photonvision; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTablesJNI; +import edu.wpi.first.wpilibj.Timer; +import java.io.IOException; import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.BeforeAll; import org.junit.jupiter.api.Test; import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.jni.PhotonTargetingJniLoader; +import org.photonvision.jni.WpilibLoader; import org.photonvision.targeting.PhotonPipelineResult; class PhotonCameraTest { + @BeforeAll + public static void load_wpilib() { + WpilibLoader.loadLibraries(); + } + @Test public void testEmpty() { Assertions.assertDoesNotThrow( @@ -40,4 +53,43 @@ class PhotonCameraTest { PhotonPipelineResult.photonStruct.pack(packet, ret); }); } + + // Just a smoketest for dev use -- don't run by default + @Test + public void testTimeSyncServerWithPhotonCamera() throws InterruptedException, IOException { + load_wpilib(); + PhotonTargetingJniLoader.load(); + + HAL.initialize(500, 0); + + NetworkTableInstance.getDefault().stopClient(); + NetworkTableInstance.getDefault().startServer(); + + var camera = new PhotonCamera("Arducam_OV2311_USB_Camera"); + PhotonCamera.setVersionCheckEnabled(false); + + for (int i = 0; i < 5; i++) { + Thread.sleep(500); + + var res = camera.getLatestResult(); + var captureTime = res.getTimestampSeconds(); + var now = Timer.getFPGATimestamp(); + + // expectTrue(captureTime < now); + + System.out.println( + "sequence " + + res.metadata.sequenceID + + " image capture " + + captureTime + + " received at " + + res.getTimestampSeconds() + + " now: " + + NetworkTablesJNI.now() / 1e6 + + " time since last pong: " + + res.metadata.timeSinceLastPong / 1e6); + } + + HAL.shutdown(); + } } diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 9ae688e7e..35ae0593c 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -64,8 +64,9 @@ class PhotonPoseEstimatorTest { cameraOne.result = new PhotonPipelineResult( 0, - 0, - 0, + 11 * 1000000, + 1100000, + 1024, List.of( new PhotonTrackedTarget( 3.0, @@ -130,7 +131,6 @@ class PhotonPoseEstimatorTest { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setReceiveTimestampMicros((long) (11 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d()); @@ -150,8 +150,9 @@ class PhotonPoseEstimatorTest { cameraOne.result = new PhotonPipelineResult( 0, - 0, - 0, + 4000000, + 1100000, + 1024, List.of( new PhotonTrackedTarget( 3.0, @@ -217,8 +218,6 @@ class PhotonPoseEstimatorTest { new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setReceiveTimestampMicros((long) (4 * 1e6)); - PhotonPoseEstimator estimator = new PhotonPoseEstimator( aprilTags, @@ -240,8 +239,9 @@ class PhotonPoseEstimatorTest { cameraOne.result = new PhotonPipelineResult( 0, - 0, - 0, + 17000000, + 1100000, + 1024, List.of( new PhotonTrackedTarget( 3.0, @@ -306,7 +306,6 @@ class PhotonPoseEstimatorTest { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setReceiveTimestampMicros((long) (17 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -330,8 +329,9 @@ class PhotonPoseEstimatorTest { cameraOne.result = new PhotonPipelineResult( 0, - 0, - 0, + 1000000, + 1100000, + 1024, List.of( new PhotonTrackedTarget( 3.0, @@ -396,7 +396,6 @@ class PhotonPoseEstimatorTest { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -412,8 +411,9 @@ class PhotonPoseEstimatorTest { cameraOne.result = new PhotonPipelineResult( 0, - 0, - 0, + 7000000, + 1100000, + 1024, List.of( new PhotonTrackedTarget( 3.0, @@ -478,7 +478,6 @@ class PhotonPoseEstimatorTest { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setReceiveTimestampMicros((long) (7 * 1e6)); estimatedPose = estimator.update(cameraOne.result); pose = estimatedPose.get().estimatedPose; @@ -495,8 +494,9 @@ class PhotonPoseEstimatorTest { var result = new PhotonPipelineResult( 0, - 0, - 0, + 20000000, + 1100000, + 1024, List.of( new PhotonTrackedTarget( 3.0, @@ -519,7 +519,6 @@ class PhotonPoseEstimatorTest { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - result.setReceiveTimestampMicros((long) (20 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -529,7 +528,7 @@ class PhotonPoseEstimatorTest { // Empty result, expect empty result cameraOne.result = new PhotonPipelineResult(); - cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6)); + cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6); Optional estimatedPose = estimator.update(cameraOne.result); assertFalse(estimatedPose.isPresent()); @@ -563,8 +562,9 @@ class PhotonPoseEstimatorTest { cameraOne.result = new PhotonPipelineResult( 0, - 0, - 0, + 20 * 1000000, + 1100000, + 1024, List.of( new PhotonTrackedTarget( 3.0, @@ -629,7 +629,6 @@ class PhotonPoseEstimatorTest { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); // 3 3 3 ambig .4 - cameraOne.result.setReceiveTimestampMicros(20 * 1000000); PhotonPoseEstimator estimator = new PhotonPoseEstimator( diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index 859aaf1cb..401ac19bd 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -27,9 +27,11 @@ package org.photonvision; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.fail; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.hal.HAL; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -39,28 +41,96 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.io.IOException; import java.util.ArrayList; import java.util.List; import java.util.stream.Stream; -import org.junit.jupiter.api.AfterAll; +import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.junit.jupiter.params.ParameterizedTest; import org.junit.jupiter.params.provider.Arguments; import org.junit.jupiter.params.provider.MethodSource; import org.junit.jupiter.params.provider.ValueSource; +import org.photonvision.estimation.OpenCVHelp; import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; +import org.photonvision.jni.PhotonTargetingJniLoader; +import org.photonvision.jni.WpilibLoader; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.VisionSystemSim; import org.photonvision.simulation.VisionTargetSim; +import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; class VisionSystemSimTest { private static final double kTrlDelta = 0.005; private static final double kRotDeltaDeg = 0.25; + NetworkTableInstance inst; + + @BeforeAll + public static void setUp() { + WpilibLoader.loadLibraries(); + try { + if (!PhotonTargetingJniLoader.load()) fail(); + } catch (UnsatisfiedLinkError | IOException e) { + e.printStackTrace(); + fail(e); + } + + OpenCVHelp.forceLoadOpenCV(); + } + + @BeforeEach + public void init() { + // // No version check for testing + // PhotonCamera.setVersionCheckEnabled(false); + } + + @BeforeEach + public void setup() { + HAL.initialize(500, 0); + + inst = NetworkTableInstance.create(); + inst.stopClient(); + inst.stopServer(); + inst.startLocal(); + SmartDashboard.setNetworkTableInstance(inst); + } + + @AfterEach + public void teardown() { + inst.close(); + inst = null; + + HAL.shutdown(); + } + + private PhotonPipelineResult waitForSequenceNumber(PhotonCamera camera, int seq) { + assertTrue(camera.heartbeatEntry.getTopic().getHandle() != 0); + + System.out.println( + "Waiting for seq=" + seq + " on " + camera.heartbeatEntry.getTopic().getName()); + // wait up to 1 second for a new result + for (int i = 0; i < 100; i++) { + var res = camera.getLatestResult(); + if (res.metadata.sequenceID == seq) { + return res; + } + + try { + Thread.sleep(10); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + throw new RuntimeException("Never saw sequence number " + seq); + } + @Test public void testEmpty() { Assertions.assertDoesNotThrow( @@ -74,41 +144,12 @@ class VisionSystemSimTest { }); } - @BeforeAll - public static void setUp() { - // NT live for debug purposes - NetworkTableInstance.getDefault().startServer(); - - // No version check for testing - PhotonCamera.setVersionCheckEnabled(false); - } - - @AfterAll - public static void shutDown() {} - - // @ParameterizedTest - // @ValueSource(doubles = {5, 10, 15, 20, 25, 30}) - // public void testDistanceAligned(double dist) { - // final var targetPose = new Pose2d(new Translation2d(15.98, 0), new Rotation2d()); - // var sysUnderTest = - // new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 320, 240, 0); - // sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 1.0, 1.0)); - - // final var robotPose = new Pose2d(new Translation2d(35 - dist, 0), new Rotation2d()); - // sysUnderTest.processFrame(robotPose); - - // var result = sysUnderTest.cam.getLatestResult(); - - // assertTrue(result.hasTargets()); - // assertEquals(result.getBestTarget().getCameraToTarget().getTranslation().getNorm(), dist); - // } - @Test public void testVisibilityCupidShuffle() { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI)); var visionSysSim = new VisionSystemSim("Test"); - var camera = new PhotonCamera("camera"); + var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); @@ -117,42 +158,51 @@ class VisionSystemSimTest { // To the right, to the right var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-70)); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // To the right, to the right robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-95)); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // To the left, to the left robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(90)); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // To the left, to the left robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(65)); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // now kick, now kick robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); + + waitForSequenceNumber(camera, 5); assertTrue(camera.getLatestResult().hasTargets()); // now kick, now kick robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-5)); visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); // now walk it by yourself robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-179)); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // now walk it by yourself visionSysSim.adjustCamera( cameraSim, new Transform3d(new Translation3d(), new Rotation3d(0, 0, Math.PI))); visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); } @@ -161,7 +211,7 @@ class VisionSystemSimTest { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI)); var visionSysSim = new VisionSystemSim("Test"); - var camera = new PhotonCamera("camera"); + var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); @@ -169,12 +219,14 @@ class VisionSystemSimTest { var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); - assertTrue(camera.getLatestResult().hasTargets()); + + assertTrue(waitForSequenceNumber(camera, 1).hasTargets()); visionSysSim.adjustCamera( // vooop selfie stick cameraSim, new Transform3d(new Translation3d(0, 0, 5000), new Rotation3d(0, 0, Math.PI))); visionSysSim.update(robotPose); - assertFalse(camera.getLatestResult().hasTargets()); + + assertFalse(waitForSequenceNumber(camera, 2).hasTargets()); } @Test @@ -184,7 +236,7 @@ class VisionSystemSimTest { var robotToCamera = new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, -Math.PI / 4, 0)); var visionSysSim = new VisionSystemSim("Test"); - var camera = new PhotonCamera("camera"); + var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, robotToCamera); cameraSim.prop.setCalibration(1234, 1234, Rotation2d.fromDegrees(80)); @@ -192,12 +244,14 @@ class VisionSystemSimTest { var robotPose = new Pose2d(new Translation2d(13.98, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); - assertTrue(camera.getLatestResult().hasTargets()); + + assertTrue(waitForSequenceNumber(camera, 1).hasTargets()); // Pitched back camera should mean target goes out of view below the robot as distance increases robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); - assertFalse(camera.getLatestResult().hasTargets()); + + assertFalse(waitForSequenceNumber(camera, 2).hasTargets()); } @Test @@ -205,7 +259,7 @@ class VisionSystemSimTest { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI)); var visionSysSim = new VisionSystemSim("Test"); - var camera = new PhotonCamera("camera"); + var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); @@ -214,11 +268,13 @@ class VisionSystemSimTest { var robotPose = new Pose2d(new Translation2d(12, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); - assertTrue(camera.getLatestResult().hasTargets()); + + assertTrue(waitForSequenceNumber(camera, 1).hasTargets()); robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); - assertFalse(camera.getLatestResult().hasTargets()); + + assertFalse(waitForSequenceNumber(camera, 2).hasTargets()); } @Test @@ -226,7 +282,7 @@ class VisionSystemSimTest { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI)); var visionSysSim = new VisionSystemSim("Test"); - var camera = new PhotonCamera("camera"); + var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); @@ -236,20 +292,22 @@ class VisionSystemSimTest { var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); - assertTrue(camera.getLatestResult().hasTargets()); + + assertTrue(waitForSequenceNumber(camera, 1).hasTargets()); robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); - assertFalse(camera.getLatestResult().hasTargets()); + + assertFalse(waitForSequenceNumber(camera, 2).hasTargets()); } @ParameterizedTest @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23}) - public void testYawAngles(double testYaw) { + public void testYawAngles(double testYaw) throws InterruptedException { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, 3 * Math.PI / 4)); var visionSysSim = new VisionSystemSim("Test"); - var camera = new PhotonCamera("camera"); + var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); @@ -259,7 +317,8 @@ class VisionSystemSimTest { // If the robot is rotated x deg (CCW+), the target yaw should be x deg (CW+) var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(testYaw)); visionSysSim.update(robotPose); - var res = camera.getLatestResult(); + + var res = waitForSequenceNumber(camera, 1); assertTrue(res.hasTargets()); var tgt = res.getBestTarget(); assertEquals(testYaw, tgt.getYaw(), kRotDeltaDeg); @@ -267,12 +326,12 @@ class VisionSystemSimTest { @ParameterizedTest @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999}) - public void testPitchAngles(double testPitch) { + public void testPitchAngles(double testPitch) throws InterruptedException { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, 3 * Math.PI / 4)); final var robotPose = new Pose2d(new Translation2d(10, 0), new Rotation2d(0)); var visionSysSim = new VisionSystemSim("Test"); - var camera = new PhotonCamera("camera"); + var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(120)); @@ -284,8 +343,10 @@ class VisionSystemSimTest { cameraSim, new Transform3d( new Translation3d(), new Rotation3d(0, Units.degreesToRadians(testPitch), 0))); + visionSysSim.update(robotPose); - var res = camera.getLatestResult(); + var res = waitForSequenceNumber(camera, 1); + System.out.println("Got result: " + res); assertTrue(res.hasTargets()); var tgt = res.getBestTarget(); @@ -335,7 +396,7 @@ class VisionSystemSimTest { var visionSysSim = new VisionSystemSim( "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!"); - var camera = new PhotonCamera("camera"); + var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(160)); @@ -349,7 +410,8 @@ class VisionSystemSimTest { // 1. These are calculated with the average of the minimum area rectangle, which does not // actually find the target center because of perspective distortion. // 2. Yaw and pitch are calculated separately which gives incorrect pitch values. - var res = camera.getLatestResult(); + + var res = waitForSequenceNumber(camera, 1); assertTrue(res.hasTargets()); var tgt = res.getBestTarget(); assertEquals(0.0, tgt.getYaw(), 0.5); @@ -375,7 +437,7 @@ class VisionSystemSimTest { new Pose3d(new Translation3d(15.98, -2, 0), new Rotation3d(0, 0, Math.PI)); var visionSysSim = new VisionSystemSim("Test"); - var camera = new PhotonCamera("camera"); + var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); @@ -450,7 +512,9 @@ class VisionSystemSimTest { var robotPose = new Pose2d(new Translation2d(6.0, 0), Rotation2d.fromDegrees(0.25)); visionSysSim.update(robotPose); - var res = camera.getLatestResult(); + + var res = waitForSequenceNumber(camera, 1); + assertTrue(res.hasTargets()); List tgtList; tgtList = res.getTargets(); @@ -460,7 +524,7 @@ class VisionSystemSimTest { @Test public void testPoseEstimation() { var visionSysSim = new VisionSystemSim("Test"); - var camera = new PhotonCamera("camera"); + var camera = new PhotonCamera(inst, "camera"); var cameraSim = new PhotonCameraSim(camera); visionSysSim.addCamera(cameraSim, new Transform3d()); cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(90)); @@ -479,11 +543,12 @@ class VisionSystemSimTest { new VisionTargetSim(tagList.get(0).pose, TargetModel.kAprilTag16h5, 0)); visionSysSim.update(robotPose); + var results = VisionEstimation.estimateCamPosePNP( camera.getCameraMatrix().get(), camera.getDistCoeffs().get(), - camera.getLatestResult().getTargets(), + waitForSequenceNumber(camera, 1).getTargets(), layout, TargetModel.kAprilTag16h5) .get(); @@ -499,6 +564,7 @@ class VisionSystemSimTest { new VisionTargetSim(tagList.get(2).pose, TargetModel.kAprilTag16h5, 2)); visionSysSim.update(robotPose); + results = VisionEstimation.estimateCamPosePNP( camera.getCameraMatrix().get(), diff --git a/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java b/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java deleted file mode 100644 index 24ea9fa6c..000000000 --- a/photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java +++ /dev/null @@ -1,79 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.photonvision.estimation; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.io.IOException; -import org.junit.jupiter.api.BeforeAll; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; - -public class ApriltagWorkbenchTest { - @BeforeAll - public static void setUp() { - // No version check for testing - PhotonCamera.setVersionCheckEnabled(false); - } - - // @Test - public void testMeme() throws IOException, InterruptedException { - NetworkTableInstance instance = NetworkTableInstance.getDefault(); - instance.stopServer(); - // set the NT server if simulating this code. - // "localhost" for photon on desktop, or "photonvision.local" / "[ip-address]" - // for coprocessor - instance.setServer("localhost"); - instance.startClient4("myRobot"); - - var robotToCamera = new Transform3d(); - var cam = new PhotonCamera("WPI2023"); - var tagLayout = - AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile); - - var pe = - new PhotonPoseEstimator( - tagLayout, - PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - robotToCamera); - - var field = new Field2d(); - SmartDashboard.putData(field); - - while (!Thread.interrupted()) { - Thread.sleep(500); - - for (var change : cam.getAllUnreadResults()) { - var ret = pe.update(change); - System.out.println(ret); - field.setRobotPose(ret.get().estimatedPose.toPose2d()); - } - } - } -} diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index 3d8c86d9c..0838c3e07 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -86,7 +86,7 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11)); photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY, @@ -147,7 +147,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; cameraOne.testResult[0].SetReceiveTimestamp(17_s); photon::PhotonPoseEstimator estimator( @@ -196,7 +196,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, @@ -247,7 +247,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17)); photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE, @@ -287,7 +287,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) { 0.4, corners, detectedCorners}}; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000}, targetsThree, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree, + std::nullopt}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21)); // std::optional estimatedPose; @@ -333,7 +334,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) { cameraOne.test = true; cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS, @@ -387,7 +388,7 @@ TEST(PhotonPoseEstimatorTest, PoseCache) { // empty input, expect empty out cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000}, + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, std::vector{}, std::nullopt}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1)); @@ -400,7 +401,7 @@ TEST(PhotonPoseEstimatorTest, PoseCache) { // Set result, and update -- expect present and timestamp to be 15 cameraOne.testResult = {photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 3000}, targets, std::nullopt}}; + photon::PhotonPipelineMetadata{0, 0, 3000, 1000}, targets, std::nullopt}}; cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15)); for (const auto& result : cameraOne.GetAllUnreadResults()) { @@ -422,7 +423,7 @@ TEST(PhotonPoseEstimatorTest, CopyResult) { std::vector targets{}; auto testResult = photon::PhotonPipelineResult{ - photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}; + photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}; testResult.SetReceiveTimestamp(units::second_t(11)); auto test2 = testResult; diff --git a/photon-serde/messages.yaml b/photon-serde/messages.yaml index d7121c36c..cbdf4e90e 100644 --- a/photon-serde/messages.yaml +++ b/photon-serde/messages.yaml @@ -7,6 +7,8 @@ type: int64 - name: publishTimestampMicros type: int64 + - name: timeSinceLastPong + type: int64 - name: Transform3d shimmed: True diff --git a/photon-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index 4330cefc0..a88a9ca16 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -17,6 +17,7 @@ package org.photonvision; +import edu.wpi.first.hal.HAL; import java.io.IOException; import java.nio.file.Files; import java.nio.file.Path; @@ -39,6 +40,7 @@ import org.photonvision.common.logging.PvCSCoreLogger; import org.photonvision.common.networking.NetworkManager; import org.photonvision.common.util.TestUtils; import org.photonvision.common.util.numbers.IntegerCouple; +import org.photonvision.jni.PhotonTargetingJniLoader; import org.photonvision.jni.RknnDetectorJNI; import org.photonvision.mrcal.MrCalJNILoader; import org.photonvision.raspi.LibCameraJNILoader; @@ -379,7 +381,25 @@ public class Main { logger.error("Failed to load native libraries!", e); System.exit(1); } - logger.info("Native libraries loaded."); + logger.info("WPI JNI libraries loaded."); + + try { + boolean success = PhotonTargetingJniLoader.load(); + + if (!success) { + logger.error("Failed to load native libraries! Giving up :("); + System.exit(1); + } + } catch (Exception e) { + logger.error("Failed to load photon-targeting JNI!", e); + System.exit(1); + } + logger.info("photon-targeting JNI libraries loaded."); + + if (!HAL.initialize(500, 0)) { + logger.error("Failed to initialize the HAL! Giving up :("); + System.exit(1); + } try { if (Platform.isRaspberryPi()) { @@ -438,6 +458,7 @@ public class Main { logger.debug("Loading NetworkTablesManager..."); NetworkTablesManager.getInstance() .setConfig(ConfigManager.getInstance().getConfig().getNetworkConfig()); + NetworkTablesManager.getInstance().registerTimedTasks(); if (isSmoketest) { logger.info("PhotonVision base functionality loaded -- smoketest complete"); diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index da7dde2d0..157150cc2 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -93,12 +93,23 @@ model { nativeUtils.useRequiredLibrary(it, "wpiutil_shared") nativeUtils.useRequiredLibrary(it, "wpinet_shared") nativeUtils.useRequiredLibrary(it, "ntcore_shared") + nativeUtils.useRequiredLibrary(it, "wpimath_shared") } all { binaries.withType(SharedLibraryBinarySpec) { binary -> // check that we're building for the platform (per PArchOverride/wpilib plat detection) - if (binary.targetPlatform.name == jniPlatform) { + def platName = jniPlatform + def realWpilibName = wpilibNativeName; + + if (jniPlatform.equals("osxarm64") || jniPlatform.equals("osxx86-64")) { + // native-utils calls this osxuniversal + platName = "osxuniversal"; + realWpilibName = "osxuniversal"; + } + + if (binary.targetPlatform.name == platName) { + // only include release binaries (hard coded for now) def isDebug = binary.buildType.name.contains('debug') @@ -106,7 +117,7 @@ model { syncOutputsFolder { // Just shove the shared library into the root of the jar output by photon-targeting:jar from(binary.sharedLibraryFile) { - into "nativelibraries/${wpilibNativeName}/" + into "nativelibraries/${realWpilibName}/" } // And (not sure if this is a hack) make the jar task depend on the build task dependsOn binary.identifier.projectScopedName @@ -206,6 +217,7 @@ def nativeTasks = wpilibTools.createExtractionTasks { nativeTasks.addToSourceSetResources(sourceSets.test) +nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpilibc") nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil") nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath") nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") diff --git a/photon-targeting/src/generated/main/java/org/photonvision/jni/WpilibLoader.java b/photon-targeting/src/generated/main/java/org/photonvision/jni/WpilibLoader.java new file mode 100644 index 000000000..80ded92c5 --- /dev/null +++ b/photon-targeting/src/generated/main/java/org/photonvision/jni/WpilibLoader.java @@ -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 . + */ + +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; + } +} diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java index fb87bea14..df28ed906 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java @@ -35,9 +35,9 @@ import edu.wpi.first.util.struct.Struct; */ public class PhotonPipelineMetadataSerde implements PacketSerde { @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 { @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[] getNestedPhotonMessages() { return new PacketSerde[] { - PhotonPipelineMetadata.photonStruct,MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct + MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct,PhotonPipelineMetadata.photonStruct }; } diff --git a/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineMetadataSerde.cpp b/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineMetadataSerde.cpp index e5b44f187..2aece0916 100644 --- a/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineMetadataSerde.cpp +++ b/photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineMetadataSerde.cpp @@ -28,6 +28,7 @@ void StructType::Pack(Packet& packet, const PhotonPipelineMetadata& value) { packet.Pack(value.sequenceID); packet.Pack(value.captureTimestampMicros); packet.Pack(value.publishTimestampMicros); + packet.Pack(value.timeSinceLastPong); } PhotonPipelineMetadata StructType::Unpack(Packet& packet) { @@ -35,6 +36,7 @@ PhotonPipelineMetadata StructType::Unpack(Packet& packet) { .sequenceID = packet.Unpack(), .captureTimestampMicros = packet.Unpack(), .publishTimestampMicros = packet.Unpack(), + .timeSinceLastPong = packet.Unpack(), }}; } diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h index ecdc88237..d35454126 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h @@ -34,12 +34,12 @@ namespace photon { template <> struct WPILIB_DLLEXPORT SerdeType { 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); diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h index 49845033c..dd0e97eca 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h @@ -39,11 +39,11 @@ namespace photon { template <> struct WPILIB_DLLEXPORT SerdeType { 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 " diff --git a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineMetadataStruct.h b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineMetadataStruct.h index a6e344071..de7e39767 100644 --- a/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineMetadataStruct.h +++ b/photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineMetadataStruct.h @@ -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; diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java index 3c36d1977..06f702769 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java @@ -87,11 +87,9 @@ public class PacketPublisher 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); diff --git a/photon-targeting/src/main/java/org/photonvision/jni/PhotonTargetingJniLoader.java b/photon-targeting/src/main/java/org/photonvision/jni/PhotonTargetingJniLoader.java new file mode 100644 index 000000000..a49e83206 --- /dev/null +++ b/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 . + */ + +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; + } +} diff --git a/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncClient.java new file mode 100644 index 000000000..eee1047f7 --- /dev/null +++ b/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 . + */ + +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); +} diff --git a/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncServer.java b/photon-targeting/src/main/java/org/photonvision/jni/TimeSyncServer.java new file mode 100644 index 000000000..455702531 --- /dev/null +++ b/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 . + */ + +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); +} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java index 2076cdb47..4633a31fe 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java +++ b/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 { - // 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 */ diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 75a488229..9537dfb37 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -40,9 +40,6 @@ public class PhotonPipelineResult // Multi-tag result public Optional 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 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 targets, Optional 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; } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java index 0451aad72..25b29d144 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java +++ b/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())) diff --git a/photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp b/photon-targeting/src/main/native/cpp/net/TimeSyncClient.cpp new file mode 100644 index 000000000..238366ddf --- /dev/null +++ b/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 . + */ + +#include "net/TimeSyncClient.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#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::GetSize()> pingData( + wpi::Struct::GetSize()); + wpi::PackStruct(pingData, ping); + + // Wrap our buffer - pingData should free itself + wpi::uv::Buffer pingBuf{pingData}; + int sent = m_udp->TrySend(wpi::SmallVector{pingBuf}); + + if (static_cast(sent) != wpi::Struct::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(nbytes) != wpi::Struct::GetSize()) { + WPI_ERROR(m_logger, "Got {} bytes for pong?", nbytes); + return; + } + + TspPong pong{ + wpi::UnpackStruct(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 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; +} diff --git a/photon-targeting/src/main/native/cpp/net/TimeSyncServer.cpp b/photon-targeting/src/main/native/cpp/net/TimeSyncServer.cpp new file mode 100644 index 000000000..baf296a6b --- /dev/null +++ b/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 . + */ + +#include "net/TimeSyncServer.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#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(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::GetSize()> pongData( + wpi::Struct::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{pongBuf}); + // fmt::println("Pong ret: {}", sent); + if (static_cast(sent) != wpi::Struct::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 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(); } diff --git a/photon-targeting/src/main/native/include/net/TimeSyncClient.h b/photon-targeting/src/main/native/include/net/TimeSyncClient.h new file mode 100644 index 000000000..d8bdce372 --- /dev/null +++ b/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 . + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#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; + using SharedTimerPtr = std::shared_ptr; + + EventLoopRunner m_loopRunner{}; + + wpi::Logger m_logger; + std::function 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 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 timeProvider = nt::Now); + + void Start(); + void Stop(); + int64_t GetOffset(); + Metadata GetMetadata(); +}; + +} // namespace tsp +} // namespace wpi diff --git a/photon-targeting/src/main/native/include/net/TimeSyncServer.h b/photon-targeting/src/main/native/include/net/TimeSyncServer.h new file mode 100644 index 000000000..faf1b9a4c --- /dev/null +++ b/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 . + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "TimeSyncStructs.h" +#include "ntcore_cpp.h" + +namespace wpi { +namespace tsp { + +class TimeSyncServer { + using SharedUdpPtr = std::shared_ptr; + + EventLoopRunner m_loopRunner{}; + + wpi::Logger m_logger; + std::function 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 timeProvider = nt::Now); + + /** + * Start listening for pings + */ + void Start(); + /** + * Stop our loop runner. After stopping, we cannot restart. + */ + void Stop(); +}; + +} // namespace tsp +} // namespace wpi diff --git a/photon-targeting/src/main/native/include/net/TimeSyncStructs.h b/photon-targeting/src/main/native/include/net/TimeSyncStructs.h new file mode 100644 index 000000000..887025972 --- /dev/null +++ b/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 . + */ + +#pragma once +#include + +#include + +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 { + 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 data) { + return wpi::tsp::TspPing{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }; + } + static void Pack(std::span 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 { + 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 data) { + return wpi::tsp::TspPong{ + wpi::tsp::TspPing{ + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + wpi::UnpackStruct(data), + }, + wpi::UnpackStruct(data), + }; + } + static void Pack(std::span 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); +static_assert(wpi::StructSerializable); diff --git a/photon-targeting/src/main/native/jni/TimeSyncClientJNI.cpp b/photon-targeting/src/main/native/jni/TimeSyncClientJNI.cpp new file mode 100644 index 000000000..d84572c65 --- /dev/null +++ b/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 . + */ + +#include + +#include +#include + +#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(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(&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, "", "(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( + new TimeSyncClient(cpp_name, static_cast(port), + std::chrono::duration_cast( + std::chrono::duration(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(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(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(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(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" diff --git a/photon-targeting/src/main/native/jni/TimeSyncServerJNI.cpp b/photon-targeting/src/main/native/jni/TimeSyncServerJNI.cpp new file mode 100644 index 000000000..3c7fc2dac --- /dev/null +++ b/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 . + */ + +#include +#include + +#include + +#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(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(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(ptr); + server->Stop(); + delete server; +} + +} // extern "C" diff --git a/photon-targeting/src/main/proto/photon.proto b/photon-targeting/src/main/proto/photon.proto index a13a61f75..6650785bd 100644 --- a/photon-targeting/src/main/proto/photon.proto +++ b/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; } diff --git a/photon-targeting/src/test/java/net/TimeSyncTest.java b/photon-targeting/src/test/java/net/TimeSyncTest.java new file mode 100644 index 000000000..f5ea486f7 --- /dev/null +++ b/photon-targeting/src/test/java/net/TimeSyncTest.java @@ -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 . + */ + +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(); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/PacketTest.java b/photon-targeting/src/test/java/org/photonvision/PacketTest.java index 3ed692af4..4b1b140d1 100644 --- a/photon-targeting/src/test/java/org/photonvision/PacketTest.java +++ b/photon-targeting/src/test/java/org/photonvision/PacketTest.java @@ -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, diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java index 5195e50d1..95d9506c3 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java @@ -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, diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java index 234ebfaed..a60dddbed 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java @@ -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, diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp index 5398fae4c..153f1c96a 100644 --- a/photon-targeting/src/test/native/cpp/PacketTest.cpp +++ b/photon-targeting/src/test/native/cpp/PacketTest.cpp @@ -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{}, std::nullopt); Packet p; @@ -130,7 +130,7 @@ TEST(PacketTest, PhotonPipelineResult) { 17.0, 22.33, 2.54}, std::vector{8, 7, 11, 22, 59, 40}}; - PhotonPipelineResult result2(PhotonPipelineMetadata{0, 0, 1}, targets, + PhotonPipelineResult result2(PhotonPipelineMetadata{0, 0, 1, 1}, targets, mtResult); Packet p2; diff --git a/photon-targeting/src/test/native/cpp/net/TimeSyncTest.cpp b/photon-targeting/src/test/native/cpp/net/TimeSyncTest.cpp new file mode 100644 index 000000000..eeec41ee3 --- /dev/null +++ b/photon-targeting/src/test/native/cpp/net/TimeSyncTest.cpp @@ -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 . + */ + +#include +#include +#include +#include + +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(); +} diff --git a/photonlib-java-examples/aimattarget/build.gradle b/photonlib-java-examples/aimattarget/build.gradle index 4d1761e7a..18108a1ef 100644 --- a/photonlib-java-examples/aimattarget/build.gradle +++ b/photonlib-java-examples/aimattarget/build.gradle @@ -24,7 +24,7 @@ deploy { // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you // want to store a team number in this file. - team = project.frc.getTeamOrDefault(4512) + team = project.frc.getTeamOrDefault(1736) debug = project.frc.getDebugOrDefault(false) artifacts { diff --git a/shared/javacpp/publish.gradle b/shared/javacpp/publish.gradle index 860bf248e..d6c946806 100644 --- a/shared/javacpp/publish.gradle +++ b/shared/javacpp/publish.gradle @@ -1,3 +1,4 @@ +import java.security.MessageDigest apply plugin: 'maven-publish' def outputsFolder = file("$buildDir/outputs") @@ -6,12 +7,15 @@ def baseArtifactId = nativeName def artifactGroupId = 'org.photonvision' def zipBaseName = "_GROUP_org_photonvision_${baseArtifactId}_ID_${baseArtifactId}-cpp_CLS" +def jniBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-jni_CLS" +def jniCvStaticBaseName = "_GROUP_edu_wpi_first_${nativeName}_ID_${nativeName}-jnicvstatic_CLS" + +def licenseFile = file("$rootDir/LICENCE") + // Quick hack to make this name visible to photon-lib for combined ext.zipBaseName = zipBaseName ext.artifactGroupId = artifactGroupId -def licenseFile = file("$rootDir/LICENCE") - task cppSourcesZip(type: Zip) { destinationDirectory = outputsFolder archiveBaseName = zipBaseName @@ -72,11 +76,34 @@ addTaskToCopyAllOutputs(cppHeadersZip) model { publishing { - def taskList = createComponentZipTasks($.components, [nativeName], zipBaseName, Zip, project, includeStandardZipFormat) + def cppTaskList = createComponentZipTasks($.components, [nativeName], zipBaseName, Zip, project, includeStandardZipFormat) + + // From https://github.com/wpilibsuite/allwpilib/blob/1c220ebc607daa8da7d983b8f17bc40323633cb2/shared/jni/publish.gradle#L80C9-L100C11 + def jniTaskList = createComponentZipTasks($.components, ["${nativeName}JNI"], jniBaseName, Jar, project, { task, value -> + value.each { binary -> + if (binary.buildable) { + if (binary instanceof SharedLibraryBinarySpec) { + task.dependsOn binary.tasks.link + def hashFile = new File(binary.sharedLibraryFile.parentFile.absolutePath, "${binary.component.baseName}.hash") + task.outputs.file(hashFile) + task.inputs.file(binary.sharedLibraryFile) + task.from(hashFile) { + into nativeUtils.getPlatformPath(binary) + } + task.doFirst { + hashFile.text = MessageDigest.getInstance("MD5").digest(binary.sharedLibraryFile.bytes).encodeHex().toString() + } + task.from(binary.sharedLibraryFile) { + into nativeUtils.getPlatformPath(binary) + } + } + } + } + }) publications { cpp(MavenPublication) { - taskList.each { + cppTaskList.each { artifact it } artifact cppHeadersZip @@ -86,6 +113,15 @@ model { groupId artifactGroupId version pubVersion } + jni(MavenPublication) { + jniTaskList.each { + artifact it + } + + artifactId = "${baseArtifactId}-jni" + groupId artifactGroupId + version pubVersion + } } repositories {