Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41: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

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