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

Create timesync JNI for testing client (#1433)

Browse Source
This commit is contained in:
Matt
2024-10-31 08:27:19 -07:00
committed by GitHub
parent 937bafa8e2
commit 37aaa49b32
69 changed files with 2252 additions and 368 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
.github/workflows/build.yml vendored
View File

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

5
.github/workflows/photonvision-docs.yml vendored
View File

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

2
.readthedocs.yaml
View File

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

1
docs/source/conf.py
View File

@@ -37,6 +37,7 @@ extensions = [
"sphinx_design",
"myst_parser",
"sphinx.ext.mathjax",
"sphinx.ext.graphviz",
]
# Configure OpenGraph support

1
docs/source/docs/contributing/design-descriptions/index.md
View File

@@ -3,4 +3,5 @@
```{toctree}
:maxdepth: 1
image-rotation
time-sync
```

111
docs/source/docs/contributing/design-descriptions/time-sync.md Normal file
View File

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

1
photon-core/build.gradle
View File

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

8
photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java
View File

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

16
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View File

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

126
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java
View File

@@ -18,6 +18,7 @@
package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.networktables.LogMessage;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEvent;
import edu.wpi.first.networktables.NetworkTableEvent.Kind;
@@ -26,7 +27,6 @@ import edu.wpi.first.networktables.StringSubscriber;
import java.io.IOException;
import java.util.EnumSet;
import java.util.HashMap;
import java.util.function.Consumer;
import org.photonvision.PhotonVersion;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.configuration.NetworkConfig;
@@ -34,6 +34,7 @@ import org.photonvision.common.dataflow.DataChangeService;
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
import org.photonvision.common.hardware.HardwareManager;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.LogLevel;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.scripting.ScriptEventType;
import org.photonvision.common.scripting.ScriptManager;
@@ -41,32 +42,39 @@ import org.photonvision.common.util.TimedTaskManager;
import org.photonvision.common.util.file.JacksonUtils;
public class NetworkTablesManager {
private static final Logger logger =
new Logger(NetworkTablesManager.class, LogGroup.NetworkTables);
private final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault();
private final String kRootTableName = "/photonvision";
private final String kFieldLayoutName = "apriltag_field_layout";
public final NetworkTable kRootTable = ntInstance.getTable(kRootTableName);
private final NTLogger m_ntLogger = new NTLogger();
private boolean m_isRetryingConnection = false;
private StringSubscriber m_fieldLayoutSubscriber =
kRootTable.getStringTopic(kFieldLayoutName).subscribe("");
private final TimeSyncManager m_timeSync = new TimeSyncManager(kRootTable);
private NetworkTablesManager() {
ntInstance.addLogger(255, 255, (event) -> {}); // to hide error messages
ntInstance.addConnectionListener(true, m_ntLogger); // to hide error messages
ntInstance.addLogger(
LogMessage.kInfo, LogMessage.kCritical, this::logNtMessage); // to hide error messages
ntInstance.addConnectionListener(true, this::checkNtConnectState); // to hide error messages
ntInstance.addListener(
m_fieldLayoutSubscriber, EnumSet.of(Kind.kValueAll), this::onFieldLayoutChanged);
TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000);
// Get the UI state in sync with the backend. NT should fire a callback when it first connects
// to the robot
broadcastConnectedStatus();
}
public void registerTimedTasks() {
m_timeSync.start();
TimedTaskManager.getInstance().addTask("NTManager", this::ntTick, 5000);
}
private static NetworkTablesManager INSTANCE;
public static NetworkTablesManager getInstance() {
@@ -74,43 +82,72 @@ public class NetworkTablesManager {
return INSTANCE;
}
private static final Logger logger = new Logger(NetworkTablesManager.class, LogGroup.General);
private void logNtMessage(NetworkTableEvent event) {
String levelmsg = "DEBUG";
LogLevel pvlevel = LogLevel.DEBUG;
if (event.logMessage.level >= LogMessage.kCritical) {
pvlevel = LogLevel.ERROR;
levelmsg = "CRITICAL";
} else if (event.logMessage.level >= LogMessage.kError) {
pvlevel = LogLevel.ERROR;
levelmsg = "ERROR";
} else if (event.logMessage.level >= LogMessage.kWarning) {
pvlevel = LogLevel.WARN;
levelmsg = "WARNING";
} else if (event.logMessage.level >= LogMessage.kInfo) {
pvlevel = LogLevel.INFO;
levelmsg = "INFO";
}
private static class NTLogger implements Consumer<NetworkTableEvent> {
private boolean hasReportedConnectionFailure = false;
logger.log(
"NT: "
+ levelmsg
+ " "
+ event.logMessage.level
+ ": "
+ event.logMessage.message
+ " ("
+ event.logMessage.filename
+ ":"
+ event.logMessage.line
+ ")",
pvlevel);
}
@Override
public void accept(NetworkTableEvent event) {
var isConnEvent = event.is(Kind.kConnected);
var isDisconnEvent = event.is(Kind.kDisconnected);
public void checkNtConnectState(NetworkTableEvent event) {
var isConnEvent = event.is(Kind.kConnected);
var isDisconnEvent = event.is(Kind.kDisconnected);
if (!hasReportedConnectionFailure && isDisconnEvent) {
var msg =
String.format(
"NT lost connection to %s:%d! (NT version %d). Will retry in background.",
event.connInfo.remote_ip,
event.connInfo.remote_port,
event.connInfo.protocol_version);
logger.error(msg);
HardwareManager.getInstance().setNTConnected(false);
if (isDisconnEvent) {
var msg =
String.format(
"NT lost connection to %s:%d! (NT version %d). Will retry in background.",
event.connInfo.remote_ip,
event.connInfo.remote_port,
event.connInfo.protocol_version);
logger.error(msg);
HardwareManager.getInstance().setNTConnected(false);
hasReportedConnectionFailure = true;
getInstance().broadcastConnectedStatus();
} else if (isConnEvent && event.connInfo != null) {
var msg =
String.format(
"NT connected to %s:%d! (NT version %d)",
event.connInfo.remote_ip,
event.connInfo.remote_port,
event.connInfo.protocol_version);
logger.info(msg);
HardwareManager.getInstance().setNTConnected(true);
getInstance().broadcastConnectedStatus();
} else if (isConnEvent && event.connInfo != null) {
var msg =
String.format(
"NT connected to %s:%d! (NT version %d)",
event.connInfo.remote_ip,
event.connInfo.remote_port,
event.connInfo.protocol_version);
logger.info(msg);
HardwareManager.getInstance().setNTConnected(true);
hasReportedConnectionFailure = false;
ScriptManager.queueEvent(ScriptEventType.kNTConnected);
getInstance().broadcastVersion();
getInstance().broadcastConnectedStatus();
}
ScriptManager.queueEvent(ScriptEventType.kNTConnected);
getInstance().broadcastVersion();
getInstance().broadcastConnectedStatus();
m_timeSync.reportNtConnected();
} else if (isConnEvent) {
logger.warn("Got connection event with no connection info??");
} else {
logger.warn("Got a non-sensical connection message that is neither connect nor disconnect?");
}
}
@@ -168,9 +205,16 @@ public class NetworkTablesManager {
} else {
setClientMode(config.ntServerAddress);
}
m_timeSync.setConfig(config);
broadcastVersion();
}
public long getOffset() {
return m_timeSync.getOffset();
}
private void setClientMode(String ntServerAddress) {
ntInstance.stopServer();
ntInstance.startClient4("photonvision");
@@ -211,4 +255,8 @@ public class NetworkTablesManager {
"[NetworkTablesManager] Could not connect to the robot! Will retry in the background...");
}
}
public long getTimeSinceLastPong() {
return m_timeSync.getTimeSinceLastPong();
}
}

169
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/TimeSyncManager.java Normal file
View File

@@ -0,0 +1,169 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.cscore.CameraServerJNI;
import edu.wpi.first.networktables.IntegerPublisher;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import org.photonvision.common.configuration.NetworkConfig;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.TimedTaskManager;
import org.photonvision.jni.PhotonTargetingJniLoader;
import org.photonvision.jni.TimeSyncClient;
import org.photonvision.jni.TimeSyncServer;
public class TimeSyncManager {
private static final Logger logger = new Logger(TimeSyncManager.class, LogGroup.NetworkTables);
private TimeSyncClient m_client = null;
private TimeSyncServer m_server = null;
private NetworkTableInstance ntInstance;
IntegerPublisher m_offsetPub;
IntegerPublisher m_rtt2Pub;
IntegerPublisher m_pingsPub;
IntegerPublisher m_pongsPub;
IntegerPublisher m_lastPongTimePub;
public TimeSyncManager(NetworkTable kRootTable) {
if (!PhotonTargetingJniLoader.isWorking) {
logger.error("PhotonTargetingJNI was not loaded! Cannot do time-sync");
}
this.ntInstance = kRootTable.getInstance();
// Need this subtable to be unique per coprocessor. TODO: consider using MAC address or
// something similar for metrics?
var timeTable = kRootTable.getSubTable(".timesync").getSubTable(CameraServerJNI.getHostname());
m_offsetPub = timeTable.getIntegerTopic("offset_us").publish();
m_rtt2Pub = timeTable.getIntegerTopic("rtt2_us").publish();
m_pingsPub = timeTable.getIntegerTopic("ping_tx_count").publish();
m_pongsPub = timeTable.getIntegerTopic("pong_rx_count").publish();
m_lastPongTimePub = timeTable.getIntegerTopic("pong_rx_time_us").publish();
// default to being a client
logger.debug("Starting TimeSyncClient on localhost (for now)");
m_client = new TimeSyncClient("127.0.0.1", 5810, 1.0);
}
// Since we're spinning off tasks in a new thread, be careful and start it seperately
public void start() {
if (!PhotonTargetingJniLoader.isWorking) {
logger.error("PhotonTargetingJNI was not loaded! Cannot start");
}
TimedTaskManager.getInstance().addTask("TimeSyncManager::tick", this::tick, 1000);
}
public synchronized long getOffset() {
if (!PhotonTargetingJniLoader.isWorking) {
return 0;
}
// if we're a client, return the offset to server time
if (m_client != null) return m_client.getOffset();
// if we're a server, our time (nt::Now) is the same as network time
if (m_server != null) return 0;
// ????? should never hit
logger.error("Client and server and null?");
return 0;
}
synchronized void setConfig(NetworkConfig config) {
if (!PhotonTargetingJniLoader.isWorking) {
return;
}
if (m_client == null && m_server == null) {
throw new RuntimeException("Neither client nor server are null?");
}
// if not already running a server, set it up
if (config.runNTServer && m_server == null) {
// tear down anything old
if (m_client != null) {
logger.debug("Tearing down old client");
m_client.stop();
m_client = null;
}
logger.debug("Starting TimeSyncServer");
m_server = new TimeSyncServer(5810);
m_server.start();
} else
// if not already running a client, set it up
if (m_client == null) {
// tear down anything old
if (m_server != null) {
logger.debug("Tearing down old server");
m_server.stop();
m_server = null;
}
// Guess at IP -- tick will take care of changing this (may take up to 1 second)
logger.debug("Starting TimeSyncClient on localhost (for now)");
m_client = new TimeSyncClient("127.0.0.1", 5810, 1.0);
}
}
synchronized void tick() {
if (m_client != null) {
var conns = ntInstance.getConnections();
if (conns.length > 0) {
logger.debug("Changing TimeSyncClient server to " + conns[0].remote_ip);
m_client.setServer(conns[0].remote_ip);
}
if (m_client != null) {
var m = m_client.getPingMetadata();
m_offsetPub.set(m.offset);
m_rtt2Pub.set(m.rtt2);
m_pingsPub.set(m.pingsSent);
m_pongsPub.set(m.pongsReceived);
m_lastPongTimePub.set(m.lastPongTime);
}
}
}
public synchronized long getTimeSinceLastPong() {
if (m_client != null) {
return m_client.getPingMetadata().timeSinceLastPong();
} else if (m_server != null) {
return 0;
} else {
// ????
return 0;
}
}
/** Restart our timesync client if NT just connected */
public synchronized void reportNtConnected() {
if (m_client != null) {
// restart (in java code; we could just add a reset metrics function...)
logger.debug(
"NT (re)connected -- restarting Time Sync Client at " + m_client.getServer() + ":5810");
m_client.stop();
m_client = new TimeSyncClient(m_client.getServer(), 5810, 1.0);
}
}
}

1
photon-core/src/main/java/org/photonvision/common/logging/LogGroup.java
View File

@@ -25,4 +25,5 @@ public enum LogGroup {
General,
Config,
CSCore,
NetworkTables,
}

3
photon-core/src/main/java/org/photonvision/common/logging/Logger.java
View File

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

60
photon-core/src/main/java/org/photonvision/common/util/TestUtils.java
View File

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

5
photon-core/src/main/java/org/photonvision/common/util/file/JacksonUtils.java
View File

@@ -33,6 +33,7 @@ import java.io.IOException;
import java.nio.file.Path;
import java.util.HashMap;
import java.util.Map;
import org.eclipse.jetty.io.EofException;
public class JacksonUtils {
public static class UIMap extends HashMap<String, Object> {}
@@ -76,6 +77,10 @@ public class JacksonUtils {
}
public static <T> T deserialize(String s, Class<T> ref) throws IOException {
if (s.length() == 0) {
throw new EofException("Provided empty string for class " + ref.getName());
}
PolymorphicTypeValidator ptv =
BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build();
ObjectMapper objectMapper =

4
photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
View File

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

14
photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java
View File

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

12
photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java
View File

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

2
photon-lib/build.gradle
View File

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

1
photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py
View File

@@ -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[?];"

10
photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py
View File

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

5
photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py
View File

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

1
photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py
View File

@@ -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[?];"

1
photon-lib/py/photonlibpy/generated/PnpResultSerde.py
View File

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

1
photon-lib/py/photonlibpy/generated/TargetCornerSerde.py
View File

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

42
photon-lib/src/generate/photonlib.json.in
View File

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

41
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View File

@@ -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<PhotonPipelineResult> 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.
*

9
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

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

16
photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java
View File

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

52
photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java Normal file
View File

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

3
photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp
View File

@@ -330,7 +330,8 @@ PhotonPipelineResult PhotonCameraSim::Process(
heartbeatCounter++;
return PhotonPipelineResult{
PhotonPipelineMetadata{heartbeatCounter, 0,
units::microsecond_t{latency}.to<int64_t>()},
units::microsecond_t{latency}.to<int64_t>(),
1000000},
detectableTgts, multiTagResults};
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) {

52
photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java
View File

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

45
photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

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

176
photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
View File

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

79
photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java
View File

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

19
photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View File

@@ -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<photon::EstimatedRobotPose> 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<photon::PhotonTrackedTarget>{}, 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<photon::PhotonTrackedTarget> 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;

2
photon-serde/messages.yaml
View File

@@ -7,6 +7,8 @@
type: int64
- name: publishTimestampMicros
type: int64
- name: timeSinceLastPong
type: int64
- name: Transform3d
shimmed: True

23
photon-server/src/main/java/org/photonvision/Main.java
View File

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

16
photon-targeting/build.gradle
View File

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

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

@@ -0,0 +1,86 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.jni;
import java.io.IOException;
import org.opencv.core.Core;
import edu.wpi.first.apriltag.jni.AprilTagJNI;
import edu.wpi.first.cscore.CameraServerJNI;
import edu.wpi.first.cscore.OpenCvLoader;
import edu.wpi.first.hal.JNIWrapper;
import edu.wpi.first.math.jni.ArmFeedforwardJNI;
import edu.wpi.first.math.jni.DAREJNI;
import edu.wpi.first.math.jni.EigenJNI;
import edu.wpi.first.math.jni.Ellipse2dJNI;
import edu.wpi.first.math.jni.Pose3dJNI;
import edu.wpi.first.math.jni.StateSpaceUtilJNI;
import edu.wpi.first.math.jni.TrajectoryUtilJNI;
import edu.wpi.first.net.WPINetJNI;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.util.CombinedRuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
public class WpilibLoader {
private static boolean has_loaded = false;
public static boolean loadLibraries() {
if (has_loaded) return true;
NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
CameraServerJNI.Helper.setExtractOnStaticLoad(false);
OpenCvLoader.Helper.setExtractOnStaticLoad(false);
JNIWrapper.Helper.setExtractOnStaticLoad(false);
WPINetJNI.Helper.setExtractOnStaticLoad(false);
AprilTagJNI.Helper.setExtractOnStaticLoad(false);
// wpimathjni is a bit odd, it's all in the wpimathjni shared lib, but the java side stuff has
// been split.
ArmFeedforwardJNI.Helper.setExtractOnStaticLoad(false);
DAREJNI.Helper.setExtractOnStaticLoad(false);
EigenJNI.Helper.setExtractOnStaticLoad(false);
Ellipse2dJNI.Helper.setExtractOnStaticLoad(false);
Pose3dJNI.Helper.setExtractOnStaticLoad(false);
StateSpaceUtilJNI.Helper.setExtractOnStaticLoad(false);
TrajectoryUtilJNI.Helper.setExtractOnStaticLoad(false);
try {
CombinedRuntimeLoader.loadLibraries(
WpilibLoader.class,
"wpiutiljni",
"wpilibc",
"wpimathjni",
"ntcorejni",
"wpinetjni",
"wpiHaljni",
"wpi",
"cscorejni",
"apriltagjni");
CombinedRuntimeLoader.loadLibraries(WpilibLoader.class, Core.NATIVE_LIBRARY_NAME);
has_loaded = true;
} catch (IOException e) {
e.printStackTrace();
has_loaded = false;
}
return has_loaded;
}
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,88 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.jni;
import edu.wpi.first.util.RuntimeDetector;
import edu.wpi.first.util.RuntimeLoader;
import java.io.File;
import java.io.FileOutputStream;
import java.io.IOException;
import java.nio.file.Files;
import java.util.List;
import org.photonvision.common.hardware.Platform;
public class PhotonTargetingJniLoader {
public static boolean isWorking = false;
public static boolean load() throws IOException, UnsatisfiedLinkError {
if (isWorking) return true;
isWorking = load_();
return isWorking;
}
public static boolean load_() throws IOException, UnsatisfiedLinkError {
// We always extract the shared object (we could hash each so, but that's a lot
// of work)
String arch_name = Platform.getNativeLibraryFolderName();
var clazz = PhotonTargetingJniLoader.class;
for (var libraryName : List.of("photontargeting", "photontargetingJNI")) {
if (RuntimeDetector.isAthena()) {
System.out.println("Detected rio - loading directly");
RuntimeLoader.loadLibrary(libraryName);
continue;
}
var nativeLibName = System.mapLibraryName(libraryName);
var path = "/nativelibraries/" + arch_name + "/" + nativeLibName;
var in = clazz.getResourceAsStream(path);
if (in == null) {
System.err.println("Could not get resource at path " + path);
return false;
}
// It's important that we don't mangle the names of these files on Windows at
// least
var tempfolder = Files.createTempDirectory("nativeextract");
File temp = new File(tempfolder.toAbsolutePath().toString(), nativeLibName);
System.out.println(temp.getAbsolutePath().toString());
FileOutputStream fos = new FileOutputStream(temp);
int read = -1;
byte[] buffer = new byte[1024];
while ((read = in.read(buffer)) != -1) {
fos.write(buffer, 0, read);
}
fos.close();
in.close();
try {
System.load(temp.getAbsolutePath());
} catch (Throwable t) {
System.err.println("Unable to System.load " + temp.getName() + " : " + t.getMessage());
t.printStackTrace();
return false;
}
System.out.println("Successfully loaded shared object " + temp.getName());
}
return true;
}
}

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

@@ -0,0 +1,156 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.jni;
import edu.wpi.first.networktables.NetworkTablesJNI;
/**
* Send ping-pongs to estimate server time, relative to nt::Now. The underlying implementation does
* technically allow us to provide a different source, but all photon code assumes nt::Now is used
*/
public class TimeSyncClient {
public static class PingMetadata {
// offset, us
public long offset;
// outgoing count
public long pingsSent;
// incoming count
public long pongsReceived;
// when we last heard back from the server
public long lastPongTime;
// RTT2, time from ping send to pong recieve at the client
public long rtt2;
public PingMetadata(
long offset, long pingsSent, long pongsReceived, long lastPongTime, long rtt2) {
this.offset = offset;
this.pingsSent = pingsSent;
this.pongsReceived = pongsReceived;
this.lastPongTime = lastPongTime;
this.rtt2 = rtt2;
}
@Override
public String toString() {
return "PingMetadata [offset="
+ offset
+ ", pingsSent="
+ pingsSent
+ ", pongsReceived="
+ pongsReceived
+ ", lastPongTime="
+ lastPongTime
+ ", rtt2="
+ rtt2
+ "]";
}
/**
* How long, in us, since we last heard back from the server
*
* @return Time between last pong RX and now, or Long.MAX_VALUE if we have heard zero pongs
*/
public long timeSinceLastPong() {
// If no pongs, it's been forever
if (pongsReceived < 1) {
return Long.MAX_VALUE;
}
return NetworkTablesJNI.now() - lastPongTime;
}
}
private final Object mutex = new Object();
private long handle;
private String server;
private int port;
private double interval;
public TimeSyncClient(String server, int port, double interval) {
this.server = server;
this.port = port;
this.interval = interval;
synchronized (mutex) {
this.handle = TimeSyncClient.create(server, port, interval);
TimeSyncClient.start(handle);
}
}
public void setServer(String newServer) {
if (!server.equals(newServer)) {
synchronized (mutex) {
stop();
this.handle = TimeSyncClient.create(newServer, port, interval);
TimeSyncClient.start(handle);
this.server = newServer;
}
}
}
public void stop() {
synchronized (mutex) {
if (handle > 0) {
TimeSyncClient.stop(handle);
handle = 0;
}
}
}
/**
* This offset, when added to the current value of nt::now(), yields the timestamp in the timebase
* of the TSP Server
*
* @return
*/
public long getOffset() {
synchronized (mutex) {
return TimeSyncClient.getOffset(handle);
}
}
/**
* Best estimate of the current timestamp at the TSP server
*
* @return The current time estimate, in microseconds, at the TSP server
*/
public long currentServerTimestamp() {
return NetworkTablesJNI.now() + getOffset();
}
public PingMetadata getPingMetadata() {
synchronized (mutex) {
return TimeSyncClient.getLatestMetadata(handle);
}
}
public String getServer() {
return server;
}
private static native long create(String serverIP, int serverPort, double pingIntervalSeconds);
private static native void start(long handle);
private static native void stop(long handle);
private static native long getOffset(long handle);
private static native PingMetadata getLatestMetadata(long handle);
}

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

@@ -0,0 +1,43 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.jni;
public class TimeSyncServer {
private long handle;
public TimeSyncServer(int port) {
this.handle = TimeSyncServer.create(port);
}
public void start() {
TimeSyncServer.start(handle);
}
public void stop() {
if (handle > 0) {
TimeSyncServer.stop(handle);
handle = 0;
}
}
private static native long create(int port);
private static native void start(long handle);
private static native void stop(long handle);
}

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

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

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

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

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

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

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

@@ -0,0 +1,200 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include "net/TimeSyncClient.h"
#include <fmt/core.h>
#include <wpinet/UDPClient.h>
#include <wpinet/uv/util.h>
#include <atomic>
#include <chrono>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <iostream>
#include <mutex>
#include <thread>
#include <Eigen/Core>
#include <wpi/Logger.h>
#include <wpi/struct/Struct.h>
#include "ntcore_cpp.h"
static void ClientLoggerFunc(unsigned int level, const char* file,
unsigned int line, const char* msg) {
if (level == 20) {
fmt::print(stderr, "TimeSyncClient: {}\n", msg);
return;
}
std::string_view levelmsg;
if (level >= 50) {
levelmsg = "CRITICAL";
} else if (level >= 40) {
levelmsg = "ERROR";
} else if (level >= 30) {
levelmsg = "WARNING";
} else {
return;
}
fmt::print(stderr, "TimeSyncClient: {}: {} ({}:{})\n", levelmsg, msg, file,
line);
}
void wpi::tsp::TimeSyncClient::Tick() {
// fmt::println("wpi::tsp::TimeSyncClient::Tick");
// Regardless of if we've gotten a pong back yet, we'll ping again. this is
// pretty naive but should be "fine" for now?
uint64_t ping_local_time{m_timeProvider()};
TspPing ping{.version = 1, .message_id = 1, .client_time = ping_local_time};
wpi::SmallVector<uint8_t, wpi::Struct<TspPing>::GetSize()> pingData(
wpi::Struct<TspPing>::GetSize());
wpi::PackStruct(pingData, ping);
// Wrap our buffer - pingData should free itself
wpi::uv::Buffer pingBuf{pingData};
int sent = m_udp->TrySend(wpi::SmallVector<wpi::uv::Buffer, 1>{pingBuf});
if (static_cast<size_t>(sent) != wpi::Struct<TspPing>::GetSize()) {
WPI_ERROR(m_logger, "Didn't send the whole ping out? sent {} bytes", sent);
return;
}
{
std::lock_guard lock{m_offsetMutex};
m_metadata.pingsSent++;
}
m_lastPing = ping;
}
void wpi::tsp::TimeSyncClient::UdpCallback(uv::Buffer& buf, size_t nbytes,
const sockaddr& sender,
unsigned flags) {
uint64_t pong_local_time = m_timeProvider();
if (static_cast<size_t>(nbytes) != wpi::Struct<TspPong>::GetSize()) {
WPI_ERROR(m_logger, "Got {} bytes for pong?", nbytes);
return;
}
TspPong pong{
wpi::UnpackStruct<TspPong>(buf.bytes()),
};
// fmt::println("->[client] Got pong: {} {} {} {}", pong.version,
// pong.message_id, pong.client_time, pong.server_time);
if (pong.version != 1) {
fmt::println("Bad version from server?");
return;
}
if (pong.message_id != 2) {
fmt::println("Bad message id from server?");
return;
}
TspPing ping = m_lastPing;
if (pong.client_time != ping.client_time) {
WPI_WARNING(m_logger,
"Pong was not a reply to our ping? Got ping {} vs pong {}",
ping.client_time, pong.client_time);
return;
}
// when time = send_time+rtt2/2, server time = server time
// server time = local time + offset
// offset = (server time - local time) = (server time) - (send_time +
// rtt2/2)
auto rtt2 = pong_local_time - ping.client_time;
int64_t serverTimeOffsetUs = pong.server_time - rtt2 / 2 - ping.client_time;
auto filtered = m_lastOffsets.Calculate(serverTimeOffsetUs);
// fmt::println("Ping-ponged! RTT2 {} uS, offset {}/filtered offset {} uS",
// rtt2,
// serverTimeOffsetUs, filtered);
{
std::lock_guard lock{m_offsetMutex};
m_metadata.offset = filtered;
m_metadata.rtt2 = rtt2;
m_metadata.pongsReceived++;
m_metadata.lastPongTime = pong_local_time;
}
using std::cout;
// fmt::println("Ping-ponged! RTT2 {} uS, offset {} uS", rtt2,
// serverTimeOffsetUs);
// fmt::println("Estimated server time {} s",
// (m_timeProvider() + serverTimeOffsetUs) / 1000000.0);
}
wpi::tsp::TimeSyncClient::TimeSyncClient(std::string_view server,
int remote_port,
std::chrono::milliseconds ping_delay,
std::function<uint64_t()> timeProvider)
: m_logger(::ClientLoggerFunc),
m_timeProvider(timeProvider),
m_udp{wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)},
m_pingTimer{wpi::uv::Timer::Create(m_loopRunner.GetLoop())},
m_serverIP{server},
m_serverPort{remote_port},
m_loopDelay(ping_delay) {
struct sockaddr_in serverAddr;
uv::NameToAddr(m_serverIP, m_serverPort, &serverAddr);
m_loopRunner.ExecSync(
[this, serverAddr](uv::Loop&) { m_udp->Connect(serverAddr); });
// fmt::println("Starting client (with server address {}:{})", server,
// remote_port);
}
void wpi::tsp::TimeSyncClient::Start() {
// fmt::println("Connecting received");
m_loopRunner.ExecSync([this](uv::Loop&) {
m_udp->received.connect(&wpi::tsp::TimeSyncClient::UdpCallback, this);
m_udp->StartRecv();
});
// fmt::println("Starting pinger");
using namespace std::chrono_literals;
m_pingTimer->timeout.connect(&wpi::tsp::TimeSyncClient::Tick, this);
m_loopRunner.ExecSync(
[this](uv::Loop&) { m_pingTimer->Start(m_loopDelay, m_loopDelay); });
}
void wpi::tsp::TimeSyncClient::Stop() { m_loopRunner.Stop(); }
int64_t wpi::tsp::TimeSyncClient::GetOffset() {
std::lock_guard lock{m_offsetMutex};
return m_metadata.offset;
}
wpi::tsp::TimeSyncClient::Metadata wpi::tsp::TimeSyncClient::GetMetadata() {
std::lock_guard lock{m_offsetMutex};
return m_metadata;
}

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

@@ -0,0 +1,116 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include "net/TimeSyncServer.h"
#include <fmt/core.h>
#include <wpinet/UDPClient.h>
#include <wpinet/uv/util.h>
#include <atomic>
#include <chrono>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <iostream>
#include <mutex>
#include <thread>
#include <wpi/Logger.h>
#include <wpi/struct/Struct.h>
#include "ntcore_cpp.h"
static void ServerLoggerFunc(unsigned int level, const char* file,
unsigned int line, const char* msg) {
if (level == 20) {
fmt::print(stderr, "TimeSyncServer: {}\n", msg);
return;
}
std::string_view levelmsg;
if (level >= 50) {
levelmsg = "CRITICAL";
} else if (level >= 40) {
levelmsg = "ERROR";
} else if (level >= 30) {
levelmsg = "WARNING";
} else {
return;
}
fmt::print(stderr, "TimeSyncServer: {}: {} ({}:{})\n", levelmsg, msg, file,
line);
}
void wpi::tsp::TimeSyncServer::UdpCallback(uv::Buffer& data, size_t n,
const sockaddr& sender,
unsigned flags) {
// fmt::println("TimeSyncServer got ping!");
TspPing ping{wpi::UnpackStruct<TspPing>(data.bytes())};
if (ping.version != 1) {
WPI_ERROR(m_logger, "Bad version from client?");
return;
}
if (ping.message_id != 1) {
WPI_ERROR(m_logger, "Bad message id from client?");
return;
}
uint64_t current_time = m_timeProvider();
TspPong pong{ping, current_time};
pong.message_id = 2;
wpi::SmallVector<uint8_t, wpi::Struct<TspPong>::GetSize()> pongData(
wpi::Struct<TspPong>::GetSize());
wpi::PackStruct(pongData, pong);
// Wrap our buffer - pongData should free itself for free
wpi::uv::Buffer pongBuf{pongData};
int sent =
m_udp->TrySend(sender, wpi::SmallVector<wpi::uv::Buffer, 1>{pongBuf});
// fmt::println("Pong ret: {}", sent);
if (static_cast<size_t>(sent) != wpi::Struct<TspPong>::GetSize()) {
WPI_ERROR(m_logger, "Didn't send the whole pong back?");
return;
}
// WPI_INFO(m_logger, "Got ping: {} {} {}", ping.version, ping.message_id,
// ping.client_time);
// WPI_INFO(m_logger, "Sent pong: {} {} {} {}", pong.version, pong.message_id,
// pong.client_time, pong.server_time);
}
wpi::tsp::TimeSyncServer::TimeSyncServer(int port,
std::function<uint64_t()> timeProvider)
: m_logger{::ServerLoggerFunc},
m_timeProvider{timeProvider},
m_udp{wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)} {
m_loopRunner.ExecSync(
[this, port](uv::Loop&) { m_udp->Bind("0.0.0.0", port); });
}
void wpi::tsp::TimeSyncServer::Start() {
m_loopRunner.ExecSync([this](uv::Loop&) {
m_udp->received.connect(&wpi::tsp::TimeSyncServer::UdpCallback, this);
m_udp->StartRecv();
});
}
void wpi::tsp::TimeSyncServer::Stop() { m_loopRunner.Stop(); }

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

@@ -0,0 +1,103 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#pragma once
#include <fmt/core.h>
#include <wpinet/EventLoopRunner.h>
#include <wpinet/UDPClient.h>
#include <wpinet/uv/Buffer.h>
#include <wpinet/uv/Timer.h>
#include <wpinet/uv/Udp.h>
#include <atomic>
#include <chrono>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <functional>
#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <frc/filter/MedianFilter.h>
#include <wpi/Logger.h>
#include <wpi/static_circular_buffer.h>
#include <wpi/struct/Struct.h>
#include "TimeSyncStructs.h"
#include "ntcore_cpp.h"
namespace wpi {
namespace tsp {
class TimeSyncClient {
public:
struct Metadata {
int64_t offset{0};
int64_t rtt2{0};
size_t pingsSent{0};
size_t pongsReceived{0};
uint64_t lastPongTime{0};
};
private:
using SharedUdpPtr = std::shared_ptr<uv::Udp>;
using SharedTimerPtr = std::shared_ptr<uv::Timer>;
EventLoopRunner m_loopRunner{};
wpi::Logger m_logger;
std::function<uint64_t()> m_timeProvider;
SharedUdpPtr m_udp;
SharedTimerPtr m_pingTimer;
std::string m_serverIP;
int m_serverPort;
std::chrono::milliseconds m_loopDelay;
std::mutex m_offsetMutex;
Metadata m_metadata;
// We only allow the most recent ping to stay alive, so only keep track of it
TspPing m_lastPing;
// 30s is a reasonable guess
frc::MedianFilter<int64_t> m_lastOffsets{30};
void Tick();
void UdpCallback(uv::Buffer& buf, size_t nbytes, const sockaddr& sender,
unsigned flags);
public:
TimeSyncClient(std::string_view server, int remote_port,
std::chrono::milliseconds ping_delay,
std::function<uint64_t()> timeProvider = nt::Now);
void Start();
void Stop();
int64_t GetOffset();
Metadata GetMetadata();
};
} // namespace tsp
} // namespace wpi

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

@@ -0,0 +1,78 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#pragma once
#include <fmt/core.h>
#include <wpinet/EventLoopRunner.h>
#include <wpinet/UDPClient.h>
#include <wpinet/uv/Buffer.h>
#include <wpinet/uv/Timer.h>
#include <wpinet/uv/Udp.h>
#include <atomic>
#include <chrono>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <functional>
#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <wpi/Logger.h>
#include <wpi/struct/Struct.h>
#include "TimeSyncStructs.h"
#include "ntcore_cpp.h"
namespace wpi {
namespace tsp {
class TimeSyncServer {
using SharedUdpPtr = std::shared_ptr<uv::Udp>;
EventLoopRunner m_loopRunner{};
wpi::Logger m_logger;
std::function<uint64_t()> m_timeProvider;
SharedUdpPtr m_udp;
std::thread m_listener;
private:
void UdpCallback(uv::Buffer& buf, size_t nbytes, const sockaddr& sender,
unsigned flags);
public:
explicit TimeSyncServer(int port = 5810,
std::function<uint64_t()> timeProvider = nt::Now);
/**
* Start listening for pings
*/
void Start();
/**
* Stop our loop runner. After stopping, we cannot restart.
*/
void Stop();
};
} // namespace tsp
} // namespace wpi

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

@@ -0,0 +1,91 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#include <wpi/struct/Struct.h>
namespace wpi {
namespace tsp {
struct TspPing {
uint8_t version;
uint8_t message_id;
uint64_t client_time;
};
struct TspPong : public TspPing {
TspPong(TspPing ping, uint64_t servertime)
: TspPing{ping}, server_time{servertime} {}
uint64_t server_time;
};
} // namespace tsp
} // namespace wpi
template <>
struct wpi::Struct<wpi::tsp::TspPing> {
static constexpr std::string_view GetTypeName() { return "TspPing"; }
static constexpr size_t GetSize() { return 10; }
static constexpr std::string_view GetSchema() {
return "uint8 version;uint8 message_id;uint64 client_time";
}
static wpi::tsp::TspPing Unpack(std::span<const uint8_t> data) {
return wpi::tsp::TspPing{
wpi::UnpackStruct<uint8_t, 0>(data),
wpi::UnpackStruct<uint8_t, 1>(data),
wpi::UnpackStruct<uint64_t, 2>(data),
};
}
static void Pack(std::span<uint8_t> data, const wpi::tsp::TspPing& value) {
wpi::PackStruct<0>(data, value.version);
wpi::PackStruct<1>(data, value.message_id);
wpi::PackStruct<2>(data, value.client_time);
}
};
template <>
struct wpi::Struct<wpi::tsp::TspPong> {
static constexpr std::string_view GetTypeName() { return "TspPong"; }
static constexpr size_t GetSize() { return 18; }
static constexpr std::string_view GetSchema() {
return "uint8 version;uint8 message_id;uint64 client_time;uint64_t "
"server_time";
}
static wpi::tsp::TspPong Unpack(std::span<const uint8_t> data) {
return wpi::tsp::TspPong{
wpi::tsp::TspPing{
wpi::UnpackStruct<uint8_t, 0>(data),
wpi::UnpackStruct<uint8_t, 1>(data),
wpi::UnpackStruct<uint64_t, 2>(data),
},
wpi::UnpackStruct<uint64_t, 10>(data),
};
}
static void Pack(std::span<uint8_t> data, const wpi::tsp::TspPong& value) {
wpi::PackStruct<0>(data, value.version);
wpi::PackStruct<1>(data, value.message_id);
wpi::PackStruct<2>(data, value.client_time);
wpi::PackStruct<10>(data, value.server_time);
}
};
static_assert(wpi::StructSerializable<wpi::tsp::TspPong>);
static_assert(wpi::StructSerializable<wpi::tsp::TspPing>);

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

@@ -0,0 +1,168 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include <org_photonvision_jni_TimeSyncClient.h>
#include <cstdio>
#include <string>
#include "net/TimeSyncClient.h"
using namespace wpi::tsp;
/**
* Finds a class and keeps it as a global reference.
*
* Use with caution, as the destructor does NOT call DeleteGlobalRef due to
* potential shutdown issues with doing so.
*/
class JClass {
public:
JClass() = default;
JClass(JNIEnv* env, const char* name) {
jclass local = env->FindClass(name);
if (!local) {
return;
}
m_cls = static_cast<jclass>(env->NewGlobalRef(local));
env->DeleteLocalRef(local);
}
void free(JNIEnv* env) {
if (m_cls) {
env->DeleteGlobalRef(m_cls);
}
m_cls = nullptr;
}
explicit operator bool() const { return m_cls; }
operator jclass() const { return m_cls; }
protected:
jclass m_cls = nullptr;
};
static JClass metadataClass;
static jmethodID metadataCtor;
// TODO - only one onload allowed
JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
JNIEnv* env;
if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
return JNI_ERR;
}
metadataClass =
JClass(env, "org/photonvision/jni/TimeSyncClient$PingMetadata");
if (!metadataClass) {
std::printf("Couldn't find class!");
return JNI_ERR;
}
metadataCtor = env->GetMethodID(metadataClass, "<init>", "(JJJJJ)V");
if (!metadataCtor) {
std::printf("Couldn't find constructor!");
return JNI_ERR;
}
return JNI_VERSION_1_6;
}
extern "C" {
/*
* Class: org_photonvision_jni_TimeSyncClient
* Method: create
* Signature: (Ljava/lang/String;ID)J
*/
JNIEXPORT jlong JNICALL
Java_org_photonvision_jni_TimeSyncClient_create
(JNIEnv* env, jclass, jstring name, jint port, jdouble interval)
{
using namespace std::chrono_literals;
const char* c_name{env->GetStringUTFChars(name, 0)};
std::string cpp_name{c_name};
jlong ret{reinterpret_cast<jlong>(
new TimeSyncClient(cpp_name, static_cast<int>(port),
std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::duration<double>(interval))))};
env->ReleaseStringUTFChars(name, c_name);
return ret;
}
/*
* Class: org_photonvision_jni_TimeSyncClient
* Method: start
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_photonvision_jni_TimeSyncClient_start
(JNIEnv*, jclass, jlong ptr)
{
TimeSyncClient* client = reinterpret_cast<TimeSyncClient*>(ptr);
client->Start();
}
/*
* Class: org_photonvision_jni_TimeSyncClient
* Method: stop
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_photonvision_jni_TimeSyncClient_stop
(JNIEnv*, jclass, jlong ptr)
{
TimeSyncClient* client = reinterpret_cast<TimeSyncClient*>(ptr);
client->Stop();
delete client;
}
/*
* Class: org_photonvision_jni_TimeSyncClient
* Method: getOffset
* Signature: (J)J
*/
JNIEXPORT jlong JNICALL
Java_org_photonvision_jni_TimeSyncClient_getOffset
(JNIEnv*, jclass, jlong ptr)
{
TimeSyncClient* client = reinterpret_cast<TimeSyncClient*>(ptr);
return client->GetOffset();
}
/*
* Class: org_photonvision_jni_TimeSyncClient
* Method: getLatestMetadata
* Signature: (J)Ljava/lang/Object;
*/
JNIEXPORT jobject JNICALL
Java_org_photonvision_jni_TimeSyncClient_getLatestMetadata
(JNIEnv* env, jclass, jlong ptr)
{
TimeSyncClient* client = reinterpret_cast<TimeSyncClient*>(ptr);
auto m{client->GetMetadata()};
auto ret = env->NewObject(metadataClass, metadataCtor, m.offset, m.pingsSent,
m.pongsReceived, m.lastPongTime, m.rtt2);
return ret;
}
} // extern "C"

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

@@ -0,0 +1,68 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include <org_photonvision_jni_TimeSyncClient.h>
#include <org_photonvision_jni_TimeSyncServer.h>
#include <cstdio>
#include "net/TimeSyncServer.h"
using namespace wpi::tsp;
extern "C" {
/*
* Class: org_photonvision_jni_TimeSyncServer
* Method: create
* Signature: (I)J
*/
JNIEXPORT jlong JNICALL
Java_org_photonvision_jni_TimeSyncServer_create
(JNIEnv*, jclass, jint port)
{
return reinterpret_cast<jlong>(new TimeSyncServer(port));
}
/*
* Class: org_photonvision_jni_TimeSyncServer
* Method: start
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_photonvision_jni_TimeSyncServer_start
(JNIEnv*, jclass, jlong ptr)
{
TimeSyncServer* server = reinterpret_cast<TimeSyncServer*>(ptr);
server->Start();
}
/*
* Class: org_photonvision_jni_TimeSyncServer
* Method: stop
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_photonvision_jni_TimeSyncServer_stop
(JNIEnv*, jclass, jlong ptr)
{
TimeSyncServer* server = reinterpret_cast<TimeSyncServer*>(ptr);
server->Stop();
delete server;
}
} // extern "C"

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

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

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

@@ -0,0 +1,69 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package net;
import static org.junit.jupiter.api.Assertions.fail;
import edu.wpi.first.hal.HAL;
import java.io.IOException;
import org.junit.jupiter.api.AfterAll;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.photonvision.jni.PhotonTargetingJniLoader;
import org.photonvision.jni.TimeSyncClient;
import org.photonvision.jni.TimeSyncServer;
import org.photonvision.jni.WpilibLoader;
public class TimeSyncTest {
@BeforeAll
public static void load_wpilib() throws UnsatisfiedLinkError, IOException {
WpilibLoader.loadLibraries();
if (!PhotonTargetingJniLoader.load()) {
fail();
}
}
@AfterAll
public static void teardown() {
HAL.shutdown();
}
@Test
public void smoketest() throws InterruptedException {
HAL.initialize(1000, 0);
// NetworkTableInstance.getDefault().stopClient();
// NetworkTableInstance.getDefault().startServer();
var server = new TimeSyncServer(5810);
System.err.println("Waiting: PID=" + ProcessHandle.current().pid());
server.start();
var client = new TimeSyncClient("127.0.0.1", 5810, 0.5);
for (int i = 0; i < 5; i++) {
Thread.sleep(100);
System.out.println(client.getPingMetadata());
}
server.stop();
client.stop();
}
}

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,42 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include <gtest/gtest.h>
#include <hal/HAL.h>
#include <net/TimeSyncClient.h>
#include <net/TimeSyncServer.h>
TEST(TimeSyncProtocolTest, Smoketest) {
using namespace wpi::tsp;
using namespace std::chrono_literals;
HAL_Initialize(500, 0);
TimeSyncServer server{5812};
TimeSyncClient client{"127.0.0.1", 5812, 100ms};
server.Start();
client.Start();
for (int i = 0; i < 10; i++) {
std::this_thread::sleep_for(100ms);
TimeSyncClient::Metadata m = client.GetMetadata();
fmt::println("Offset={} rtt={}", m.offset, m.rtt2);
}
server.Stop();
}

2
photonlib-java-examples/aimattarget/build.gradle
View File

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

44
shared/javacpp/publish.gradle
View File

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