mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-24 01:31:44 +00:00
Compare commits
11 Commits
v2025.0.1
...
v2025.0.0-
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
14f7155a23 | ||
|
|
d188c37466 | ||
|
|
14fcc5d485 | ||
|
|
1d8d934a8a | ||
|
|
bdb2949b4b | ||
|
|
4cf1c7eee4 | ||
|
|
04ec99f17a | ||
|
|
150561abf2 | ||
|
|
58a0597c86 | ||
|
|
a842581785 | ||
|
|
8dcf0b31a2 |
1
.github/workflows/build.yml
vendored
1
.github/workflows/build.yml
vendored
@@ -165,6 +165,7 @@ jobs:
|
||||
- run: |
|
||||
chmod +x gradlew
|
||||
./gradlew photon-targeting:build photon-lib:build -i
|
||||
name: Build with Gradle
|
||||
- run: ./gradlew photon-lib:publish photon-targeting:publish
|
||||
name: Publish
|
||||
env:
|
||||
|
||||
1
.github/workflows/lint-format.yml
vendored
1
.github/workflows/lint-format.yml
vendored
@@ -64,6 +64,7 @@ jobs:
|
||||
- run: |
|
||||
chmod +x gradlew
|
||||
./gradlew spotlessCheck
|
||||
name: Run spotless
|
||||
|
||||
client-lint-format:
|
||||
name: "PhotonClient Lint and Formatting"
|
||||
|
||||
@@ -4,6 +4,7 @@ plugins {
|
||||
|
||||
import java.nio.file.Path
|
||||
|
||||
ext.licenseFile = file("$rootDir/LICENSE")
|
||||
apply from: "${rootDir}/shared/common.gradle"
|
||||
|
||||
wpilibTools.deps.wpilibVersion = wpi.versions.wpilibVersion.get()
|
||||
|
||||
@@ -0,0 +1,77 @@
|
||||
/*
|
||||
* 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.logging;
|
||||
|
||||
import edu.wpi.first.util.RuntimeDetector;
|
||||
import java.io.IOException;
|
||||
import java.nio.file.Files;
|
||||
import java.nio.file.Path;
|
||||
import org.photonvision.common.util.TimedTaskManager;
|
||||
import org.photonvision.jni.QueuedFileLogger;
|
||||
|
||||
/**
|
||||
* Listens for and reproduces Linux kernel logs, from /var/log/kern.log, into the Photon logger
|
||||
* ecosystem
|
||||
*/
|
||||
public class KernelLogLogger {
|
||||
private static KernelLogLogger INSTANCE;
|
||||
|
||||
public static KernelLogLogger getInstance() {
|
||||
if (INSTANCE == null) {
|
||||
INSTANCE = new KernelLogLogger();
|
||||
}
|
||||
return INSTANCE;
|
||||
}
|
||||
|
||||
QueuedFileLogger listener = null;
|
||||
Logger logger = new Logger(KernelLogLogger.class, LogGroup.General);
|
||||
|
||||
public KernelLogLogger() {
|
||||
if (RuntimeDetector.isLinux()) {
|
||||
logger.info("Listening for klogs on /var/log/dmesg ! Boot logs:");
|
||||
|
||||
try {
|
||||
var bootlog = Files.readAllLines(Path.of("/var/log/dmesg"));
|
||||
for (var line : bootlog) {
|
||||
logger.log(line, LogLevel.DEBUG);
|
||||
}
|
||||
} catch (IOException e) {
|
||||
logger.error("Couldn't read /var/log/dmesg - not printing boot logs");
|
||||
}
|
||||
|
||||
listener = new QueuedFileLogger("/var/log/kern.log");
|
||||
} else {
|
||||
System.out.println("NOT for klogs");
|
||||
}
|
||||
|
||||
// arbitrary frequency to grab logs. The underlying native buffer will grow unbounded without
|
||||
// this, lol
|
||||
TimedTaskManager.getInstance().addTask("outputPrintk", this::outputNewPrintks, 1000);
|
||||
}
|
||||
|
||||
public void outputNewPrintks() {
|
||||
if (listener == null) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (var msg : listener.getNewlines()) {
|
||||
// We currently set all logs to debug regardless of their actual level
|
||||
logger.log(msg, LogLevel.DEBUG);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -26,4 +26,5 @@ public enum LogGroup {
|
||||
Config,
|
||||
CSCore,
|
||||
NetworkTables,
|
||||
System,
|
||||
}
|
||||
|
||||
@@ -30,8 +30,34 @@ import org.photonvision.common.dataflow.DataChangeService;
|
||||
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
|
||||
import org.photonvision.common.util.TimedTaskManager;
|
||||
|
||||
@SuppressWarnings("unused")
|
||||
/** TODO: get rid of static {} blocks and refactor to singleton pattern */
|
||||
public class Logger {
|
||||
private static final HashMap<LogGroup, LogLevel> levelMap = new HashMap<>();
|
||||
private static final List<LogAppender> currentAppenders = new ArrayList<>();
|
||||
|
||||
private static final UILogAppender uiLogAppender = new UILogAppender();
|
||||
|
||||
// // TODO why's the logger care about this? split it out
|
||||
// private static KernelLogLogger klogListener = null;
|
||||
|
||||
static {
|
||||
levelMap.put(LogGroup.Camera, LogLevel.INFO);
|
||||
levelMap.put(LogGroup.General, LogLevel.INFO);
|
||||
levelMap.put(LogGroup.WebServer, LogLevel.INFO);
|
||||
levelMap.put(LogGroup.Data, LogLevel.INFO);
|
||||
levelMap.put(LogGroup.VisionModule, LogLevel.INFO);
|
||||
levelMap.put(LogGroup.Config, LogLevel.INFO);
|
||||
levelMap.put(LogGroup.CSCore, LogLevel.TRACE);
|
||||
levelMap.put(LogGroup.NetworkTables, LogLevel.DEBUG);
|
||||
levelMap.put(LogGroup.System, LogLevel.DEBUG);
|
||||
|
||||
currentAppenders.add(new ConsoleLogAppender());
|
||||
currentAppenders.add(uiLogAppender);
|
||||
addFileAppender(PathManager.getInstance().getLogPath());
|
||||
|
||||
cleanLogs(PathManager.getInstance().getLogsDir());
|
||||
}
|
||||
|
||||
public static final String ANSI_RESET = "\u001B[0m";
|
||||
public static final String ANSI_BLACK = "\u001B[30m";
|
||||
public static final String ANSI_RED = "\u001B[31m";
|
||||
@@ -50,8 +76,6 @@ public class Logger {
|
||||
private static final List<Pair<String, LogLevel>> uiBacklog = new ArrayList<>();
|
||||
private static boolean connected = false;
|
||||
|
||||
private static final UILogAppender uiLogAppender = new UILogAppender();
|
||||
|
||||
private final String className;
|
||||
private final LogGroup group;
|
||||
|
||||
@@ -89,27 +113,6 @@ public class Logger {
|
||||
return builder.toString();
|
||||
}
|
||||
|
||||
private static final HashMap<LogGroup, LogLevel> levelMap = new HashMap<>();
|
||||
private static final List<LogAppender> currentAppenders = new ArrayList<>();
|
||||
|
||||
static {
|
||||
levelMap.put(LogGroup.Camera, LogLevel.INFO);
|
||||
levelMap.put(LogGroup.General, LogLevel.INFO);
|
||||
levelMap.put(LogGroup.WebServer, LogLevel.INFO);
|
||||
levelMap.put(LogGroup.Data, LogLevel.INFO);
|
||||
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 {
|
||||
currentAppenders.add(new ConsoleLogAppender());
|
||||
currentAppenders.add(uiLogAppender);
|
||||
addFileAppender(PathManager.getInstance().getLogPath());
|
||||
cleanLogs(PathManager.getInstance().getLogsDir());
|
||||
}
|
||||
|
||||
@SuppressWarnings("ResultOfMethodCallIgnored")
|
||||
public static void addFileAppender(Path logFilePath) {
|
||||
var file = logFilePath.toFile();
|
||||
|
||||
@@ -96,9 +96,12 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
|
||||
var autoExpProp = findProperty("exposure_auto", "auto_exposure");
|
||||
|
||||
exposureAbsProp = expProp.get();
|
||||
autoExposureProp = autoExpProp.get();
|
||||
this.minExposure = exposureAbsProp.getMin();
|
||||
this.maxExposure = exposureAbsProp.getMax();
|
||||
|
||||
if (autoExpProp.isPresent()) {
|
||||
autoExposureProp = autoExpProp.get();
|
||||
}
|
||||
}
|
||||
|
||||
public void setAllCamDefaults() {
|
||||
@@ -169,7 +172,7 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
|
||||
softSet("auto_exposure_bias", 0);
|
||||
softSet("iso_sensitivity_auto", 0); // Disable auto ISO adjustment
|
||||
softSet("iso_sensitivity", 0); // Manual ISO adjustment
|
||||
autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
|
||||
if (autoExposureProp != null) autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
|
||||
|
||||
// Most cameras leave exposure time absolute at the last value from their AE
|
||||
// algorithm.
|
||||
@@ -199,7 +202,7 @@ public class GenericUSBCameraSettables extends VisionSourceSettables {
|
||||
public void setExposureRaw(double exposureRaw) {
|
||||
if (exposureRaw >= 0.0) {
|
||||
try {
|
||||
autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
|
||||
if (autoExposureProp != null) autoExposureProp.set(PROP_AUTO_EXPOSURE_DISABLED);
|
||||
|
||||
int propVal = (int) MathUtil.clamp(exposureRaw, minExposure, maxExposure);
|
||||
|
||||
|
||||
@@ -9,6 +9,7 @@ ext {
|
||||
includePhotonTargeting = true
|
||||
// Include the generated Version file
|
||||
generatedHeaders = "src/generate/native/include"
|
||||
licenseFile = file("LICENSE")
|
||||
}
|
||||
|
||||
apply plugin: 'cpp'
|
||||
|
||||
@@ -21,14 +21,25 @@
|
||||
###############################################################################
|
||||
|
||||
from ..targeting import *
|
||||
from ..packet import Packet
|
||||
|
||||
|
||||
class MultiTargetPNPResultSerde:
|
||||
|
||||
# Message definition md5sum. See photon_packet.adoc for details
|
||||
MESSAGE_VERSION = "541096947e9f3ca2d3f425ff7b04aa7b"
|
||||
MESSAGE_FORMAT = "PnpResult:ae4d655c0a3104d88df4f5db144c1e86 estimatedPose;int16 fiducialIDsUsed[?];"
|
||||
|
||||
@staticmethod
|
||||
def pack(value: "MultiTargetPNPResult") -> "Packet":
|
||||
ret = Packet()
|
||||
|
||||
# estimatedPose is of non-intrinsic type PnpResult
|
||||
ret.encodeBytes(PnpResult.photonStruct.pack(value.estimatedPose).getData())
|
||||
|
||||
# fiducialIDsUsed is a custom VLA!
|
||||
ret.encodeShortList(value.fiducialIDsUsed)
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def unpack(packet: "Packet") -> "MultiTargetPNPResult":
|
||||
ret = MultiTargetPNPResult()
|
||||
|
||||
@@ -21,14 +21,31 @@
|
||||
###############################################################################
|
||||
|
||||
from ..targeting import *
|
||||
from ..packet import Packet
|
||||
|
||||
|
||||
class PhotonPipelineMetadataSerde:
|
||||
|
||||
# Message definition md5sum. See photon_packet.adoc for details
|
||||
MESSAGE_VERSION = "ac0a45f686457856fb30af77699ea356"
|
||||
MESSAGE_FORMAT = "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;int64 timeSinceLastPong;"
|
||||
|
||||
@staticmethod
|
||||
def pack(value: "PhotonPipelineMetadata") -> "Packet":
|
||||
ret = Packet()
|
||||
|
||||
# sequenceID is of intrinsic type int64
|
||||
ret.encodeLong(value.sequenceID)
|
||||
|
||||
# captureTimestampMicros is of intrinsic type int64
|
||||
ret.encodeLong(value.captureTimestampMicros)
|
||||
|
||||
# publishTimestampMicros is of intrinsic type int64
|
||||
ret.encodeLong(value.publishTimestampMicros)
|
||||
|
||||
# timeSinceLastPong is of intrinsic type int64
|
||||
ret.encodeLong(value.timeSinceLastPong)
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def unpack(packet: "Packet") -> "PhotonPipelineMetadata":
|
||||
ret = PhotonPipelineMetadata()
|
||||
|
||||
@@ -21,14 +21,30 @@
|
||||
###############################################################################
|
||||
|
||||
from ..targeting import *
|
||||
from ..packet import Packet
|
||||
|
||||
|
||||
class PhotonPipelineResultSerde:
|
||||
|
||||
# Message definition md5sum. See photon_packet.adoc for details
|
||||
MESSAGE_VERSION = "4b2ff16a964b5e2bf04be0c1454d91c4"
|
||||
MESSAGE_FORMAT = "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"
|
||||
|
||||
@staticmethod
|
||||
def pack(value: "PhotonPipelineResult") -> "Packet":
|
||||
ret = Packet()
|
||||
|
||||
# metadata is of non-intrinsic type PhotonPipelineMetadata
|
||||
ret.encodeBytes(
|
||||
PhotonPipelineMetadata.photonStruct.pack(value.metadata).getData()
|
||||
)
|
||||
|
||||
# targets is a custom VLA!
|
||||
ret.encodeList(value.targets, PhotonTrackedTarget.photonStruct)
|
||||
|
||||
# multitagResult is optional! it better not be a VLA too
|
||||
ret.encodeOptional(value.multitagResult, MultiTargetPNPResult.photonStruct)
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def unpack(packet: "Packet") -> "PhotonPipelineResult":
|
||||
ret = PhotonPipelineResult()
|
||||
|
||||
@@ -21,14 +21,53 @@
|
||||
###############################################################################
|
||||
|
||||
from ..targeting import *
|
||||
from ..packet import Packet
|
||||
|
||||
|
||||
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[?];"
|
||||
|
||||
@staticmethod
|
||||
def pack(value: "PhotonTrackedTarget") -> "Packet":
|
||||
ret = Packet()
|
||||
|
||||
# yaw is of intrinsic type float64
|
||||
ret.encodeDouble(value.yaw)
|
||||
|
||||
# pitch is of intrinsic type float64
|
||||
ret.encodeDouble(value.pitch)
|
||||
|
||||
# area is of intrinsic type float64
|
||||
ret.encodeDouble(value.area)
|
||||
|
||||
# skew is of intrinsic type float64
|
||||
ret.encodeDouble(value.skew)
|
||||
|
||||
# fiducialId is of intrinsic type int32
|
||||
ret.encodeInt(value.fiducialId)
|
||||
|
||||
# objDetectId is of intrinsic type int32
|
||||
ret.encodeInt(value.objDetectId)
|
||||
|
||||
# objDetectConf is of intrinsic type float32
|
||||
ret.encodeFloat(value.objDetectConf)
|
||||
|
||||
ret.encodeTransform(value.bestCameraToTarget)
|
||||
|
||||
ret.encodeTransform(value.altCameraToTarget)
|
||||
|
||||
# poseAmbiguity is of intrinsic type float64
|
||||
ret.encodeDouble(value.poseAmbiguity)
|
||||
|
||||
# minAreaRectCorners is a custom VLA!
|
||||
ret.encodeList(value.minAreaRectCorners, TargetCorner.photonStruct)
|
||||
|
||||
# detectedCorners is a custom VLA!
|
||||
ret.encodeList(value.detectedCorners, TargetCorner.photonStruct)
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def unpack(packet: "Packet") -> "PhotonTrackedTarget":
|
||||
ret = PhotonTrackedTarget()
|
||||
|
||||
@@ -21,14 +21,32 @@
|
||||
###############################################################################
|
||||
|
||||
from ..targeting import *
|
||||
from ..packet import Packet
|
||||
|
||||
|
||||
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;"
|
||||
|
||||
@staticmethod
|
||||
def pack(value: "PnpResult") -> "Packet":
|
||||
ret = Packet()
|
||||
|
||||
ret.encodeTransform(value.best)
|
||||
|
||||
ret.encodeTransform(value.alt)
|
||||
|
||||
# bestReprojErr is of intrinsic type float64
|
||||
ret.encodeDouble(value.bestReprojErr)
|
||||
|
||||
# altReprojErr is of intrinsic type float64
|
||||
ret.encodeDouble(value.altReprojErr)
|
||||
|
||||
# ambiguity is of intrinsic type float64
|
||||
ret.encodeDouble(value.ambiguity)
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def unpack(packet: "Packet") -> "PnpResult":
|
||||
ret = PnpResult()
|
||||
|
||||
@@ -21,14 +21,25 @@
|
||||
###############################################################################
|
||||
|
||||
from ..targeting import *
|
||||
from ..packet import Packet
|
||||
|
||||
|
||||
class TargetCornerSerde:
|
||||
|
||||
# Message definition md5sum. See photon_packet.adoc for details
|
||||
MESSAGE_VERSION = "16f6ac0dedc8eaccb951f4895d9e18b6"
|
||||
MESSAGE_FORMAT = "float64 x;float64 y;"
|
||||
|
||||
@staticmethod
|
||||
def pack(value: "TargetCorner") -> "Packet":
|
||||
ret = Packet()
|
||||
|
||||
# x is of intrinsic type float64
|
||||
ret.encodeDouble(value.x)
|
||||
|
||||
# y is of intrinsic type float64
|
||||
ret.encodeDouble(value.y)
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def unpack(packet: "Packet") -> "TargetCorner":
|
||||
ret = TargetCorner()
|
||||
|
||||
@@ -22,7 +22,7 @@ import wpilib
|
||||
|
||||
|
||||
class Packet:
|
||||
def __init__(self, data: bytes):
|
||||
def __init__(self, data: bytes = b""):
|
||||
"""
|
||||
* Constructs an empty packet.
|
||||
*
|
||||
@@ -198,3 +198,110 @@ class Packet:
|
||||
return serde.unpack(self)
|
||||
else:
|
||||
return None
|
||||
|
||||
def _encodeGeneric(self, packFormat, value):
|
||||
"""
|
||||
Append bytes to the packet data buffer.
|
||||
"""
|
||||
self.packetData = self.packetData + struct.pack(packFormat, value)
|
||||
self.size = len(self.packetData)
|
||||
|
||||
def encode8(self, value: int):
|
||||
"""
|
||||
Encodes a single byte and appends it to the packet.
|
||||
"""
|
||||
self._encodeGeneric("<b", value)
|
||||
|
||||
def encode16(self, value: int):
|
||||
"""
|
||||
Encodes a short (2 bytes) and appends it to the packet.
|
||||
"""
|
||||
self._encodeGeneric("<h", value)
|
||||
|
||||
def encodeInt(self, value: int):
|
||||
"""
|
||||
Encodes an int (4 bytes) and appends it to the packet.
|
||||
"""
|
||||
self._encodeGeneric("<l", value)
|
||||
|
||||
def encodeFloat(self, value: float):
|
||||
"""
|
||||
Encodes a float (4 bytes) and appends it to the packet.
|
||||
"""
|
||||
self._encodeGeneric("<f", value)
|
||||
|
||||
def encodeLong(self, value: int):
|
||||
"""
|
||||
Encodes a long (8 bytes) and appends it to the packet.
|
||||
"""
|
||||
self._encodeGeneric("<q", value)
|
||||
|
||||
def encodeDouble(self, value: float):
|
||||
"""
|
||||
Encodes a double (8 bytes) and appends it to the packet.
|
||||
"""
|
||||
self._encodeGeneric("<d", value)
|
||||
|
||||
def encodeBoolean(self, value: bool):
|
||||
"""
|
||||
Encodes a boolean as a single byte and appends it to the packet.
|
||||
"""
|
||||
self.encode8(1 if value else 0)
|
||||
|
||||
def encodeDoubleArray(self, values: list[float]):
|
||||
"""
|
||||
Encodes an array of doubles and appends it to the packet.
|
||||
"""
|
||||
self.encode8(len(values))
|
||||
for value in values:
|
||||
self.encodeDouble(value)
|
||||
|
||||
def encodeShortList(self, values: list[int]):
|
||||
"""
|
||||
Encodes a list of shorts, with length prefixed as a single byte.
|
||||
"""
|
||||
self.encode8(len(values))
|
||||
for value in values:
|
||||
self.encode16(value)
|
||||
|
||||
def encodeTransform(self, transform: Transform3d):
|
||||
"""
|
||||
Encodes a Transform3d (translation and rotation) and appends it to the packet.
|
||||
"""
|
||||
# Encode Translation3d part (x, y, z)
|
||||
self.encodeDouble(transform.translation().x)
|
||||
self.encodeDouble(transform.translation().y)
|
||||
self.encodeDouble(transform.translation().z)
|
||||
|
||||
# Encode Rotation3d as Quaternion (w, x, y, z)
|
||||
quaternion = transform.rotation().getQuaternion()
|
||||
self.encodeDouble(quaternion.W())
|
||||
self.encodeDouble(quaternion.X())
|
||||
self.encodeDouble(quaternion.Y())
|
||||
self.encodeDouble(quaternion.Z())
|
||||
|
||||
def encodeList(self, values: list[Any], serde: Type):
|
||||
"""
|
||||
Encodes a list of items using a specific serializer and appends it to the packet.
|
||||
"""
|
||||
self.encode8(len(values))
|
||||
for item in values:
|
||||
packed = serde.pack(item)
|
||||
self.packetData = self.packetData + packed.getData()
|
||||
self.size = len(self.packetData)
|
||||
|
||||
def encodeOptional(self, value: Optional[Any], serde: Type):
|
||||
"""
|
||||
Encodes an optional value using a specific serializer.
|
||||
"""
|
||||
if value is None:
|
||||
self.encodeBoolean(False)
|
||||
else:
|
||||
self.encodeBoolean(True)
|
||||
packed = serde.pack(value)
|
||||
self.packetData = self.packetData + packed.getData()
|
||||
self.size = len(self.packetData)
|
||||
|
||||
def encodeBytes(self, value: bytes):
|
||||
self.packetData = self.packetData + value
|
||||
self.size = len(self.packetData)
|
||||
|
||||
@@ -269,8 +269,8 @@ class PhotonPoseEstimator:
|
||||
def _multiTagOnCoprocStrategy(
|
||||
self, result: PhotonPipelineResult
|
||||
) -> Optional[EstimatedRobotPose]:
|
||||
if result.multiTagResult.estimatedPose.isPresent:
|
||||
best_tf = result.multiTagResult.estimatedPose.best
|
||||
if result.multitagResult is not None:
|
||||
best_tf = result.multitagResult.estimatedPose.best
|
||||
best = (
|
||||
Pose3d()
|
||||
.transformBy(best_tf) # field-to-camera
|
||||
|
||||
@@ -8,8 +8,8 @@ class PnpResult:
|
||||
best: Transform3d = field(default_factory=Transform3d)
|
||||
alt: Transform3d = field(default_factory=Transform3d)
|
||||
ambiguity: float = 0.0
|
||||
bestReprojError: float = 0.0
|
||||
altReprojError: float = 0.0
|
||||
bestReprojErr: float = 0.0
|
||||
altReprojErr: float = 0.0
|
||||
|
||||
photonStruct: "PNPResultSerde" = None
|
||||
|
||||
|
||||
@@ -15,6 +15,8 @@ class PhotonPipelineMetadata:
|
||||
# Mirror of the heartbeat entry -- monotonically increasing
|
||||
sequenceID: int = -1
|
||||
|
||||
timeSinceLastPong: int = -1
|
||||
|
||||
photonStruct: "PhotonPipelineMetadataSerde" = None
|
||||
|
||||
|
||||
@@ -27,7 +29,7 @@ class PhotonPipelineResult:
|
||||
# Python users beware! We don't currently run a Time Sync Server, so these timestamps are in
|
||||
# an arbitrary timebase. This is not true in C++ or Java.
|
||||
metadata: PhotonPipelineMetadata = field(default_factory=PhotonPipelineMetadata)
|
||||
multiTagResult: Optional[MultiTargetPNPResult] = None
|
||||
multitagResult: Optional[MultiTargetPNPResult] = None
|
||||
|
||||
def getLatencyMillis(self) -> float:
|
||||
return (
|
||||
|
||||
@@ -13,9 +13,11 @@ class PhotonTrackedTarget:
|
||||
fiducialId: int = -1
|
||||
bestCameraToTarget: Transform3d = field(default_factory=Transform3d)
|
||||
altCameraToTarget: Transform3d = field(default_factory=Transform3d)
|
||||
minAreaRectCorners: list[TargetCorner] | None = None
|
||||
detectedCorners: list[TargetCorner] | None = None
|
||||
minAreaRectCorners: list[TargetCorner] = field(default_factory=list[TargetCorner])
|
||||
detectedCorners: list[TargetCorner] = field(default_factory=list[TargetCorner])
|
||||
poseAmbiguity: float = 0.0
|
||||
objDetectId: int = -1
|
||||
objDetectConf: float = 0.0
|
||||
|
||||
def getYaw(self) -> float:
|
||||
return self.yaw
|
||||
@@ -35,10 +37,10 @@ class PhotonTrackedTarget:
|
||||
def getPoseAmbiguity(self) -> float:
|
||||
return self.poseAmbiguity
|
||||
|
||||
def getMinAreaRectCorners(self) -> list[TargetCorner] | None:
|
||||
def getMinAreaRectCorners(self) -> list[TargetCorner]:
|
||||
return self.minAreaRectCorners
|
||||
|
||||
def getDetectedCorners(self) -> list[TargetCorner] | None:
|
||||
def getDetectedCorners(self) -> list[TargetCorner]:
|
||||
return self.detectedCorners
|
||||
|
||||
def getBestCameraToTarget(self) -> Transform3d:
|
||||
|
||||
@@ -15,247 +15,260 @@
|
||||
## along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
###############################################################################
|
||||
|
||||
# from photonlibpy import MultiTargetPNPResult, PnpResult
|
||||
# from photonlibpy import PhotonPipelineResult
|
||||
# from photonlibpy import PhotonPoseEstimator, PoseStrategy
|
||||
# from photonlibpy import PhotonTrackedTarget, TargetCorner, PhotonPipelineMetadata
|
||||
# from robotpy_apriltag import AprilTag, AprilTagFieldLayout
|
||||
# from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
|
||||
from photonlibpy.targeting.multiTargetPNPResult import MultiTargetPNPResult, PnpResult
|
||||
from photonlibpy.targeting.photonPipelineResult import PhotonPipelineResult
|
||||
from photonlibpy import PhotonPoseEstimator, PoseStrategy
|
||||
from photonlibpy.targeting import (
|
||||
PhotonTrackedTarget,
|
||||
TargetCorner,
|
||||
PhotonPipelineMetadata,
|
||||
)
|
||||
from robotpy_apriltag import AprilTag, AprilTagFieldLayout
|
||||
from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
|
||||
|
||||
|
||||
# class PhotonCameraInjector:
|
||||
# result: PhotonPipelineResult
|
||||
class PhotonCameraInjector:
|
||||
result: PhotonPipelineResult
|
||||
|
||||
# def getLatestResult(self) -> PhotonPipelineResult:
|
||||
# return self.result
|
||||
def getLatestResult(self) -> PhotonPipelineResult:
|
||||
return self.result
|
||||
|
||||
|
||||
# def setupCommon() -> AprilTagFieldLayout:
|
||||
# tagList = []
|
||||
# tagPoses = (
|
||||
# Pose3d(3, 3, 3, Rotation3d()),
|
||||
# Pose3d(5, 5, 5, Rotation3d()),
|
||||
# )
|
||||
# for id_, pose in enumerate(tagPoses):
|
||||
# aprilTag = AprilTag()
|
||||
# aprilTag.ID = id_
|
||||
# aprilTag.pose = pose
|
||||
# tagList.append(aprilTag)
|
||||
def setupCommon() -> AprilTagFieldLayout:
|
||||
tagList = []
|
||||
tagPoses = (
|
||||
Pose3d(3, 3, 3, Rotation3d()),
|
||||
Pose3d(5, 5, 5, Rotation3d()),
|
||||
)
|
||||
for id_, pose in enumerate(tagPoses):
|
||||
aprilTag = AprilTag()
|
||||
aprilTag.ID = id_
|
||||
aprilTag.pose = pose
|
||||
tagList.append(aprilTag)
|
||||
|
||||
# fieldLength = 54 / 3.281 # 54 ft -> meters
|
||||
# fieldWidth = 27 / 3.281 # 24 ft -> meters
|
||||
fieldLength = 54 / 3.281 # 54 ft -> meters
|
||||
fieldWidth = 27 / 3.281 # 24 ft -> meters
|
||||
|
||||
# return AprilTagFieldLayout(tagList, fieldLength, fieldWidth)
|
||||
return AprilTagFieldLayout(tagList, fieldLength, fieldWidth)
|
||||
|
||||
|
||||
# def test_lowestAmbiguityStrategy():
|
||||
# aprilTags = setupCommon()
|
||||
def test_lowestAmbiguityStrategy():
|
||||
aprilTags = setupCommon()
|
||||
|
||||
# cameraOne = PhotonCameraInjector()
|
||||
# cameraOne.result = PhotonPipelineResult(
|
||||
# 11 * 1e6,
|
||||
# [
|
||||
# PhotonTrackedTarget(
|
||||
# 3.0,
|
||||
# -4.0,
|
||||
# 9.0,
|
||||
# 4.0,
|
||||
# 0,
|
||||
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
# [
|
||||
# TargetCorner(1, 2),
|
||||
# TargetCorner(3, 4),
|
||||
# TargetCorner(5, 6),
|
||||
# TargetCorner(7, 8),
|
||||
# ],
|
||||
# [
|
||||
# TargetCorner(1, 2),
|
||||
# TargetCorner(3, 4),
|
||||
# TargetCorner(5, 6),
|
||||
# TargetCorner(7, 8),
|
||||
# ],
|
||||
# 0.7,
|
||||
# ),
|
||||
# PhotonTrackedTarget(
|
||||
# 3.0,
|
||||
# -4.0,
|
||||
# 9.1,
|
||||
# 6.7,
|
||||
# 1,
|
||||
# Transform3d(Translation3d(4, 2, 3), Rotation3d(0, 0, 0)),
|
||||
# Transform3d(Translation3d(4, 2, 3), Rotation3d(1, 5, 3)),
|
||||
# [
|
||||
# TargetCorner(1, 2),
|
||||
# TargetCorner(3, 4),
|
||||
# TargetCorner(5, 6),
|
||||
# TargetCorner(7, 8),
|
||||
# ],
|
||||
# [
|
||||
# TargetCorner(1, 2),
|
||||
# TargetCorner(3, 4),
|
||||
# TargetCorner(5, 6),
|
||||
# TargetCorner(7, 8),
|
||||
# ],
|
||||
# 0.3,
|
||||
# ),
|
||||
# PhotonTrackedTarget(
|
||||
# 9.0,
|
||||
# -2.0,
|
||||
# 19.0,
|
||||
# 3.0,
|
||||
# 0,
|
||||
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
# [
|
||||
# TargetCorner(1, 2),
|
||||
# TargetCorner(3, 4),
|
||||
# TargetCorner(5, 6),
|
||||
# TargetCorner(7, 8),
|
||||
# ],
|
||||
# [
|
||||
# TargetCorner(1, 2),
|
||||
# TargetCorner(3, 4),
|
||||
# TargetCorner(5, 6),
|
||||
# TargetCorner(7, 8),
|
||||
# ],
|
||||
# 0.4,
|
||||
# ),
|
||||
# ],
|
||||
# None,
|
||||
# metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0),
|
||||
# )
|
||||
cameraOne = PhotonCameraInjector()
|
||||
cameraOne.result = PhotonPipelineResult(
|
||||
int(11 * 1e6),
|
||||
[
|
||||
PhotonTrackedTarget(
|
||||
3.0,
|
||||
-4.0,
|
||||
9.0,
|
||||
4.0,
|
||||
0,
|
||||
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
[
|
||||
TargetCorner(1, 2),
|
||||
TargetCorner(3, 4),
|
||||
TargetCorner(5, 6),
|
||||
TargetCorner(7, 8),
|
||||
],
|
||||
[
|
||||
TargetCorner(1, 2),
|
||||
TargetCorner(3, 4),
|
||||
TargetCorner(5, 6),
|
||||
TargetCorner(7, 8),
|
||||
],
|
||||
0.7,
|
||||
),
|
||||
PhotonTrackedTarget(
|
||||
3.0,
|
||||
-4.0,
|
||||
9.1,
|
||||
6.7,
|
||||
1,
|
||||
Transform3d(Translation3d(4, 2, 3), Rotation3d(0, 0, 0)),
|
||||
Transform3d(Translation3d(4, 2, 3), Rotation3d(1, 5, 3)),
|
||||
[
|
||||
TargetCorner(1, 2),
|
||||
TargetCorner(3, 4),
|
||||
TargetCorner(5, 6),
|
||||
TargetCorner(7, 8),
|
||||
],
|
||||
[
|
||||
TargetCorner(1, 2),
|
||||
TargetCorner(3, 4),
|
||||
TargetCorner(5, 6),
|
||||
TargetCorner(7, 8),
|
||||
],
|
||||
0.3,
|
||||
),
|
||||
PhotonTrackedTarget(
|
||||
9.0,
|
||||
-2.0,
|
||||
19.0,
|
||||
3.0,
|
||||
0,
|
||||
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
[
|
||||
TargetCorner(1, 2),
|
||||
TargetCorner(3, 4),
|
||||
TargetCorner(5, 6),
|
||||
TargetCorner(7, 8),
|
||||
],
|
||||
[
|
||||
TargetCorner(1, 2),
|
||||
TargetCorner(3, 4),
|
||||
TargetCorner(5, 6),
|
||||
TargetCorner(7, 8),
|
||||
],
|
||||
0.4,
|
||||
),
|
||||
],
|
||||
metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0),
|
||||
multitagResult=None,
|
||||
)
|
||||
|
||||
# estimator = PhotonPoseEstimator(
|
||||
# aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
|
||||
# )
|
||||
estimator = PhotonPoseEstimator(
|
||||
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
|
||||
)
|
||||
|
||||
# estimatedPose = estimator.update()
|
||||
# pose = estimatedPose.estimatedPose
|
||||
estimatedPose = estimator.update()
|
||||
|
||||
# assertEquals(11 - 0.002, estimatedPose.timestampSeconds, 1e-3)
|
||||
# assertEquals(1, pose.x, 0.01)
|
||||
# assertEquals(3, pose.y, 0.01)
|
||||
# assertEquals(2, pose.z, 0.01)
|
||||
assert estimatedPose is not None
|
||||
|
||||
pose = estimatedPose.estimatedPose
|
||||
|
||||
assertEquals(11 - 0.002, estimatedPose.timestampSeconds, 1e-3)
|
||||
assertEquals(1, pose.x, 0.01)
|
||||
assertEquals(3, pose.y, 0.01)
|
||||
assertEquals(2, pose.z, 0.01)
|
||||
|
||||
|
||||
# def test_multiTagOnCoprocStrategy():
|
||||
# cameraOne = PhotonCameraInjector()
|
||||
# cameraOne.result = PhotonPipelineResult(
|
||||
# 11 * 1e6,
|
||||
# # There needs to be at least one target present for pose estimation to work
|
||||
# # Doesn't matter which/how many targets for this test
|
||||
# [
|
||||
# PhotonTrackedTarget(
|
||||
# 3.0,
|
||||
# -4.0,
|
||||
# 9.0,
|
||||
# 4.0,
|
||||
# 0,
|
||||
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
# [
|
||||
# TargetCorner(1, 2),
|
||||
# TargetCorner(3, 4),
|
||||
# TargetCorner(5, 6),
|
||||
# TargetCorner(7, 8),
|
||||
# ],
|
||||
# [
|
||||
# TargetCorner(1, 2),
|
||||
# TargetCorner(3, 4),
|
||||
# TargetCorner(5, 6),
|
||||
# TargetCorner(7, 8),
|
||||
# ],
|
||||
# 0.7,
|
||||
# )
|
||||
# ],
|
||||
# multiTagResult=MultiTargetPNPResult(
|
||||
# PnpResult(True, Transform3d(1, 3, 2, Rotation3d()))
|
||||
# ),
|
||||
# metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0),
|
||||
# )
|
||||
def test_multiTagOnCoprocStrategy():
|
||||
cameraOne = PhotonCameraInjector()
|
||||
cameraOne.result = PhotonPipelineResult(
|
||||
int(11 * 1e6),
|
||||
# There needs to be at least one target present for pose estimation to work
|
||||
# Doesn't matter which/how many targets for this test
|
||||
[
|
||||
PhotonTrackedTarget(
|
||||
3.0,
|
||||
-4.0,
|
||||
9.0,
|
||||
4.0,
|
||||
0,
|
||||
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
[
|
||||
TargetCorner(1, 2),
|
||||
TargetCorner(3, 4),
|
||||
TargetCorner(5, 6),
|
||||
TargetCorner(7, 8),
|
||||
],
|
||||
[
|
||||
TargetCorner(1, 2),
|
||||
TargetCorner(3, 4),
|
||||
TargetCorner(5, 6),
|
||||
TargetCorner(7, 8),
|
||||
],
|
||||
0.7,
|
||||
)
|
||||
],
|
||||
metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0),
|
||||
multitagResult=MultiTargetPNPResult(
|
||||
PnpResult(Transform3d(1, 3, 2, Rotation3d()))
|
||||
),
|
||||
)
|
||||
|
||||
# estimator = PhotonPoseEstimator(
|
||||
# AprilTagFieldLayout(),
|
||||
# PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
|
||||
# cameraOne,
|
||||
# Transform3d(),
|
||||
# )
|
||||
estimator = PhotonPoseEstimator(
|
||||
AprilTagFieldLayout(),
|
||||
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
|
||||
cameraOne,
|
||||
Transform3d(),
|
||||
)
|
||||
|
||||
# estimatedPose = estimator.update()
|
||||
# pose = estimatedPose.estimatedPose
|
||||
estimatedPose = estimator.update()
|
||||
|
||||
# assertEquals(11 - 2e-3, estimatedPose.timestampSeconds, 1e-3)
|
||||
# assertEquals(1, pose.x, 0.01)
|
||||
# assertEquals(3, pose.y, 0.01)
|
||||
# assertEquals(2, pose.z, 0.01)
|
||||
assert estimatedPose is not None
|
||||
|
||||
pose = estimatedPose.estimatedPose
|
||||
|
||||
assertEquals(11 - 2e-3, estimatedPose.timestampSeconds, 1e-3)
|
||||
assertEquals(1, pose.x, 0.01)
|
||||
assertEquals(3, pose.y, 0.01)
|
||||
assertEquals(2, pose.z, 0.01)
|
||||
|
||||
|
||||
# def test_cacheIsInvalidated():
|
||||
# aprilTags = setupCommon()
|
||||
def test_cacheIsInvalidated():
|
||||
aprilTags = setupCommon()
|
||||
|
||||
# cameraOne = PhotonCameraInjector()
|
||||
# result = PhotonPipelineResult(
|
||||
# 20 * 1e6,
|
||||
# [
|
||||
# PhotonTrackedTarget(
|
||||
# 3.0,
|
||||
# -4.0,
|
||||
# 9.0,
|
||||
# 4.0,
|
||||
# 0,
|
||||
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
# Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
# [
|
||||
# TargetCorner(1, 2),
|
||||
# TargetCorner(3, 4),
|
||||
# TargetCorner(5, 6),
|
||||
# TargetCorner(7, 8),
|
||||
# ],
|
||||
# [
|
||||
# TargetCorner(1, 2),
|
||||
# TargetCorner(3, 4),
|
||||
# TargetCorner(5, 6),
|
||||
# TargetCorner(7, 8),
|
||||
# ],
|
||||
# 0.7,
|
||||
# )
|
||||
# ],
|
||||
# metadata=PhotonPipelineMetadata(0, 2 * 1e3, 0),
|
||||
# )
|
||||
cameraOne = PhotonCameraInjector()
|
||||
result = PhotonPipelineResult(
|
||||
int(20 * 1e6),
|
||||
[
|
||||
PhotonTrackedTarget(
|
||||
3.0,
|
||||
-4.0,
|
||||
9.0,
|
||||
4.0,
|
||||
0,
|
||||
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
Transform3d(Translation3d(1, 2, 3), Rotation3d(1, 2, 3)),
|
||||
[
|
||||
TargetCorner(1, 2),
|
||||
TargetCorner(3, 4),
|
||||
TargetCorner(5, 6),
|
||||
TargetCorner(7, 8),
|
||||
],
|
||||
[
|
||||
TargetCorner(1, 2),
|
||||
TargetCorner(3, 4),
|
||||
TargetCorner(5, 6),
|
||||
TargetCorner(7, 8),
|
||||
],
|
||||
0.7,
|
||||
)
|
||||
],
|
||||
metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0),
|
||||
)
|
||||
|
||||
# estimator = PhotonPoseEstimator(
|
||||
# aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
|
||||
# )
|
||||
estimator = PhotonPoseEstimator(
|
||||
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
|
||||
)
|
||||
|
||||
# # Empty result, expect empty result
|
||||
# cameraOne.result = PhotonPipelineResult(0)
|
||||
# estimatedPose = estimator.update()
|
||||
# assert estimatedPose is None
|
||||
# Empty result, expect empty result
|
||||
cameraOne.result = PhotonPipelineResult(0)
|
||||
estimatedPose = estimator.update()
|
||||
assert estimatedPose is None
|
||||
|
||||
# # Set actual result
|
||||
# cameraOne.result = result
|
||||
# estimatedPose = estimator.update()
|
||||
# assert estimatedPose is not None
|
||||
# assertEquals(20, estimatedPose.timestampSeconds, 0.01)
|
||||
# assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
|
||||
# Set actual result
|
||||
cameraOne.result = result
|
||||
estimatedPose = estimator.update()
|
||||
assert estimatedPose is not None
|
||||
assertEquals(20, estimatedPose.timestampSeconds, 0.01)
|
||||
assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
|
||||
|
||||
# # And again -- pose cache should mean this is empty
|
||||
# cameraOne.result = result
|
||||
# estimatedPose = estimator.update()
|
||||
# assert estimatedPose is None
|
||||
# # Expect the old timestamp to still be here
|
||||
# assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
|
||||
# And again -- pose cache should mean this is empty
|
||||
cameraOne.result = result
|
||||
estimatedPose = estimator.update()
|
||||
assert estimatedPose is None
|
||||
# Expect the old timestamp to still be here
|
||||
assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
|
||||
|
||||
# # Set new field layout -- right after, the pose cache timestamp should be -1
|
||||
# estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0)
|
||||
# assertEquals(-1, estimator._poseCacheTimestampSeconds)
|
||||
# # Update should cache the current timestamp (20) again
|
||||
# cameraOne.result = result
|
||||
# estimatedPose = estimator.update()
|
||||
# assertEquals(20, estimatedPose.timestampSeconds, 0.01)
|
||||
# assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
|
||||
# Set new field layout -- right after, the pose cache timestamp should be -1
|
||||
estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0)
|
||||
assertEquals(-1, estimator._poseCacheTimestampSeconds)
|
||||
# Update should cache the current timestamp (20) again
|
||||
cameraOne.result = result
|
||||
estimatedPose = estimator.update()
|
||||
|
||||
assert estimatedPose is not None
|
||||
|
||||
assertEquals(20, estimatedPose.timestampSeconds, 0.01)
|
||||
assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
|
||||
|
||||
|
||||
# def assertEquals(expected, actual, epsilon=0.0):
|
||||
# assert abs(expected - actual) <= epsilon
|
||||
def assertEquals(expected, actual, epsilon=0.0):
|
||||
assert abs(expected - actual) <= epsilon
|
||||
|
||||
@@ -60,10 +60,21 @@ inline constexpr std::string_view bfw =
|
||||
">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
|
||||
"\n\n";
|
||||
|
||||
// bit of a hack -- start a TimeSync server on port 5810 (hard-coded)
|
||||
static std::mutex g_timeSyncServerMutex;
|
||||
static bool g_timeSyncServerStarted;
|
||||
static wpi::tsp::TimeSyncServer timesyncServer{5810};
|
||||
// bit of a hack -- start a TimeSync server on port 5810 (hard-coded). We want
|
||||
// to avoid calling this from static initialization
|
||||
static void InitTspServer() {
|
||||
// We dont impose requirements about not calling the PhotonCamera constructor
|
||||
// from different threads, so i guess we need this?
|
||||
static std::mutex g_timeSyncServerMutex;
|
||||
static bool g_timeSyncServerStarted{false};
|
||||
static wpi::tsp::TimeSyncServer timesyncServer{5810};
|
||||
|
||||
std::lock_guard lock{g_timeSyncServerMutex};
|
||||
if (!g_timeSyncServerStarted) {
|
||||
timesyncServer.Start();
|
||||
g_timeSyncServerStarted = true;
|
||||
}
|
||||
}
|
||||
|
||||
namespace photon {
|
||||
|
||||
@@ -117,13 +128,10 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
|
||||
HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount);
|
||||
InstanceCount++;
|
||||
|
||||
{
|
||||
std::lock_guard lock{g_timeSyncServerMutex};
|
||||
if (!g_timeSyncServerStarted) {
|
||||
timesyncServer.Start();
|
||||
g_timeSyncServerStarted = true;
|
||||
}
|
||||
}
|
||||
// The Robot class is actually created here:
|
||||
// https://github.com/wpilibsuite/allwpilib/blob/811b1309683e930a1ce69fae818f943ff161b7a5/wpilibc/src/main/native/include/frc/RobotBase.h#L33
|
||||
// so we should be fine to call this from the ctor
|
||||
InitTspServer();
|
||||
}
|
||||
|
||||
PhotonCamera::PhotonCamera(const std::string_view cameraName)
|
||||
|
||||
@@ -46,6 +46,7 @@ class MessageType(TypedDict):
|
||||
# C++ helpers
|
||||
cpp_include: str
|
||||
# python shim types
|
||||
python_encode_shim: str
|
||||
python_decode_shim: str
|
||||
# Java import name
|
||||
java_import: str
|
||||
|
||||
@@ -5,29 +5,35 @@ bool:
|
||||
java_type: bool
|
||||
cpp_type: bool
|
||||
java_decode_method: decodeBoolean
|
||||
java_encode_shim: encodeBoolean
|
||||
int16:
|
||||
len: 2
|
||||
java_type: short
|
||||
cpp_type: int16_t
|
||||
java_decode_method: decodeShort
|
||||
java_list_decode_method: decodeShortList
|
||||
java_encode_shim: encodeShort
|
||||
int32:
|
||||
len: 4
|
||||
java_type: int
|
||||
cpp_type: int32_t
|
||||
java_decode_method: decodeInt
|
||||
java_encode_shim: encodeInt
|
||||
int64:
|
||||
len: 8
|
||||
java_type: long
|
||||
cpp_type: int64_t
|
||||
java_decode_method: decodeLong
|
||||
java_encode_shim: encodeLong
|
||||
float32:
|
||||
len: 4
|
||||
java_type: float
|
||||
cpp_type: float
|
||||
java_decode_method: decodeFloat
|
||||
java_encode_shim: encodeFloat
|
||||
float64:
|
||||
len: 8
|
||||
java_type: double
|
||||
cpp_type: double
|
||||
java_decode_method: decodeDouble
|
||||
java_encode_shim: encodeDouble
|
||||
|
||||
@@ -17,6 +17,7 @@
|
||||
cpp_type: frc::Transform3d
|
||||
cpp_include: "<frc/geometry/Transform3d.h>"
|
||||
python_decode_shim: packet.decodeTransform
|
||||
python_encode_shim: encodeTransform
|
||||
java_import: edu.wpi.first.math.geometry.Transform3d
|
||||
# shim since we expect fields to at least exist
|
||||
fields: []
|
||||
|
||||
@@ -21,6 +21,7 @@
|
||||
###############################################################################
|
||||
|
||||
from ..targeting import *
|
||||
from ..packet import Packet
|
||||
|
||||
class {{ name }}Serde:
|
||||
|
||||
@@ -28,6 +29,34 @@ class {{ name }}Serde:
|
||||
MESSAGE_VERSION = "{{ message_hash }}"
|
||||
MESSAGE_FORMAT = "{{ message_fmt }}"
|
||||
|
||||
@staticmethod
|
||||
def pack(value: '{{ name }}' ) -> 'Packet':
|
||||
ret = Packet()
|
||||
{% for field in fields -%}
|
||||
{%- if field.type | is_shimmed %}
|
||||
ret.{{ get_message_by_name(field.type).python_encode_shim}}(value.{{ field.name }})
|
||||
{%- elif field.optional == True %}
|
||||
# {{ field.name }} is optional! it better not be a VLA too
|
||||
ret.encodeOptional(value.{{ field.name }}, {{ field.type }}.photonStruct)
|
||||
{%- elif field.vla == True and not field.type | is_intrinsic %}
|
||||
# {{ field.name }} is a custom VLA!
|
||||
ret.encodeList(value.{{ field.name }}, {{ field.type }}.photonStruct)
|
||||
{%- elif field.vla == True and field.type | is_intrinsic %}
|
||||
# {{ field.name }} is a custom VLA!
|
||||
ret.encode{{ type_map[field.type].java_type.title() }}List(value.{{ field.name }})
|
||||
{%- elif field.type | is_intrinsic %}
|
||||
# {{ field.name }} is of intrinsic type {{ field.type }}
|
||||
ret.{{ type_map[field.type].java_encode_shim }}(value.{{field.name}})
|
||||
{%- else %}
|
||||
# {{ field.name }} is of non-intrinsic type {{ field.type }}
|
||||
ret.encodeBytes({{ field.type }}.photonStruct.pack(value.{{field.name}}).getData())
|
||||
{%- endif %}
|
||||
{%- if not loop.last %}
|
||||
{% endif -%}
|
||||
{% endfor%}
|
||||
return ret
|
||||
|
||||
|
||||
@staticmethod
|
||||
def unpack(packet: 'Packet') -> '{{ name }}':
|
||||
ret = {{ name }}()
|
||||
|
||||
@@ -11,6 +11,9 @@ apply from: "${rootDir}/shared/common.gradle"
|
||||
dependencies {
|
||||
implementation project(':photon-core')
|
||||
|
||||
// Zip
|
||||
implementation 'org.zeroturnaround:zt-zip:1.14'
|
||||
|
||||
// Needed for Javalin Runtime Logging
|
||||
implementation "org.slf4j:slf4j-simple:2.0.7"
|
||||
}
|
||||
@@ -22,6 +25,10 @@ application {
|
||||
mainClass = 'org.photonvision.Main'
|
||||
}
|
||||
|
||||
jar {
|
||||
from file("$rootDir/LICENSE")
|
||||
}
|
||||
|
||||
shadowJar {
|
||||
archiveBaseName = "photonvision"
|
||||
archiveVersion = project.version as String
|
||||
|
||||
@@ -33,6 +33,7 @@ import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
|
||||
import org.photonvision.common.hardware.HardwareManager;
|
||||
import org.photonvision.common.hardware.PiVersion;
|
||||
import org.photonvision.common.hardware.Platform;
|
||||
import org.photonvision.common.logging.KernelLogLogger;
|
||||
import org.photonvision.common.logging.LogGroup;
|
||||
import org.photonvision.common.logging.LogLevel;
|
||||
import org.photonvision.common.logging.Logger;
|
||||
@@ -437,6 +438,10 @@ public class Main {
|
||||
Logger.setLevel(LogGroup.General, logLevel);
|
||||
logger.info("Logging initialized in debug mode.");
|
||||
|
||||
// Add Linux kernel log->Photon logger
|
||||
KernelLogLogger.getInstance();
|
||||
|
||||
// Add CSCore->Photon logger
|
||||
PvCSCoreLogger.getInstance();
|
||||
|
||||
logger.debug("Loading ConfigManager...");
|
||||
|
||||
@@ -53,6 +53,7 @@ import org.photonvision.common.util.file.ProgramDirectoryUtilities;
|
||||
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
|
||||
import org.photonvision.vision.camera.CameraQuirk;
|
||||
import org.photonvision.vision.processes.VisionModuleManager;
|
||||
import org.zeroturnaround.zip.ZipUtil;
|
||||
|
||||
public class RequestHandler {
|
||||
// Treat all 2XX calls as "INFO"
|
||||
@@ -422,20 +423,34 @@ public class RequestHandler {
|
||||
try {
|
||||
ShellExec shell = new ShellExec();
|
||||
var tempPath = Files.createTempFile("photonvision-journalctl", ".txt");
|
||||
shell.executeBashCommand("journalctl -u photonvision.service > " + tempPath.toAbsolutePath());
|
||||
var tempPath2 = Files.createTempFile("photonvision-kernelogs", ".txt");
|
||||
shell.executeBashCommand(
|
||||
"journalctl -u photonvision.service > "
|
||||
+ tempPath.toAbsolutePath()
|
||||
+ " && journalctl -k > "
|
||||
+ tempPath2.toAbsolutePath());
|
||||
|
||||
while (!shell.isOutputCompleted()) {
|
||||
// TODO: add timeout
|
||||
}
|
||||
|
||||
if (shell.getExitCode() == 0) {
|
||||
// Wrote to the temp file! Add it to the ctx
|
||||
var stream = new FileInputStream(tempPath.toFile());
|
||||
ctx.contentType("text/plain");
|
||||
ctx.header("Content-Disposition", "attachment; filename=\"photonvision-journalctl.txt\"");
|
||||
ctx.status(200);
|
||||
// Wrote to the temp file! Zip and yeet it to the client
|
||||
|
||||
var out = Files.createTempFile("photonvision-logs", "zip").toFile();
|
||||
|
||||
try {
|
||||
ZipUtil.packEntries(new File[] {tempPath.toFile(), tempPath2.toFile()}, out);
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
var stream = new FileInputStream(out);
|
||||
ctx.contentType("application/zip");
|
||||
ctx.header("Content-Disposition", "attachment; filename=\"photonvision-logs.zip\"");
|
||||
ctx.result(stream);
|
||||
logger.info("Uploading settings with size " + stream.available());
|
||||
ctx.status(200);
|
||||
logger.info("Outputting log ZIP with size " + stream.available());
|
||||
} else {
|
||||
ctx.status(500);
|
||||
ctx.result("The journalctl service was unable to export logs");
|
||||
|
||||
@@ -8,6 +8,7 @@ apply plugin: 'edu.wpi.first.NativeUtils'
|
||||
apply plugin: 'edu.wpi.first.WpilibTools'
|
||||
apply plugin: 'edu.wpi.first.GradleJni'
|
||||
|
||||
ext.licenseFile = file("$rootDir/LICENSE")
|
||||
apply from: "${rootDir}/shared/config.gradle"
|
||||
apply from: "${rootDir}/shared/javacommon.gradle"
|
||||
|
||||
@@ -220,3 +221,6 @@ nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore")
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, wpi.versions.opencvVersion.get())
|
||||
nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag")
|
||||
|
||||
@@ -0,0 +1,60 @@
|
||||
/*
|
||||
* 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 QueuedFileLogger {
|
||||
long m_handle = 0;
|
||||
|
||||
public QueuedFileLogger(String path) {
|
||||
m_handle = QueuedFileLogger.create(path);
|
||||
}
|
||||
|
||||
public String[] getNewlines() {
|
||||
String newBuffer = null;
|
||||
|
||||
synchronized (this) {
|
||||
if (m_handle == 0) {
|
||||
System.err.println("QueuedFileLogger use after free");
|
||||
return new String[0];
|
||||
}
|
||||
|
||||
newBuffer = QueuedFileLogger.getNewLines(m_handle);
|
||||
}
|
||||
|
||||
if (newBuffer == null) {
|
||||
return new String[0];
|
||||
}
|
||||
|
||||
return newBuffer.split("\n");
|
||||
}
|
||||
|
||||
public void stop() {
|
||||
synchronized (this) {
|
||||
if (m_handle != 0) {
|
||||
QueuedFileLogger.destroy(m_handle);
|
||||
m_handle = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static native long create(String path);
|
||||
|
||||
private static native void destroy(long handle);
|
||||
|
||||
private static native String getNewLines(long handle);
|
||||
}
|
||||
@@ -156,17 +156,11 @@ wpi::tsp::TimeSyncClient::TimeSyncClient(std::string_view server,
|
||||
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_udp{},
|
||||
m_pingTimer{},
|
||||
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);
|
||||
}
|
||||
@@ -175,6 +169,13 @@ void wpi::tsp::TimeSyncClient::Start() {
|
||||
// fmt::println("Connecting received");
|
||||
|
||||
m_loopRunner.ExecSync([this](uv::Loop&) {
|
||||
struct sockaddr_in serverAddr;
|
||||
uv::NameToAddr(m_serverIP, m_serverPort, &serverAddr);
|
||||
|
||||
m_udp = {wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)};
|
||||
m_pingTimer = {wpi::uv::Timer::Create(m_loopRunner.GetLoop())};
|
||||
|
||||
m_udp->Connect(serverAddr);
|
||||
m_udp->received.connect(&wpi::tsp::TimeSyncClient::UdpCallback, this);
|
||||
m_udp->StartRecv();
|
||||
});
|
||||
|
||||
@@ -101,13 +101,13 @@ 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); });
|
||||
}
|
||||
m_udp{},
|
||||
m_port(port) {}
|
||||
|
||||
void wpi::tsp::TimeSyncServer::Start() {
|
||||
m_loopRunner.ExecSync([this](uv::Loop&) {
|
||||
m_udp = {wpi::uv::Udp::Create(m_loopRunner.GetLoop(), AF_INET)};
|
||||
m_udp->Bind("0.0.0.0", m_port);
|
||||
m_udp->received.connect(&wpi::tsp::TimeSyncServer::UdpCallback, this);
|
||||
m_udp->StartRecv();
|
||||
});
|
||||
|
||||
@@ -53,6 +53,7 @@ class TimeSyncServer {
|
||||
wpi::Logger m_logger;
|
||||
std::function<uint64_t()> m_timeProvider;
|
||||
SharedUdpPtr m_udp;
|
||||
int m_port;
|
||||
|
||||
std::thread m_listener;
|
||||
|
||||
|
||||
103
photon-targeting/src/main/native/jni/FileLoggerExtrasJNI.cpp
Normal file
103
photon-targeting/src/main/native/jni/FileLoggerExtrasJNI.cpp
Normal 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/>.
|
||||
*/
|
||||
|
||||
#include <functional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <wpi/FileLogger.h>
|
||||
|
||||
#include "jni_utils.h"
|
||||
#include "org_photonvision_jni_QueuedFileLogger.h"
|
||||
|
||||
struct QueuedFileLogger {
|
||||
// ew ew ew ew ew ew ew ew
|
||||
std::vector<char> m_data{};
|
||||
|
||||
std::mutex m_mutex;
|
||||
|
||||
wpi::FileLogger logger;
|
||||
|
||||
explicit QueuedFileLogger(std::string_view file)
|
||||
: logger{file, std::bind(&QueuedFileLogger::callback, this,
|
||||
std::placeholders::_1)} {
|
||||
// fmt::println("Watching {}", file);
|
||||
}
|
||||
|
||||
void callback(std::string_view newline) {
|
||||
std::lock_guard lock{m_mutex};
|
||||
// fmt::println("FileLogger got: {}", newline);
|
||||
m_data.insert(m_data.end(), newline.begin(), newline.end());
|
||||
}
|
||||
|
||||
std::vector<char> SwapData() {
|
||||
std::vector<char> ret;
|
||||
{
|
||||
std::lock_guard lock{m_mutex};
|
||||
ret.swap(m_data);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
};
|
||||
|
||||
extern "C" {
|
||||
|
||||
/*
|
||||
* Class: org_photonvision_jni_QueuedFileLogger
|
||||
* Method: create
|
||||
* Signature: (Ljava/lang/String;)J
|
||||
*/
|
||||
JNIEXPORT jlong JNICALL
|
||||
Java_org_photonvision_jni_QueuedFileLogger_create
|
||||
(JNIEnv* env, jclass, jstring name)
|
||||
{
|
||||
const char* c_name{env->GetStringUTFChars(name, 0)};
|
||||
std::string cpp_name{c_name};
|
||||
jlong ret{reinterpret_cast<jlong>(new QueuedFileLogger(cpp_name))};
|
||||
env->ReleaseStringUTFChars(name, c_name);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: org_photonvision_jni_QueuedFileLogger
|
||||
* Method: destroy
|
||||
* Signature: (J)V
|
||||
*/
|
||||
JNIEXPORT void JNICALL
|
||||
Java_org_photonvision_jni_QueuedFileLogger_destroy
|
||||
(JNIEnv*, jclass, jlong handle)
|
||||
{
|
||||
CHECK_PTR(handle);
|
||||
delete reinterpret_cast<QueuedFileLogger*>(handle);
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: org_photonvision_jni_QueuedFileLogger
|
||||
* Method: getNewLines
|
||||
* Signature: (J)Ljava/lang/String;
|
||||
*/
|
||||
JNIEXPORT jstring JNICALL
|
||||
Java_org_photonvision_jni_QueuedFileLogger_getNewLines
|
||||
(JNIEnv* env, jclass, jlong handle)
|
||||
{
|
||||
CHECK_PTR_RETURN(handle, nullptr);
|
||||
QueuedFileLogger* logger = reinterpret_cast<QueuedFileLogger*>(handle);
|
||||
|
||||
return env->NewStringUTF(logger->SwapData().data());
|
||||
}
|
||||
} // extern "C"
|
||||
@@ -20,21 +20,11 @@
|
||||
#include <cstdio>
|
||||
#include <string>
|
||||
|
||||
#include "jni_utils.h"
|
||||
#include "net/TimeSyncClient.h"
|
||||
|
||||
using namespace wpi::tsp;
|
||||
|
||||
#define CHECK_PTR(ptr) \
|
||||
if (!ptr) { \
|
||||
fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \
|
||||
return; \
|
||||
}
|
||||
#define CHECK_PTR_RETURN(ptr, default) \
|
||||
if (!ptr) { \
|
||||
fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \
|
||||
return default; \
|
||||
}
|
||||
|
||||
/**
|
||||
* Finds a class and keeps it as a global reference.
|
||||
*
|
||||
|
||||
@@ -20,21 +20,11 @@
|
||||
|
||||
#include <cstdio>
|
||||
|
||||
#include "jni_utils.h"
|
||||
#include "net/TimeSyncServer.h"
|
||||
|
||||
using namespace wpi::tsp;
|
||||
|
||||
#define CHECK_PTR(ptr) \
|
||||
if (!ptr) { \
|
||||
fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \
|
||||
return; \
|
||||
}
|
||||
#define CHECK_PTR_RETURN(ptr, default) \
|
||||
if (!ptr) { \
|
||||
fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \
|
||||
return default; \
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
|
||||
/*
|
||||
|
||||
29
photon-targeting/src/main/native/jni/jni_utils.h
Normal file
29
photon-targeting/src/main/native/jni/jni_utils.h
Normal file
@@ -0,0 +1,29 @@
|
||||
/*
|
||||
* 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
|
||||
|
||||
#define CHECK_PTR(ptr) \
|
||||
if (!ptr) { \
|
||||
fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \
|
||||
return; \
|
||||
}
|
||||
#define CHECK_PTR_RETURN(ptr, default) \
|
||||
if (!ptr) { \
|
||||
fmt::println("Got invalid pointer?? {}:{}", __FILE__, __LINE__); \
|
||||
return default; \
|
||||
}
|
||||
@@ -36,6 +36,8 @@ public class TimeSyncTest {
|
||||
if (!PhotonTargetingJniLoader.load()) {
|
||||
fail();
|
||||
}
|
||||
|
||||
HAL.initialize(1000, 0);
|
||||
}
|
||||
|
||||
@AfterAll
|
||||
@@ -45,8 +47,6 @@ public class TimeSyncTest {
|
||||
|
||||
@Test
|
||||
public void smoketest() throws InterruptedException {
|
||||
HAL.initialize(1000, 0);
|
||||
|
||||
// NetworkTableInstance.getDefault().stopClient();
|
||||
// NetworkTableInstance.getDefault().startServer();
|
||||
|
||||
|
||||
@@ -0,0 +1,62 @@
|
||||
/*
|
||||
* 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 wpiutil_extras;
|
||||
|
||||
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.QueuedFileLogger;
|
||||
import org.photonvision.jni.WpilibLoader;
|
||||
|
||||
public class FileLoggerTest {
|
||||
@BeforeAll
|
||||
public static void load_wpilib() throws UnsatisfiedLinkError, IOException {
|
||||
if (!WpilibLoader.loadLibraries()) {
|
||||
fail();
|
||||
}
|
||||
if (!PhotonTargetingJniLoader.load()) {
|
||||
fail();
|
||||
}
|
||||
|
||||
HAL.initialize(1000, 0);
|
||||
}
|
||||
|
||||
@AfterAll
|
||||
public static void teardown() {
|
||||
HAL.shutdown();
|
||||
}
|
||||
|
||||
@Test
|
||||
public void smoketest() throws InterruptedException {
|
||||
var logger = new QueuedFileLogger("/var/log/kern.log");
|
||||
for (int i = 0; i < 100; i++) {
|
||||
Thread.sleep(1000);
|
||||
|
||||
for (var line : logger.getNewlines()) {
|
||||
System.out.println(" ->:" + line);
|
||||
}
|
||||
}
|
||||
|
||||
logger.stop();
|
||||
}
|
||||
}
|
||||
@@ -1,336 +1,7 @@
|
||||
#!/bin/bash
|
||||
|
||||
needs_arg() {
|
||||
if [ -z "$OPTARG" ]; then
|
||||
die "Argument is required for --$OPT option" \
|
||||
"See './install.sh -h' for more information."
|
||||
fi;
|
||||
}
|
||||
|
||||
die() {
|
||||
for arg in "$@"; do
|
||||
echo "$arg" 1>&2
|
||||
done
|
||||
exit 1
|
||||
}
|
||||
|
||||
debug() {
|
||||
if [ -z "$QUIET" ] ; then
|
||||
for arg in "$@"; do
|
||||
echo "$arg"
|
||||
done
|
||||
fi
|
||||
}
|
||||
|
||||
package_is_installed(){
|
||||
dpkg-query -W -f='${Status}' "$1" 2>/dev/null | grep -q "ok installed"
|
||||
}
|
||||
|
||||
install_if_missing() {
|
||||
if package_is_installed "$1" ; then
|
||||
debug "Found existing $1. Skipping..."
|
||||
return
|
||||
fi
|
||||
|
||||
debug "Installing $1..."
|
||||
apt-get install --yes "$1"
|
||||
debug "$1 installation complete."
|
||||
}
|
||||
|
||||
get_photonvision_releases() {
|
||||
# Return cached input
|
||||
if [ -n "$PHOTON_VISION_RELEASES" ] ; then
|
||||
echo "$PHOTON_VISION_RELEASES"
|
||||
return
|
||||
fi
|
||||
|
||||
# Use curl if available, otherwise fallback to wget
|
||||
if command -v curl > /dev/null 2>&1 ; then
|
||||
PHOTON_VISION_RELEASES="$(curl -sk https://api.github.com/repos/photonvision/photonvision/releases)"
|
||||
else
|
||||
PHOTON_VISION_RELEASES="$(wget -qO- https://api.github.com/repos/photonvision/photonvision/releases)"
|
||||
fi
|
||||
|
||||
echo "$PHOTON_VISION_RELEASES"
|
||||
}
|
||||
|
||||
get_versions() {
|
||||
if [ -z "$PHOTON_VISION_VERSIONS" ] ; then
|
||||
PHOTON_VISION_VERSIONS=$(get_photonvision_releases | \
|
||||
sed -En 's/\"tag_name\": \"v([0-9]+\.[0-9]+\.[0-9]+)(-(beta|alpha)(-[0-9])?(\.[0-9]+)?)?\",/\1\2/p' | \
|
||||
sed 's/^[[:space:]]*//')
|
||||
fi
|
||||
|
||||
echo "$PHOTON_VISION_VERSIONS"
|
||||
}
|
||||
|
||||
is_version_available() {
|
||||
local target_version="$1"
|
||||
|
||||
# latest is a special case
|
||||
if [ "$target_version" = "latest" ]; then
|
||||
return 0
|
||||
fi
|
||||
|
||||
# Check if multiple lines are match. You can only match 1.
|
||||
if [ "$(get_versions | grep -cFx "$target_version")" -ne 1 ] ; then
|
||||
return 1
|
||||
fi
|
||||
|
||||
return 0
|
||||
}
|
||||
|
||||
help() {
|
||||
cat << EOF
|
||||
This script installs Photonvision.
|
||||
It must be run as root.
|
||||
|
||||
Syntax: sudo ./install.sh [options]
|
||||
options:
|
||||
-h, --help
|
||||
Display this help message.
|
||||
-l, --list-versions
|
||||
Lists all available versions of PhotonVision.
|
||||
-v <version>, --version=<version>
|
||||
Specifies which version of PhotonVision to install.
|
||||
If not specified, the latest stable release is installed.
|
||||
Ignores leading 'v's.
|
||||
-a <arch>, --arch=<arch>
|
||||
Install PhotonVision for the specified architecture.
|
||||
Supported values: aarch64, x86_64
|
||||
-m [option], --install-nm=[option]
|
||||
Controls NetworkManager installation (Ubuntu only).
|
||||
Options: "yes", "no", "ask".
|
||||
Default: "ask" (unless -q or --quiet is specified, then "no").
|
||||
"ask" prompts for installation. Ignored on other distros.
|
||||
-n, --no-networking
|
||||
Disable networking. This will also prevent installation of
|
||||
NetworkManager, overriding -m,--install-nm.
|
||||
-q, --quiet
|
||||
Silent install, automatically accepts all defaults. For
|
||||
non-interactive use. Makes -m,--install-nm default to "no".
|
||||
|
||||
EOF
|
||||
}
|
||||
|
||||
INSTALL_NETWORK_MANAGER="ask"
|
||||
VERSION="latest"
|
||||
|
||||
while getopts "hlv:a:mnq-:" OPT; do
|
||||
if [ "$OPT" = "-" ]; then
|
||||
OPT="${OPTARG%%=*}" # extract long option name
|
||||
OPTARG="${OPTARG#"$OPT"}" # extract long option argument (may be empty)
|
||||
OPTARG="${OPTARG#=}" # if long option argument, remove assigning `=`
|
||||
fi
|
||||
|
||||
case "$OPT" in
|
||||
h | help)
|
||||
help
|
||||
exit 0
|
||||
;;
|
||||
l | list-versions)
|
||||
get_versions
|
||||
exit 0
|
||||
;;
|
||||
v | version)
|
||||
needs_arg
|
||||
VERSION=${OPTARG#v} # drop leading 'v's
|
||||
;;
|
||||
a | arch) needs_arg; ARCH=$OPTARG
|
||||
;;
|
||||
m | install-nm)
|
||||
INSTALL_NETWORK_MANAGER="$(echo "${OPTARG:-'yes'}" | tr '[:upper:]' '[:lower:]')"
|
||||
case "$INSTALL_NETWORK_MANAGER" in
|
||||
yes)
|
||||
;;
|
||||
no)
|
||||
;;
|
||||
ask)
|
||||
;;
|
||||
* )
|
||||
die "Valid options for -m, --install-nm are: 'yes', 'no', and 'ask'"
|
||||
;;
|
||||
esac
|
||||
;;
|
||||
n | no-networking) DISABLE_NETWORKING="true"
|
||||
;;
|
||||
q | quiet) QUIET="true"
|
||||
;;
|
||||
\?) # Handle invalid short options
|
||||
die "Error: Invalid option -$OPTARG" \
|
||||
"See './install.sh -h' for more information."
|
||||
;;
|
||||
* ) # Handle invalid long options
|
||||
die "Error: Invalid option --$OPT" \
|
||||
"See './install.sh -h' for more information."
|
||||
;;
|
||||
esac
|
||||
done
|
||||
|
||||
if [ "$(id -u)" != "0" ]; then
|
||||
die "This script must be run as root"
|
||||
fi
|
||||
|
||||
if [[ -z "$ARCH" ]]; then
|
||||
debug "Arch was not specified. Inferring..."
|
||||
ARCH=$(uname -m)
|
||||
debug "Arch was inferred to be $ARCH"
|
||||
fi
|
||||
|
||||
ARCH_NAME=""
|
||||
if [ "$ARCH" = "aarch64" ]; then
|
||||
ARCH_NAME="linuxarm64"
|
||||
elif [ "$ARCH" = "armv7l" ]; then
|
||||
die "ARM32 is not supported by PhotonVision. Exiting."
|
||||
elif [ "$ARCH" = "x86_64" ]; then
|
||||
ARCH_NAME="linuxx64"
|
||||
else
|
||||
die "Unsupported or unknown architecture: '$ARCH'." \
|
||||
"Please specify your architecture using: ./install.sh -a <arch> " \
|
||||
"Run './install.sh -h' for more information."
|
||||
fi
|
||||
|
||||
debug "This is the installation script for PhotonVision."
|
||||
debug "Installing for platform $ARCH"
|
||||
|
||||
DISTRO=$(lsb_release -is)
|
||||
|
||||
# Only ask if it makes sense to do so.
|
||||
# i.e. the distro is Ubuntu, you haven't requested disabling networking,
|
||||
# and you have requested a quiet install.
|
||||
if [[ "$INSTALL_NETWORK_MANAGER" == "ask" ]]; then
|
||||
if [[ "$DISTRO" != "Ubuntu" || -n "$DISABLE_NETWORKING" || -n "$QUIET" ]] ; then
|
||||
INSTALL_NETWORK_MANAGER="no"
|
||||
fi
|
||||
fi
|
||||
|
||||
if [[ "$INSTALL_NETWORK_MANAGER" == "ask" ]]; then
|
||||
debug ""
|
||||
debug "Photonvision uses NetworkManager to control networking on your device."
|
||||
debug "This could possibly mess up the network configuration in Ubuntu."
|
||||
read -p "Do you want this script to install and configure NetworkManager? [y/N]: " response
|
||||
if [[ $response == [yY] || $response == [yY][eE][sS] ]]; then
|
||||
INSTALL_NETWORK_MANAGER="yes"
|
||||
fi
|
||||
fi
|
||||
|
||||
debug "Updating package list..."
|
||||
apt-get update
|
||||
debug "Updated package list."
|
||||
|
||||
install_if_missing curl
|
||||
install_if_missing avahi-daemon
|
||||
install_if_missing cpufrequtils
|
||||
install_if_missing libatomic1
|
||||
install_if_missing v4l-utils
|
||||
install_if_missing sqlite3
|
||||
install_if_missing openjdk-17-jre-headless
|
||||
|
||||
debug "Setting cpufrequtils to performance mode"
|
||||
if [ -f /etc/default/cpufrequtils ]; then
|
||||
sed -i -e 's/^#\?GOVERNOR=.*$/GOVERNOR=performance/' /etc/default/cpufrequtils
|
||||
else
|
||||
echo 'GOVERNOR=performance' > /etc/default/cpufrequtils
|
||||
fi
|
||||
|
||||
if [[ "$INSTALL_NETWORK_MANAGER" == "yes" ]]; then
|
||||
debug "NetworkManager installation specified. Installing components..."
|
||||
install_if_missing network-manager
|
||||
install_if_missing net-tools
|
||||
|
||||
debug "Configuring..."
|
||||
systemctl disable systemd-networkd-wait-online.service
|
||||
cat > /etc/netplan/00-default-nm-renderer.yaml <<EOF
|
||||
network:
|
||||
renderer: NetworkManager
|
||||
EOF
|
||||
debug "network-manager installation complete."
|
||||
fi
|
||||
|
||||
debug ""
|
||||
debug "Installing additional math packages"
|
||||
if [[ "$DISTRO" = "Ubuntu" && -z $(apt-cache search libcholmod3) ]]; then
|
||||
debug "Adding jammy to list of apt sources"
|
||||
add-apt-repository -y -S 'deb http://ports.ubuntu.com/ubuntu-ports jammy main universe'
|
||||
fi
|
||||
|
||||
install_if_missing libcholmod3
|
||||
install_if_missing liblapack3
|
||||
install_if_missing libsuitesparseconfig5
|
||||
|
||||
debug ""
|
||||
|
||||
if ! is_version_available "$VERSION" ; then
|
||||
die "PhotonVision v$VERSION is not available" \
|
||||
"See ./install --list-versions for a complete list of available versions."
|
||||
fi
|
||||
|
||||
if [ "$VERSION" = "latest" ] ; then
|
||||
RELEASE_URL="https://api.github.com/repos/photonvision/photonvision/releases/latest"
|
||||
debug "Downloading PhotonVision (latest)..."
|
||||
else
|
||||
RELEASE_URL="https://api.github.com/repos/photonvision/photonvision/releases/tags/v$VERSION"
|
||||
debug "Downloading PhotonVision (v$VERSION)..."
|
||||
fi
|
||||
|
||||
mkdir -p /opt/photonvision
|
||||
cd /opt/photonvision || die "Tried to enter /opt/photonvision, but it was not created."
|
||||
curl -sk "$RELEASE_URL" |
|
||||
grep "browser_download_url.*$ARCH_NAME.jar" |
|
||||
cut -d : -f 2,3 |
|
||||
tr -d '"' |
|
||||
wget -qi - -O photonvision.jar
|
||||
debug "Downloaded PhotonVision."
|
||||
|
||||
debug "Creating the PhotonVision systemd service..."
|
||||
|
||||
# service --status-all doesn't list photonvision on OrangePi use systemctl instead:
|
||||
if systemctl --quiet is-active photonvision; then
|
||||
debug "PhotonVision is already running. Stopping service."
|
||||
systemctl stop photonvision
|
||||
systemctl disable photonvision
|
||||
rm /lib/systemd/system/photonvision.service
|
||||
rm /etc/systemd/system/photonvision.service
|
||||
systemctl daemon-reload
|
||||
systemctl reset-failed
|
||||
fi
|
||||
|
||||
cat > /lib/systemd/system/photonvision.service <<EOF
|
||||
[Unit]
|
||||
Description=Service that runs PhotonVision
|
||||
|
||||
[Service]
|
||||
WorkingDirectory=/opt/photonvision
|
||||
# Run photonvision at "nice" -10, which is higher priority than standard
|
||||
Nice=-10
|
||||
# for non-uniform CPUs, like big.LITTLE, you want to select the big cores
|
||||
# look up the right values for your CPU
|
||||
# AllowedCPUs=4-7
|
||||
|
||||
ExecStart=/usr/bin/java -Xmx512m -jar /opt/photonvision/photonvision.jar
|
||||
ExecStop=/bin/systemctl kill photonvision
|
||||
Type=simple
|
||||
Restart=on-failure
|
||||
RestartSec=1
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
EOF
|
||||
|
||||
if [ "$DISABLE_NETWORKING" = "true" ]; then
|
||||
sed -i "s/photonvision.jar/photonvision.jar -n/" /lib/systemd/system/photonvision.service
|
||||
fi
|
||||
|
||||
if grep -q "RK3588" /proc/cpuinfo; then
|
||||
debug "This has a Rockchip RK3588, enabling all cores"
|
||||
sed -i 's/# AllowedCPUs=4-7/AllowedCPUs=4-7/g' /lib/systemd/system/photonvision.service
|
||||
fi
|
||||
|
||||
cp /lib/systemd/system/photonvision.service /etc/systemd/system/photonvision.service
|
||||
chmod 644 /etc/systemd/system/photonvision.service
|
||||
systemctl daemon-reload
|
||||
systemctl enable photonvision.service
|
||||
|
||||
debug "Created PhotonVision systemd service."
|
||||
|
||||
debug "PhotonVision installation successful."
|
||||
# The install script is now in photon-image-modifier
|
||||
# this downloads and runs that install script for people using the old short URL
|
||||
wget -q https://raw.githubusercontent.com/PhotonVision/photon-image-modifier/master/install.sh -O ./real_install.sh
|
||||
chmod +x ./real_install.sh
|
||||
./real_install.sh "$@"
|
||||
rm ./real_install.sh
|
||||
|
||||
@@ -85,8 +85,6 @@ ext.appendDebugPathToBinaries = { binaries ->
|
||||
}
|
||||
}
|
||||
|
||||
def licenseFile = file("$rootDir/LICENCE")
|
||||
|
||||
// Create ZIP tasks for each component.
|
||||
ext.createComponentZipTasks = { components, names, base, type, project, func ->
|
||||
def stringNames = names.collect { it.toString() }
|
||||
|
||||
@@ -13,6 +13,7 @@ def artifactGroupId = 'org.photonvision'
|
||||
def javaBaseName = "_GROUP_org_photonvision_${baseArtifactId}_ID_${baseArtifactId}-java_CLS"
|
||||
|
||||
def outputsFolder = file("$buildDir/outputs")
|
||||
def licenseFile = ext.licenseFile
|
||||
|
||||
javadoc {
|
||||
options {
|
||||
@@ -21,20 +22,27 @@ javadoc {
|
||||
}
|
||||
}
|
||||
|
||||
jar {
|
||||
from licenseFile
|
||||
}
|
||||
|
||||
task sourcesJar(type: Jar, dependsOn: classes) {
|
||||
archiveClassifier = 'sources'
|
||||
from sourceSets.main.allSource
|
||||
from licenseFile
|
||||
}
|
||||
|
||||
task javadocJar(type: Jar, dependsOn: javadoc) {
|
||||
archiveClassifier = 'javadoc'
|
||||
from javadoc.destinationDir
|
||||
from licenseFile
|
||||
}
|
||||
|
||||
task outputJar(type: Jar, dependsOn: classes) {
|
||||
archiveBaseName = javaBaseName
|
||||
destinationDirectory = outputsFolder
|
||||
from sourceSets.main.output
|
||||
from licenseFile
|
||||
}
|
||||
|
||||
task outputSourcesJar(type: Jar, dependsOn: classes) {
|
||||
@@ -42,6 +50,7 @@ task outputSourcesJar(type: Jar, dependsOn: classes) {
|
||||
destinationDirectory = outputsFolder
|
||||
archiveClassifier = 'sources'
|
||||
from sourceSets.main.allSource
|
||||
from licenseFile
|
||||
}
|
||||
|
||||
task outputJavadocJar(type: Jar, dependsOn: javadoc) {
|
||||
@@ -49,6 +58,7 @@ task outputJavadocJar(type: Jar, dependsOn: javadoc) {
|
||||
destinationDirectory = outputsFolder
|
||||
archiveClassifier = 'javadoc'
|
||||
from javadoc.destinationDir
|
||||
from licenseFile
|
||||
}
|
||||
|
||||
artifacts {
|
||||
|
||||
@@ -10,8 +10,7 @@ def zipBaseName = "_GROUP_org_photonvision_${baseArtifactId}_ID_${baseArtifactId
|
||||
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")
|
||||
|
||||
def licenseFile = ext.licenseFile
|
||||
// Quick hack to make this name visible to photon-lib for combined
|
||||
ext.zipBaseName = zipBaseName
|
||||
ext.artifactGroupId = artifactGroupId
|
||||
|
||||
Reference in New Issue
Block a user