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

Auto-generate packet dataclasses with Jinja (#1374)

Browse Source
This commit is contained in:
Matt
2024-08-31 13:44:19 -04:00
committed by GitHub
parent c19d54c633
commit 169595e56e
140 changed files with 4445 additions and 2097 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -48,6 +48,8 @@ jobs:
fetch-depth: 0
- name: Fetch tags
run: git fetch --tags --force
- name: Install RoboRIO Toolchain
run: ./gradlew installRoboRioToolchain
- name: Install Java 17
uses: actions/setup-java@v4
with:

3
.gitignore vendored
View File

@@ -165,3 +165,6 @@ photon-server/src/main/resources/web/index.html
photon-lib/src/generate/native/cpp/PhotonVersion.cpp
venv
.venv/*
.venv

2
build.gradle
View File

@@ -64,7 +64,7 @@ spotless {
java {
target fileTree('.') {
include '**/*.java'
exclude '**/build/**', '**/build-*/**', "photon-core\\src\\main\\java\\org\\photonvision\\PhotonVersion.java", "photon-lib\\src\\main\\java\\org\\photonvision\\PhotonVersion.java"
exclude '**/build/**', '**/build-*/**', "photon-core\\src\\main\\java\\org\\photonvision\\PhotonVersion.java", "photon-lib\\src\\main\\java\\org\\photonvision\\PhotonVersion.java", "**/src/generated/**"
}
toggleOffOn()
googleJavaFormat()

1
devTools/calibrationUtils.py
View File

@@ -3,7 +3,6 @@ import base64
from dataclasses import dataclass
import json
import os
from typing import Union
import cv2
import numpy as np
import mrcal

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

@@ -139,7 +139,8 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
TrackedTarget.simpleFromTrackedTargets(result.targets),
result.multiTagResult);
ts.resultPublisher.set(simplified, simplified.getPacketSize());
// random guess at size of the array
ts.resultPublisher.set(simplified, 1024);
if (ConfigManager.getInstance().getConfig().getNetworkConfig().shouldPublishProto) {
ts.protoResultPublisher.set(simplified);
}

9
photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java
View File

@@ -62,13 +62,14 @@ public class UIDataPublisher implements CVPipelineResultConsumer {
dataMap.put("classNames", result.objectDetectionClassNames);
// Only send Multitag Results if they are present, similar to 3d pose
if (result.multiTagResult.estimatedPose.isPresent) {
if (result.multiTagResult.isPresent()) {
var multitagData = new HashMap<String, Object>();
multitagData.put(
"bestTransform",
SerializationUtils.transformToHashMap(result.multiTagResult.estimatedPose.best));
multitagData.put("bestReprojectionError", result.multiTagResult.estimatedPose.bestReprojErr);
multitagData.put("fiducialIDsUsed", result.multiTagResult.fiducialIDsUsed);
SerializationUtils.transformToHashMap(result.multiTagResult.get().estimatedPose.best));
multitagData.put(
"bestReprojectionError", result.multiTagResult.get().estimatedPose.bestReprojErr);
multitagData.put("fiducialIDsUsed", result.multiTagResult.get().fiducialIDsUsed);
dataMap.put("multitagResult", multitagData);
}

23
photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java
View File

@@ -20,6 +20,7 @@ package org.photonvision.vision.pipe.impl;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.estimation.TargetModel;
@@ -32,13 +33,15 @@ import org.photonvision.vision.target.TrackedTarget;
/** Estimate the camera pose given multiple Apriltag observations */
public class MultiTargetPNPPipe
extends CVPipe<
List<TrackedTarget>, MultiTargetPNPResult, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
List<TrackedTarget>,
Optional<MultiTargetPNPResult>,
MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
private static final Logger logger = new Logger(MultiTargetPNPPipe.class, LogGroup.VisionModule);
private boolean hasWarned = false;
@Override
protected MultiTargetPNPResult process(List<TrackedTarget> targetList) {
protected Optional<MultiTargetPNPResult> process(List<TrackedTarget> targetList) {
if (params == null
|| params.cameraCoefficients == null
|| params.cameraCoefficients.getCameraIntrinsicsMat() == null
@@ -48,23 +51,23 @@ public class MultiTargetPNPPipe
"Cannot perform solvePNP an uncalibrated camera! Please calibrate this resolution...");
hasWarned = true;
}
return new MultiTargetPNPResult();
return Optional.empty();
}
return calculateCameraInField(targetList);
}
private MultiTargetPNPResult calculateCameraInField(List<TrackedTarget> targetList) {
private Optional<MultiTargetPNPResult> calculateCameraInField(List<TrackedTarget> targetList) {
// Find tag IDs that exist in the tag layout
var tagIDsUsed = new ArrayList<Integer>();
var tagIDsUsed = new ArrayList<Short>();
for (var target : targetList) {
int id = target.getFiducialId();
if (params.atfl.getTagPose(id).isPresent()) tagIDsUsed.add(id);
if (params.atfl.getTagPose(id).isPresent()) tagIDsUsed.add((short) id);
}
// Only run with multiple targets
if (tagIDsUsed.size() < 2) {
return new MultiTargetPNPResult();
return Optional.empty();
}
var estimatedPose =
@@ -75,7 +78,11 @@ public class MultiTargetPNPPipe
params.atfl,
params.targetModel);
return new MultiTargetPNPResult(estimatedPose, tagIDsUsed);
if (estimatedPose.isPresent()) {
return Optional.of(new MultiTargetPNPResult(estimatedPose.get(), tagIDsUsed));
} else {
return Optional.empty();
}
}
public static class MultiTargetPNPPipeParams {

10
photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View File

@@ -28,6 +28,7 @@ import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.util.Units;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.estimation.TargetModel;
@@ -149,7 +150,7 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
}
// Do multi-tag pose estimation
MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
Optional<MultiTargetPNPResult> multiTagResult = Optional.empty();
if (settings.solvePNPEnabled && settings.doMultiTarget) {
var multiTagOutput = multiTagPNPPipe.run(targetList);
sumPipeNanosElapsed += multiTagOutput.nanosElapsed;
@@ -167,20 +168,21 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
AprilTagPoseEstimate tagPoseEstimate = null;
// Do single-tag estimation when "always enabled" or if a tag was not used for multitag
if (settings.doSingleTargetAlways
|| !multiTagResult.fiducialIDsUsed.contains(Integer.valueOf(detection.getId()))) {
|| !(multiTagResult.isPresent()
&& multiTagResult.get().fiducialIDsUsed.contains((short) detection.getId()))) {
var poseResult = singleTagPoseEstimatorPipe.run(detection);
sumPipeNanosElapsed += poseResult.nanosElapsed;
tagPoseEstimate = poseResult.output;
}
// If single-tag estimation was not done, this is a multi-target tag from the layout
if (tagPoseEstimate == null) {
if (tagPoseEstimate == null && multiTagResult.isPresent()) {
// compute this tag's camera-to-tag transform using the multitag result
var tagPose = atfl.getTagPose(detection.getId());
if (tagPose.isPresent()) {
var camToTag =
new Transform3d(
new Pose3d().plus(multiTagResult.estimatedPose.best), tagPose.get());
new Pose3d().plus(multiTagResult.get().estimatedPose.best), tagPose.get());
// match expected AprilTag coordinate system
camToTag =
CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN());

10
photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java
View File

@@ -41,6 +41,7 @@ import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.util.Units;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.opencv.core.Mat;
import org.opencv.imgproc.Imgproc;
import org.opencv.objdetect.Objdetect;
@@ -170,7 +171,7 @@ public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSet
}
// Do multi-tag pose estimation
MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
Optional<MultiTargetPNPResult> multiTagResult = Optional.empty();
if (settings.solvePNPEnabled && settings.doMultiTarget) {
var multiTagOutput = multiTagPNPPipe.run(targetList);
sumPipeNanosElapsed += multiTagOutput.nanosElapsed;
@@ -188,20 +189,21 @@ public class ArucoPipeline extends CVPipeline<CVPipelineResult, ArucoPipelineSet
AprilTagPoseEstimate tagPoseEstimate = null;
// Do single-tag estimation when "always enabled" or if a tag was not used for multitag
if (settings.doSingleTargetAlways
|| !multiTagResult.fiducialIDsUsed.contains(detection.getId())) {
|| !(multiTagResult.isPresent()
&& multiTagResult.get().fiducialIDsUsed.contains((short) detection.getId()))) {
var poseResult = singleTagPoseEstimatorPipe.run(detection);
sumPipeNanosElapsed += poseResult.nanosElapsed;
tagPoseEstimate = poseResult.output;
}
// If single-tag estimation was not done, this is a multi-target tag from the layout
if (tagPoseEstimate == null) {
if (tagPoseEstimate == null && multiTagResult.isPresent()) {
// compute this tag's camera-to-tag transform using the multitag result
var tagPose = atfl.getTagPose(detection.getId());
if (tagPose.isPresent()) {
var camToTag =
new Transform3d(
new Pose3d().plus(multiTagResult.estimatedPose.best), tagPose.get());
new Pose3d().plus(multiTagResult.get().estimatedPose.best), tagPose.get());
// match expected OpenCV coordinate system
camToTag =
CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN());

27
photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java
View File

@@ -19,6 +19,7 @@ package org.photonvision.vision.pipeline.result;
import java.util.Collections;
import java.util.List;
import java.util.Optional;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.vision.frame.Frame;
@@ -32,7 +33,7 @@ public class CVPipelineResult implements Releasable {
public final double fps;
public final List<TrackedTarget> targets;
public final Frame inputAndOutputFrame;
public MultiTargetPNPResult multiTagResult;
public Optional<MultiTargetPNPResult> multiTagResult;
public final List<String> objectDetectionClassNames;
public CVPipelineResult(
@@ -41,14 +42,7 @@ public class CVPipelineResult implements Releasable {
double fps,
List<TrackedTarget> targets,
Frame inputFrame) {
this(
sequenceID,
processingNanos,
fps,
targets,
new MultiTargetPNPResult(),
inputFrame,
List.of());
this(sequenceID, processingNanos, fps, targets, Optional.empty(), inputFrame, List.of());
}
public CVPipelineResult(
@@ -58,14 +52,7 @@ public class CVPipelineResult implements Releasable {
List<TrackedTarget> targets,
Frame inputFrame,
List<String> classNames) {
this(
sequenceID,
processingNanos,
fps,
targets,
new MultiTargetPNPResult(),
inputFrame,
classNames);
this(sequenceID, processingNanos, fps, targets, Optional.empty(), inputFrame, classNames);
}
public CVPipelineResult(
@@ -73,7 +60,7 @@ public class CVPipelineResult implements Releasable {
double processingNanos,
double fps,
List<TrackedTarget> targets,
MultiTargetPNPResult multiTagResult,
Optional<MultiTargetPNPResult> multiTagResult,
Frame inputFrame) {
this(sequenceID, processingNanos, fps, targets, multiTagResult, inputFrame, List.of());
}
@@ -83,7 +70,7 @@ public class CVPipelineResult implements Releasable {
double processingNanos,
double fps,
List<TrackedTarget> targets,
MultiTargetPNPResult multiTagResult,
Optional<MultiTargetPNPResult> multiTagResult,
Frame inputFrame,
List<String> classNames) {
this.sequenceID = sequenceID;
@@ -101,7 +88,7 @@ public class CVPipelineResult implements Releasable {
double processingNanos,
double fps,
List<TrackedTarget> targets,
MultiTargetPNPResult multiTagResult) {
Optional<MultiTargetPNPResult> multiTagResult) {
this(sequenceID, processingNanos, fps, targets, multiTagResult, null, List.of());
}

2
photon-docs/build.gradle
View File

@@ -155,7 +155,7 @@ doxygen {
html_timestamp true
javadoc_autobrief true
project_name 'PhotonVision C++'
project_logo '../wpiutil/src/main/native/resources/wpilib-128.png'
project_logo '../photon-client/src/assets/images/logoSmall.svg'
project_number pubVersion
quiet true
recursive true

20
photon-lib/build.gradle
View File

@@ -18,7 +18,25 @@ apply from: "${rootDir}/versioningHelper.gradle"
nativeUtils {
exportsConfigs {
"${nativeName}" {}
"${nativeName}" {
// From https://github.com/wpilibsuite/allwpilib/blob/a32589831184969939fd3d63f449a2788a0a8542/wpimath/build.gradle#L72
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
x64ExcludeSymbols = [
'_CT??_R0?AV_System_error',
'_CT??_R0?AVexception',
'_CT??_R0?AVfailure',
'_CT??_R0?AVruntime_error',
'_CT??_R0?AVsystem_error',
'_CTA5?AVfailure',
'_TI5?AVfailure',
'_CT??_R0?AVout_of_range',
'_CTA3?AVout_of_range',
'_TI3?AVout_of_range',
'_CT??_R0?AVbad_cast'
]
}
}
}

14
photon-lib/py/buildAndTest.sh Executable file
View File

@@ -0,0 +1,14 @@
# Uninstall if it already was installed
python3 -m pip uninstall -y photonlibpy
# Build wheel
python3 setup.py bdist_wheel
# Install whatever wheel was made
for f in dist/*.whl; do
echo "installing $f"
python3 -m pip install --no-cache-dir "$f"
done
# Run the test suite
pytest -rP --full-trace

5
photon-lib/py/photonlibpy/__init__.py
View File

@@ -1 +1,6 @@
# No one here but us chickens
from .packet import Packet # noqa
from .estimatedRobotPose import EstimatedRobotPose # noqa
from .photonPoseEstimator import PhotonPoseEstimator, PoseStrategy # noqa
from .photonCamera import PhotonCamera # noqa

2
photon-lib/py/photonlibpy/estimatedRobotPose.py
View File

@@ -3,7 +3,7 @@ from typing import TYPE_CHECKING
from wpimath.geometry import Pose3d
from .photonTrackedTarget import PhotonTrackedTarget
from .targeting.photonTrackedTarget import PhotonTrackedTarget
if TYPE_CHECKING:
from .photonPoseEstimator import PoseStrategy

46
photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py Normal file
View File

@@ -0,0 +1,46 @@
###############################################################################
## 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/>.
###############################################################################
###############################################################################
## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py.
## --> DO NOT MODIFY <--
###############################################################################
from ..targeting import *
class MultiTargetPNPResultSerde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "ffc1cb847deb6e796a583a5b1885496b"
MESSAGE_FORMAT = "PnpResult estimatedPose;int16[?] fiducialIDsUsed;"
@staticmethod
def unpack(packet: "Packet") -> "MultiTargetPNPResult":
ret = MultiTargetPNPResult()
# estimatedPose is of non-intrinsic type PnpResult
ret.estimatedPose = PnpResult.photonStruct.unpack(packet)
# fiducialIDsUsed is a custom VLA!
ret.fiducialIDsUsed = packet.decodeShortList()
return ret
# Hack ourselves into the base class
MultiTargetPNPResult.photonStruct = MultiTargetPNPResultSerde()

51
photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py Normal file
View File

@@ -0,0 +1,51 @@
###############################################################################
## 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/>.
###############################################################################
###############################################################################
## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py.
## --> DO NOT MODIFY <--
###############################################################################
from ..targeting import *
class PhotonPipelineMetadataSerde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "2a7039527bda14d13028a1b9282d40a2"
MESSAGE_FORMAT = (
"int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;"
)
@staticmethod
def unpack(packet: "Packet") -> "PhotonPipelineMetadata":
ret = PhotonPipelineMetadata()
# sequenceID is of intrinsic type int64
ret.sequenceID = packet.decodeLong()
# captureTimestampMicros is of intrinsic type int64
ret.captureTimestampMicros = packet.decodeLong()
# publishTimestampMicros is of intrinsic type int64
ret.publishTimestampMicros = packet.decodeLong()
return ret
# Hack ourselves into the base class
PhotonPipelineMetadata.photonStruct = PhotonPipelineMetadataSerde()

49
photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py Normal file
View File

@@ -0,0 +1,49 @@
###############################################################################
## 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/>.
###############################################################################
###############################################################################
## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py.
## --> DO NOT MODIFY <--
###############################################################################
from ..targeting import *
class PhotonPipelineResultSerde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "cb3e1605048ba49325888eb797399fe2"
MESSAGE_FORMAT = "PhotonPipelineMetadata metadata;PhotonTrackedTarget[?] targets;MultiTargetPNPResult? multiTagResult;"
@staticmethod
def unpack(packet: "Packet") -> "PhotonPipelineResult":
ret = PhotonPipelineResult()
# metadata is of non-intrinsic type PhotonPipelineMetadata
ret.metadata = PhotonPipelineMetadata.photonStruct.unpack(packet)
# targets is a custom VLA!
ret.targets = packet.decodeList(PhotonTrackedTarget.photonStruct)
# multiTagResult is optional! it better not be a VLA too
ret.multiTagResult = packet.decodeOptional(MultiTargetPNPResult.photonStruct)
return ret
# Hack ourselves into the base class
PhotonPipelineResult.photonStruct = PhotonPipelineResultSerde()

76
photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py Normal file
View File

@@ -0,0 +1,76 @@
###############################################################################
## 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/>.
###############################################################################
###############################################################################
## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py.
## --> DO NOT MODIFY <--
###############################################################################
from ..targeting import *
class PhotonTrackedTargetSerde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "8fdada56b9162f2e32bd24f0055d7b60"
MESSAGE_FORMAT = "float64 yaw;float64 pitch;float64 area;float64 skew;int32 fiducialId;int32 objDetectId;float32 objDetectConf;Transform3d bestCameraToTarget;Transform3d altCameraToTarget;float64 poseAmbiguity;TargetCorner[?] minAreaRectCorners;TargetCorner[?] detectedCorners;"
@staticmethod
def unpack(packet: "Packet") -> "PhotonTrackedTarget":
ret = PhotonTrackedTarget()
# yaw is of intrinsic type float64
ret.yaw = packet.decodeDouble()
# pitch is of intrinsic type float64
ret.pitch = packet.decodeDouble()
# area is of intrinsic type float64
ret.area = packet.decodeDouble()
# skew is of intrinsic type float64
ret.skew = packet.decodeDouble()
# fiducialId is of intrinsic type int32
ret.fiducialId = packet.decodeInt()
# objDetectId is of intrinsic type int32
ret.objDetectId = packet.decodeInt()
# objDetectConf is of intrinsic type float32
ret.objDetectConf = packet.decodeFloat()
# field is shimmed!
ret.bestCameraToTarget = packet.decodeTransform()
# field is shimmed!
ret.altCameraToTarget = packet.decodeTransform()
# poseAmbiguity is of intrinsic type float64
ret.poseAmbiguity = packet.decodeDouble()
# minAreaRectCorners is a custom VLA!
ret.minAreaRectCorners = packet.decodeList(TargetCorner.photonStruct)
# detectedCorners is a custom VLA!
ret.detectedCorners = packet.decodeList(TargetCorner.photonStruct)
return ret
# Hack ourselves into the base class
PhotonTrackedTarget.photonStruct = PhotonTrackedTargetSerde()

55
photon-lib/py/photonlibpy/generated/PnpResultSerde.py Normal file
View File

@@ -0,0 +1,55 @@
###############################################################################
## 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/>.
###############################################################################
###############################################################################
## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py.
## --> DO NOT MODIFY <--
###############################################################################
from ..targeting import *
class PnpResultSerde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "0d1f2546b00f24718e30f38d206d4491"
MESSAGE_FORMAT = "Transform3d best;Transform3d alt;float64 bestReprojErr;float64 altReprojErr;float64 ambiguity;"
@staticmethod
def unpack(packet: "Packet") -> "PnpResult":
ret = PnpResult()
# field is shimmed!
ret.best = packet.decodeTransform()
# field is shimmed!
ret.alt = packet.decodeTransform()
# bestReprojErr is of intrinsic type float64
ret.bestReprojErr = packet.decodeDouble()
# altReprojErr is of intrinsic type float64
ret.altReprojErr = packet.decodeDouble()
# ambiguity is of intrinsic type float64
ret.ambiguity = packet.decodeDouble()
return ret
# Hack ourselves into the base class
PnpResult.photonStruct = PnpResultSerde()

46
photon-lib/py/photonlibpy/generated/TargetCornerSerde.py Normal file
View File

@@ -0,0 +1,46 @@
###############################################################################
## 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/>.
###############################################################################
###############################################################################
## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py.
## --> DO NOT MODIFY <--
###############################################################################
from ..targeting import *
class TargetCornerSerde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "22b1ff7551d10215af6fb3672fe4eda8"
MESSAGE_FORMAT = "float64 x;float64 y;"
@staticmethod
def unpack(packet: "Packet") -> "TargetCorner":
ret = TargetCorner()
# x is of intrinsic type float64
ret.x = packet.decodeDouble()
# y is of intrinsic type float64
ret.y = packet.decodeDouble()
return ret
# Hack ourselves into the base class
TargetCorner.photonStruct = TargetCornerSerde()

9
photon-lib/py/photonlibpy/generated/__init__.py Normal file
View File

@@ -0,0 +1,9 @@
# no one but us chickens
from .MultiTargetPNPResultSerde import MultiTargetPNPResultSerde # noqa
from .PhotonPipelineMetadataSerde import PhotonPipelineMetadataSerde # noqa
from .PhotonPipelineMetadataSerde import PhotonPipelineMetadataSerde # noqa
from .PhotonPipelineResultSerde import PhotonPipelineResultSerde # noqa
from .PhotonTrackedTargetSerde import PhotonTrackedTargetSerde # noqa
from .PnpResultSerde import PnpResultSerde # noqa
from .TargetCornerSerde import TargetCornerSerde # noqa

49
photon-lib/py/photonlibpy/multiTargetPNPResult.py
View File

@@ -1,49 +0,0 @@
from dataclasses import dataclass, field
from wpimath.geometry import Transform3d
from photonlibpy.packet import Packet
@dataclass
class PNPResult:
_NUM_BYTES_IN_FLOAT = 8
PACK_SIZE_BYTES = 1 + (_NUM_BYTES_IN_FLOAT * 7 * 2) + (_NUM_BYTES_IN_FLOAT * 3)
isPresent: bool = False
best: Transform3d = field(default_factory=Transform3d)
alt: Transform3d = field(default_factory=Transform3d)
ambiguity: float = 0.0
bestReprojError: float = 0.0
altReprojError: float = 0.0
def createFromPacket(self, packet: Packet) -> Packet:
self.isPresent = packet.decodeBoolean()
if not self.isPresent:
return packet
self.best = packet.decodeTransform()
self.alt = packet.decodeTransform()
self.bestReprojError = packet.decodeDouble()
self.altReprojError = packet.decodeDouble()
self.ambiguity = packet.decodeDouble()
return packet
@dataclass
class MultiTargetPNPResult:
_MAX_IDS = 32
# pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally)
_PACK_SIZE_BYTES = PNPResult.PACK_SIZE_BYTES + (1 * _MAX_IDS)
estimatedPose: PNPResult = field(default_factory=PNPResult)
fiducialIDsUsed: list[int] = field(default_factory=list)
def createFromPacket(self, packet: Packet) -> Packet:
self.estimatedPose = PNPResult()
self.estimatedPose.createFromPacket(packet)
self.fiducialIDsUsed = []
for _ in range(MultiTargetPNPResult._MAX_IDS):
fidId = packet.decode16()
if fidId >= 0:
self.fiducialIDsUsed.append(fidId)
return packet

34
photon-lib/py/photonlibpy/packet.py
View File

@@ -1,4 +1,5 @@
import struct
from typing import Any, Optional, Type
from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Quaternion
import wpilib
@@ -82,13 +83,13 @@ class Packet:
def decode16(self) -> int:
"""
* Returns a single decoded byte from the packet.
* Returns a single decoded short from the packet.
*
* @return A decoded byte from the packet.
* @return A decoded short from the packet.
"""
return self._decodeGeneric(">h", 2)
def decode32(self) -> int:
def decodeInt(self) -> int:
"""
* Returns a decoded int (32 bytes) from the packet.
*
@@ -104,7 +105,7 @@ class Packet:
"""
return self._decodeGeneric(">f", 4)
def decodei64(self) -> int:
def decodeLong(self) -> int:
"""
* Returns a decoded int64 from the packet.
*
@@ -131,14 +132,22 @@ class Packet:
def decodeDoubleArray(self, length: int) -> list[float]:
"""
* Returns a decoded array of floats from the packet.
*
* @return A decoded array of floats from the packet.
"""
ret = []
for _ in range(length):
ret.append(self.decodeDouble())
return ret
def decodeShortList(self) -> list[float]:
"""
* Returns a decoded array of shorts from the packet.
"""
length = self.decode8()
ret = []
for _ in range(length):
ret.append(self.decode16())
return ret
def decodeTransform(self) -> Transform3d:
"""
* Returns a decoded Transform3d
@@ -157,3 +166,16 @@ class Packet:
rotation = Rotation3d(Quaternion(w, x, y, z))
return Transform3d(translation, rotation)
def decodeList(self, serde: Type):
retList = []
arr_len = self.decode8()
for _ in range(arr_len):
retList.append(serde.unpack(self))
return retList
def decodeOptional(self, serde: Type) -> Optional[Any]:
if self.decodeBoolean():
return serde.unpack(self)
else:
return None

26
photon-lib/py/photonlibpy/photonCamera.py
View File

@@ -3,9 +3,12 @@ from typing import List
import ntcore
from wpilib import RobotController, Timer
import wpilib
from photonlibpy.packet import Packet
from photonlibpy.photonPipelineResult import PhotonPipelineResult
from photonlibpy.version import PHOTONVISION_VERSION, PHOTONLIB_VERSION # type: ignore[import-untyped]
from .packet import Packet
from .targeting.photonPipelineResult import PhotonPipelineResult
from .version import PHOTONVISION_VERSION, PHOTONLIB_VERSION # type: ignore[import-untyped]
# magical import to make serde stuff work
import photonlibpy.generated # noqa
class VisionLEDMode(Enum):
@@ -100,11 +103,9 @@ class PhotonCamera:
else:
newResult = PhotonPipelineResult()
pkt = Packet(byteList)
newResult.populateFromPacket(pkt)
newResult = PhotonPipelineResult.photonStruct.unpack(pkt)
# NT4 allows us to correct the timestamp based on when the message was sent
newResult.setTimestampSeconds(
timestamp / 1e6 - newResult.getLatencyMillis() / 1e3
)
newResult.ntReceiveTimestampMicros = timestamp / 1e6
ret.append(newResult)
return ret
@@ -113,18 +114,17 @@ class PhotonCamera:
self._versionCheck()
now = RobotController.getFPGATime()
retVal = PhotonPipelineResult()
packetWithTimestamp = self._rawBytesEntry.getAtomic()
byteList = packetWithTimestamp.value
timestamp = packetWithTimestamp.time
packetWithTimestamp.time
if len(byteList) < 1:
return retVal
return PhotonPipelineResult()
else:
pkt = Packet(byteList)
retVal.populateFromPacket(pkt)
retVal = PhotonPipelineResult.photonStruct.unpack(pkt)
# We don't trust NT4 time, hack around
retVal.ntRecieveTimestampMicros = now
retVal.ntReceiveTimestampMicros = now
return retVal
def getDriverMode(self) -> bool:
@@ -233,6 +233,6 @@ class PhotonCamera:
wpilib.reportWarning(bfw)
errText = f"Photon version {PHOTONLIB_VERSION} does not match coprocessor version {versionString}. Please install photonlibpy version {PHOTONLIB_VERSION}."
errText = f"Photon version {PHOTONLIB_VERSION} does not match coprocessor version {versionString}. Please install photonlibpy version {versionString}, or update your coprocessor to {PHOTONLIB_VERSION}."
wpilib.reportError(errText, True)
raise Exception(errText)

2
photon-lib/py/photonlibpy/photonPoseEstimator.py
View File

@@ -5,7 +5,7 @@ import wpilib
from robotpy_apriltag import AprilTagFieldLayout
from wpimath.geometry import Transform3d, Pose3d, Pose2d
from .photonPipelineResult import PhotonPipelineResult
from .targeting.photonPipelineResult import PhotonPipelineResult
from .photonCamera import PhotonCamera
from .estimatedRobotPose import EstimatedRobotPose

9
photon-lib/py/photonlibpy/targeting/TargetCorner.py Normal file
View File

@@ -0,0 +1,9 @@
from dataclasses import dataclass
@dataclass
class TargetCorner:
x: float = 0
y: float = 9
photonStruct: "TargetCornerSerde" = None

6
photon-lib/py/photonlibpy/targeting/__init__.py Normal file
View File

@@ -0,0 +1,6 @@
# no one but us chickens
from .TargetCorner import TargetCorner # noqa
from .multiTargetPNPResult import MultiTargetPNPResult, PnpResult # noqa
from .photonPipelineResult import PhotonPipelineMetadata, PhotonPipelineResult # noqa
from .photonTrackedTarget import PhotonTrackedTarget # noqa

34
photon-lib/py/photonlibpy/targeting/multiTargetPNPResult.py Normal file
View File

@@ -0,0 +1,34 @@
from dataclasses import dataclass, field
from wpimath.geometry import Transform3d
from ..packet import Packet
@dataclass
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
photonStruct: "PNPResultSerde" = None
@dataclass
class MultiTargetPNPResult:
_MAX_IDS = 32
estimatedPose: PnpResult = field(default_factory=PnpResult)
fiducialIDsUsed: list[int] = field(default_factory=list)
def createFromPacket(self, packet: Packet) -> Packet:
self.estimatedPose = PnpResult()
self.estimatedPose.createFromPacket(packet)
self.fiducialIDsUsed = []
for _ in range(MultiTargetPNPResult._MAX_IDS):
fidId = packet.decode16()
if fidId >= 0:
self.fiducialIDsUsed.append(fidId)
return packet
photonStruct: "MultiTargetPNPResultSerde" = None

60
photon-lib/py/photonlibpy/photonPipelineResult.py → photon-lib/py/photonlibpy/targeting/photonPipelineResult.py
View File

@@ -1,12 +1,12 @@
from dataclasses import dataclass, field
from typing import Optional
from photonlibpy.multiTargetPNPResult import MultiTargetPNPResult
from photonlibpy.packet import Packet
from photonlibpy.photonTrackedTarget import PhotonTrackedTarget
from .multiTargetPNPResult import MultiTargetPNPResult
from .photonTrackedTarget import PhotonTrackedTarget
@dataclass
class PhotonPipelineResult:
class PhotonPipelineMetadata:
# Image capture and NT publish timestamp, in microseconds and in the coprocessor timebase. As
# reported by WPIUtilJNI::now.
captureTimestampMicros: int = -1
@@ -15,49 +15,44 @@ class PhotonPipelineResult:
# Mirror of the heartbeat entry -- monotonically increasing
sequenceID: int = -1
photonStruct: "PhotonPipelineMetadataSerde" = None
@dataclass
class PhotonPipelineResult:
# Since we don't trust NT time sync, keep track of when we got this packet into robot code
ntRecieveTimestampMicros: int = -1
ntReceiveTimestampMicros: int = -1
targets: list[PhotonTrackedTarget] = field(default_factory=list)
multiTagResult: MultiTargetPNPResult = field(default_factory=MultiTargetPNPResult)
def populateFromPacket(self, packet: Packet) -> Packet:
self.targets = []
self.sequenceID = packet.decodei64()
self.captureTimestampMicros = packet.decodei64()
self.publishTimestampMicros = packet.decodei64()
targetCount = packet.decode8()
for _ in range(targetCount):
target = PhotonTrackedTarget()
target.createFromPacket(packet)
self.targets.append(target)
self.multiTagResult = MultiTargetPNPResult()
self.multiTagResult.createFromPacket(packet)
return packet
metadata: PhotonPipelineMetadata = field(default_factory=PhotonPipelineMetadata)
multiTagResult: Optional[MultiTargetPNPResult] = None
def getLatencyMillis(self) -> float:
return (self.publishTimestampMicros - self.captureTimestampMicros) / 1e3
return (
self.metadata.publishTimestampMicros - self.metadata.captureTimestampMicros
) / 1e3
def getTimestampSeconds(self) -> float:
"""
Returns the estimated time the frame was taken, in the recieved system's time base. This is
calculated as (NT recieve time (robot base) - (publish timestamp, coproc timebase - capture
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))
"""
# TODO - we don't trust NT4 to correctly latency-compensate ntRecieveTimestampMicros
# TODO - we don't trust NT4 to correctly latency-compensate ntReceiveTimestampMicros
return (
self.ntRecieveTimestampMicros
- (self.publishTimestampMicros - self.captureTimestampMicros)
self.ntReceiveTimestampMicros
- (
self.metadata.publishTimestampMicros
- self.metadata.captureTimestampMicros
)
) / 1e6
def getTargets(self) -> list[PhotonTrackedTarget]:
return self.targets
def hasTargets(self) -> bool:
return len(self.targets) > 0
def getBestTarget(self) -> PhotonTrackedTarget:
"""
Returns the best target in this pipeline result. If there are no targets, this method will
@@ -67,5 +62,4 @@ class PhotonPipelineResult:
return None
return self.getTargets()[0]
def hasTargets(self) -> bool:
return len(self.targets) > 0
photonStruct: "PhotonPipelineResultSerde" = None

33
photon-lib/py/photonlibpy/photonTrackedTarget.py → photon-lib/py/photonlibpy/targeting/photonTrackedTarget.py
View File

@@ -1,20 +1,11 @@
from dataclasses import dataclass, field
from wpimath.geometry import Transform3d
from photonlibpy.packet import Packet
@dataclass
class TargetCorner:
x: float
y: float
from ..packet import Packet
from .TargetCorner import TargetCorner
@dataclass
class PhotonTrackedTarget:
_MAX_CORNERS = 8
_NUM_BYTES_IN_FLOAT = 8
_PACK_SIZE_BYTES = _NUM_BYTES_IN_FLOAT * (5 + 7 + 2 * 4 + 1 + 7 + 2 * _MAX_CORNERS)
yaw: float = 0.0
pitch: float = 0.0
area: float = 0.0
@@ -64,22 +55,4 @@ class PhotonTrackedTarget:
retList.append(TargetCorner(cx, cy))
return retList
def createFromPacket(self, packet: Packet) -> Packet:
self.yaw = packet.decodeDouble()
self.pitch = packet.decodeDouble()
self.area = packet.decodeDouble()
self.skew = packet.decodeDouble()
self.fiducialId = packet.decode32()
self.classId = packet.decode32()
self.objDetectConf = packet.decodeFloat()
self.bestCameraToTarget = packet.decodeTransform()
self.altCameraToTarget = packet.decodeTransform()
self.poseAmbiguity = packet.decodeDouble()
self.minAreaRectCorners = self._decodeTargetList(packet, 4) # always four
numCorners = packet.decode8()
self.detectedCorners = self._decodeTargetList(packet, numCorners)
return packet
photonStruct: "PhotonTrackedTargetSerde" = None

5
photon-lib/py/setup.py
View File

@@ -48,10 +48,7 @@ with open("photonlibpy/version.py", "w", encoding="utf-8") as fp:
fp.write(f'PHOTONVISION_VERSION="{gitDescribeResult}"\n')
descriptionStr = f"""
Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors.
Implemented with PhotonVision version {gitDescribeResult} .
"""
descriptionStr = f"Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. Implemented with PhotonVision version {gitDescribeResult} ."
setup(
name="photonlibpy",

439
photon-lib/py/test/photonPoseEstimator_test.py
View File

@@ -1,251 +1,244 @@
from photonlibpy.multiTargetPNPResult import MultiTargetPNPResult, PNPResult
from photonlibpy.photonPipelineResult import PhotonPipelineResult
from photonlibpy.photonPoseEstimator import PhotonPoseEstimator, PoseStrategy
from photonlibpy.photonTrackedTarget import PhotonTrackedTarget, TargetCorner
from robotpy_apriltag import AprilTag, AprilTagFieldLayout
from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
# 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
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(
0,
2 * 1e3,
1,
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,
),
],
)
# 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),
# )
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()
# 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)
# 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(
0,
2 * 1e3,
1,
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()))
),
)
# 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),
# )
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()
# 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)
# 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(
0,
2 * 1e3,
1,
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,
)
],
)
# 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),
# )
estimator = PhotonPoseEstimator(
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
)
# estimator = PhotonPoseEstimator(
# aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
# )
# Empty result, expect empty result
cameraOne.result = PhotonPipelineResult(
captureTimestampMicros=0, publishTimestampMicros=0, ntRecieveTimestampMicros=1e6
)
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()
# 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

26
photon-lib/py/test/photonlibpy_test.py
View File

@@ -1,3 +1,25 @@
from time import sleep
from photonlibpy import PhotonCamera
import ntcore
from photonlibpy.photonCamera import setVersionCheckEnabled
def test_roundTrip():
# TODO implement packet encoding, or just kill me
assert True
ntcore.NetworkTableInstance.getDefault().stopServer()
ntcore.NetworkTableInstance.getDefault().setServer("localhost")
ntcore.NetworkTableInstance.getDefault().startClient4("meme")
camera = PhotonCamera("WPI2024")
setVersionCheckEnabled(False)
for i in range(5):
sleep(0.1)
result = camera.getLatestResult()
print(result)
print(camera._rawBytesEntry.getTopic().getProperties())
if __name__ == "__main__":
test_roundTrip()

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

@@ -142,7 +142,7 @@ public class PhotonCamera implements AutoCloseable {
PubSubOption.periodic(0.01),
PubSubOption.sendAll(true),
PubSubOption.pollStorage(20));
resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.serde);
resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.photonStruct);
driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false);
inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
@@ -193,7 +193,7 @@ public class PhotonCamera implements AutoCloseable {
// make time sync more reliable.
for (var c : changes) {
var result = c.value;
result.setRecieveTimestampMicros(c.timestamp);
result.setReceiveTimestampMicros(c.timestamp);
ret.add(result);
}
@@ -201,7 +201,7 @@ public class PhotonCamera implements AutoCloseable {
}
/**
* Returns the latest pipeline result. This is simply the most recent result recieved via NT.
* Returns the latest pipeline result. This is simply the most recent result Received via NT.
* Calling this multiple times will always return the most recent result.
*
* <p>Replaced by {@link #getAllUnreadResults()} over getLatestResult, as this function can miss
@@ -221,7 +221,7 @@ public class PhotonCamera implements AutoCloseable {
// 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.setRecieveTimestampMicros(ret.timestamp);
result.setReceiveTimestampMicros(ret.timestamp);
return result;
}
@@ -411,9 +411,20 @@ public class PhotonCamera implements AutoCloseable {
"PhotonVision coprocessor at path " + path + " is not sending new data.", true);
}
// Check for version. Warn if the versions aren't aligned.
String versionString = versionEntry.get("");
if (!versionString.isEmpty() && !PhotonVersion.versionMatches(versionString)) {
// Check mdef UUID
String local_uuid = PhotonPipelineResult.photonStruct.getInterfaceUUID();
String remote_uuid = resultSubscriber.getInterfaceUUID();
if (remote_uuid == null || remote_uuid.isEmpty()) {
// not connected yet?
DriverStation.reportWarning(
"PhotonVision coprocessor at path "
+ path
+ " has note reported a message interface UUID - is your coprocessor's camera started?",
true);
} else if (!local_uuid.equals(remote_uuid)) {
// Error on a verified version mismatch
// But stay silent otherwise
@@ -439,8 +450,14 @@ public class PhotonCamera implements AutoCloseable {
var versionMismatchMessage =
"Photon version "
+ PhotonVersion.versionString
+ " (message definition version "
+ local_uuid
+ ")"
+ " does not match coprocessor version "
+ versionString
+ " (message definition version "
+ remote_uuid
+ ")"
+ "!";
DriverStation.reportError(versionMismatchMessage, false);
throw new UnsupportedOperationException(versionMismatchMessage);

8
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -394,8 +394,8 @@ public class PhotonPoseEstimator {
}
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
if (result.getMultiTagResult().estimatedPose.isPresent) {
var best_tf = result.getMultiTagResult().estimatedPose.best;
if (result.getMultiTagResult().isPresent()) {
var best_tf = result.getMultiTagResult().get().estimatedPose.best;
var best =
new Pose3d()
.plus(best_tf) // field-to-camera
@@ -427,11 +427,11 @@ public class PhotonPoseEstimator {
VisionEstimation.estimateCamPosePNP(
cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResult.isPresent)
if (!pnpResult.isPresent())
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
var best =
new Pose3d()
.plus(pnpResult.best) // field-to-camera
.plus(pnpResult.get().best) // field-to-camera
.plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(

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

@@ -55,9 +55,9 @@ import org.photonvision.estimation.RotTrlTransform3d;
import org.photonvision.estimation.TargetModel;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.PnpResult;
/**
* A handle for simulating {@link PhotonCamera} values. Processing simulated targets through this
@@ -420,14 +420,15 @@ public class PhotonCameraSim implements AutoCloseable {
// projected target can't be detected, skip to next
if (!(canSeeCorners(noisyTargetCorners) && areaPercent >= minTargetAreaPercent)) continue;
var pnpSim = new PNPResult();
var pnpSim = new PnpResult();
if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP
pnpSim =
OpenCVHelp.solvePNP_SQUARE(
prop.getIntrinsics(),
prop.getDistCoeffs(),
tgt.getModel().vertices,
noisyTargetCorners);
OpenCVHelp.solvePNP_SQPNP(
prop.getIntrinsics(),
prop.getDistCoeffs(),
tgt.getModel().vertices,
noisyTargetCorners)
.get();
}
detectableTgts.add(
@@ -519,13 +520,13 @@ public class PhotonCameraSim implements AutoCloseable {
} else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose);
// calculate multitag results
var multitagResult = new MultiTargetPNPResult();
Optional<MultiTargetPNPResult> multitagResult = Optional.empty();
// TODO: Implement ATFL subscribing in backend
// var tagLayout = cam.getAprilTagFieldLayout();
var visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(detectableTgts, tagLayout);
if (visibleLayoutTags.size() > 1) {
List<Integer> usedIDs =
visibleLayoutTags.stream().map(t -> t.ID).sorted().collect(Collectors.toList());
List<Short> usedIDs =
visibleLayoutTags.stream().map(t -> (short) t.ID).sorted().collect(Collectors.toList());
var pnpResult =
VisionEstimation.estimateCamPosePNP(
prop.getIntrinsics(),
@@ -533,7 +534,10 @@ public class PhotonCameraSim implements AutoCloseable {
detectableTgts,
tagLayout,
TargetModel.kAprilTag36h11);
multitagResult = new MultiTargetPNPResult(pnpResult, usedIDs);
if (pnpResult.isPresent()) {
multitagResult = Optional.of(new MultiTargetPNPResult(pnpResult.get(), usedIDs));
}
}
// sort target order
@@ -550,7 +554,7 @@ public class PhotonCameraSim implements AutoCloseable {
now,
detectableTgts,
multitagResult);
ret.setRecieveTimestampMicros(now);
ret.setReceiveTimestampMicros(now);
return ret;
}
@@ -573,9 +577,10 @@ public class PhotonCameraSim implements AutoCloseable {
* @param receiveTimestamp The (sim) timestamp when this result was read by NT in microseconds
*/
public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimestamp) {
ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp);
ts.latencyMillisEntry.set(result.metadata.getLatencyMillis(), receiveTimestamp);
ts.resultPublisher.set(result, result.getPacketSize());
// Results are now dynamically sized, so let's guess 1024 bytes is big enough
ts.resultPublisher.set(result, 1024);
boolean hasTargets = result.hasTargets();
ts.hasTargetEntry.set(hasTargets, receiveTimestamp);

64
photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
View File

@@ -35,6 +35,7 @@
#include <frc/Timer.h>
#include <opencv2/core.hpp>
#include <opencv2/core/mat.hpp>
#include <wpi/json.h>
#include "PhotonVersion.h"
#include "photon/dataflow/structures/Packet.h"
@@ -121,20 +122,18 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
// Prints warning if not connected
VerifyVersion();
// Create the new result;
PhotonPipelineResult result;
// Fill the packet with latest data and populate result.
units::microsecond_t now =
units::microsecond_t(frc::RobotController::GetFPGATime());
const auto value = rawBytesEntry.Get();
if (!value.size()) return result;
if (!value.size()) return PhotonPipelineResult{};
photon::Packet packet{value};
packet >> result;
// Create the new result;
PhotonPipelineResult result = packet.Unpack<PhotonPipelineResult>();
result.SetRecieveTimestamp(now);
result.SetReceiveTimestamp(now);
return result;
}
@@ -149,8 +148,9 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
const auto changes = rawBytesEntry.ReadQueue();
// Create the new result list -- these will be updated in-place
std::vector<PhotonPipelineResult> ret(changes.size());
// Create the new result list
std::vector<PhotonPipelineResult> ret;
ret.reserve(changes.size());
for (size_t i = 0; i < changes.size(); i++) {
const nt::Timestamped<std::vector<uint8_t>>& value = changes[i];
@@ -161,13 +161,14 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
// Fill the packet with latest data and populate result.
photon::Packet packet{value.value};
auto result = packet.Unpack<PhotonPipelineResult>();
PhotonPipelineResult& result = ret[i];
packet >> result;
// 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.
result.SetRecieveTimestamp(units::microsecond_t(value.time) -
result.SetReceiveTimestamp(units::microsecond_t(value.time) -
result.GetLatency());
ret.push_back(result);
}
return ret;
@@ -209,9 +210,11 @@ std::optional<PhotonCamera::CameraMatrix> PhotonCamera::GetCameraMatrix() {
auto camCoeffs = cameraIntrinsicsSubscriber.Get();
if (camCoeffs.size() == 9) {
PhotonCamera::CameraMatrix retVal =
Eigen::Map<const PhotonCamera::CameraMatrix>(camCoeffs.data());
Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
camCoeffs.data());
return retVal;
}
return std::nullopt;
}
@@ -230,22 +233,6 @@ std::optional<PhotonCamera::DistortionMatrix> PhotonCamera::GetDistCoeffs() {
return std::nullopt;
}
static bool VersionMatches(std::string them_str) {
std::smatch match;
std::regex versionPattern{"v[0-9]+.[0-9]+.[0-9]+"};
std::string us_str = PhotonVersion::versionString;
// Check that both versions are in the right format
if (std::regex_search(us_str, match, versionPattern) &&
std::regex_search(them_str, match, versionPattern)) {
// If they are, check string equality
return (us_str == them_str);
} else {
return false;
}
}
void PhotonCamera::VerifyVersion() {
if (!PhotonCamera::VERSION_CHECK_ENABLED) {
return;
@@ -282,13 +269,20 @@ void PhotonCamera::VerifyVersion() {
"Found the following PhotonVision cameras on NetworkTables:{}",
cameraNameOutString);
}
} else if (!VersionMatches(versionString)) {
FRC_ReportError(frc::warn::Warning, bfw);
std::string error_str = fmt::format(
"Photonlib version {} does not match coprocessor version {}!",
PhotonVersion::versionString, versionString);
FRC_ReportError(frc::err::Error, "{}", error_str);
throw std::runtime_error(error_str);
} else {
std::string local_uuid{SerdeType<PhotonPipelineResult>::GetSchemaHash()};
std::string remote_uuid =
rawBytesEntry.GetTopic().GetProperty("message_uuid");
if (local_uuid != remote_uuid) {
FRC_ReportError(frc::warn::Warning, bfw);
std::string error_str = fmt::format(
"Photonlib version {} (message definition version {}) does not match "
"coprocessor version {} (message definition version {})!",
PhotonVersion::versionString, local_uuid, versionString, remote_uuid);
FRC_ReportError(frc::err::Error, "{}", error_str);
throw std::runtime_error(error_str);
}
}
}

17
photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
View File

@@ -100,6 +100,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
FRC_ReportError(frc::warn::Warning,
"Result timestamp was reported in the past!");
return std::nullopt;
}
@@ -164,7 +166,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
}
if (ret) {
lastPose = ret.value().estimatedPose;
lastPose = ret->estimatedPose;
}
return ret;
}
@@ -197,8 +199,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
}
return EstimatedRobotPose{
fiducialPose.value()
.TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
fiducialPose->TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY};
}
@@ -220,7 +221,7 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
target.GetFiducialId());
continue;
}
frc::Pose3d const targetPose = fiducialPose.value();
frc::Pose3d const targetPose = *fiducialPose;
units::meter_t const alternativeDifference = units::math::abs(
m_robotToCamera.Z() -
@@ -349,8 +350,8 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
PhotonPipelineResult result) {
if (result.MultiTagResult().result.isPresent) {
const auto field2camera = result.MultiTagResult().result.best;
if (result.MultiTagResult()) {
const auto field2camera = result.MultiTagResult()->estimatedPose.best;
const auto fieldToRobot =
frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
@@ -398,8 +399,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
tagCorners.has_value()) {
auto const targetCorners = target.GetDetectedCorners();
for (size_t cornerIdx = 0; cornerIdx < 4; ++cornerIdx) {
imagePoints.emplace_back(targetCorners[cornerIdx].first,
targetCorners[cornerIdx].second);
imagePoints.emplace_back(targetCorners[cornerIdx].x,
targetCorners[cornerIdx].y);
objectPoints.emplace_back((*tagCorners)[cornerIdx]);
}
}

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

@@ -201,33 +201,34 @@ PhotonPipelineResult PhotonCameraSim::Process(
continue;
}
PNPResult pnpSim{};
std::optional<photon::PnpResult> pnpSim = std::nullopt;
if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) {
pnpSim = OpenCVHelp::SolvePNP_Square(
pnpSim = OpenCVHelp::SolvePNP_SQPNP(
prop.GetIntrinsics(), prop.GetDistCoeffs(),
tgt.GetModel().GetVertices(), noisyTargetCorners);
}
std::vector<std::pair<float, float>> tempCorners =
OpenCVHelp::PointsToCorners(minAreaRectPts);
wpi::SmallVector<std::pair<double, double>, 4> smallVec;
std::vector<TargetCorner> smallVec;
for (const auto& corner : tempCorners) {
smallVec.emplace_back(std::make_pair(static_cast<double>(corner.first),
static_cast<double>(corner.second)));
smallVec.emplace_back(static_cast<double>(corner.first),
static_cast<double>(corner.second));
}
std::vector<std::pair<float, float>> cornersFloat =
OpenCVHelp::PointsToCorners(noisyTargetCorners);
auto cornersFloat = OpenCVHelp::PointsToTargetCorners(noisyTargetCorners);
std::vector<std::pair<double, double>> cornersDouble{cornersFloat.begin(),
cornersFloat.end()};
std::vector<TargetCorner> cornersDouble{cornersFloat.begin(),
cornersFloat.end()};
detectableTgts.emplace_back(
-centerRot.Z().convert<units::degrees>().to<double>(),
-centerRot.Y().convert<units::degrees>().to<double>(), areaPercent,
centerRot.X().convert<units::degrees>().to<double>(), tgt.fiducialId,
tgt.objDetClassId, tgt.objDetConf, pnpSim.best, pnpSim.alt,
pnpSim.ambiguity, smallVec, cornersDouble);
tgt.objDetClassId, tgt.objDetConf,
pnpSim ? pnpSim->best : frc::Transform3d{},
pnpSim ? pnpSim->alt : frc::Transform3d{},
pnpSim ? pnpSim->ambiguity : -1, smallVec, cornersDouble);
}
if (videoSimRawEnabled) {
@@ -275,36 +276,27 @@ PhotonPipelineResult PhotonCameraSim::Process(
cv::LINE_AA);
for (const auto& tgt : detectableTgts) {
auto detectedCornersDouble = tgt.GetDetectedCorners();
std::vector<std::pair<float, float>> detectedCornerFloat{
detectedCornersDouble.begin(), detectedCornersDouble.end()};
if (tgt.GetFiducialId() >= 0) {
VideoSimUtil::DrawTagDetection(
tgt.GetFiducialId(),
OpenCVHelp::CornersToPoints(detectedCornerFloat),
OpenCVHelp::CornersToPoints(detectedCornersDouble),
videoSimFrameProcessed);
} else {
cv::rectangle(videoSimFrameProcessed,
OpenCVHelp::GetBoundingRect(
OpenCVHelp::CornersToPoints(detectedCornerFloat)),
OpenCVHelp::CornersToPoints(detectedCornersDouble)),
cv::Scalar{0, 0, 255},
static_cast<int>(VideoSimUtil::GetScaledThickness(
1, videoSimFrameProcessed)),
cv::LINE_AA);
wpi::SmallVector<std::pair<double, double>, 4> smallVec =
tgt.GetMinAreaRectCorners();
auto smallVec = tgt.GetMinAreaRectCorners();
std::vector<std::pair<float, float>> cornersCopy{};
cornersCopy.reserve(4);
for (const auto& corner : smallVec) {
cornersCopy.emplace_back(
std::make_pair(static_cast<float>(corner.first),
static_cast<float>(corner.second)));
}
VideoSimUtil::DrawPoly(
OpenCVHelp::CornersToPoints(cornersCopy),
OpenCVHelp::CornersToPoints(smallVec),
static_cast<int>(
VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
cv::Scalar{255, 30, 30}, true, videoSimFrameProcessed);
@@ -316,75 +308,81 @@ PhotonPipelineResult PhotonCameraSim::Process(
cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
}
MultiTargetPNPResult multiTagResults{};
std::optional<MultiTargetPNPResult> multiTagResults = std::nullopt;
std::vector<frc::AprilTag> visibleLayoutTags =
VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout);
if (visibleLayoutTags.size() > 1) {
wpi::SmallVector<int16_t, 32> usedIds{};
std::vector<int16_t> usedIds{};
usedIds.resize(visibleLayoutTags.size());
std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(),
usedIds.begin(),
[](const frc::AprilTag& tag) { return tag.ID; });
std::sort(usedIds.begin(), usedIds.end());
PNPResult pnpResult = VisionEstimation::EstimateCamPosePNP(
auto pnpResult = VisionEstimation::EstimateCamPosePNP(
prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout,
kAprilTag36h11);
multiTagResults = MultiTargetPNPResult{pnpResult, usedIds};
if (pnpResult) {
multiTagResults = MultiTargetPNPResult{*pnpResult, usedIds};
}
}
heartbeatCounter++;
return PhotonPipelineResult{heartbeatCounter, 0_s, latency, detectableTgts,
multiTagResults};
return PhotonPipelineResult{
PhotonPipelineMetadata{heartbeatCounter, 0,
units::microsecond_t{latency}.to<int64_t>()},
detectableTgts, multiTagResults};
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) {
SubmitProcessedFrame(result, wpi::Now());
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
uint64_t recieveTimestamp) {
uint64_t ReceiveTimestamp) {
ts.latencyMillisEntry.Set(
result.GetLatency().convert<units::milliseconds>().to<double>(),
recieveTimestamp);
ReceiveTimestamp);
Packet newPacket{};
newPacket << result;
newPacket.Pack(result);
ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp);
ts.rawBytesEntry.Set(newPacket.GetData(), ReceiveTimestamp);
bool hasTargets = result.HasTargets();
ts.hasTargetEntry.Set(hasTargets, recieveTimestamp);
ts.hasTargetEntry.Set(hasTargets, ReceiveTimestamp);
if (!hasTargets) {
ts.targetPitchEntry.Set(0.0, recieveTimestamp);
ts.targetYawEntry.Set(0.0, recieveTimestamp);
ts.targetAreaEntry.Set(0.0, recieveTimestamp);
ts.targetPitchEntry.Set(0.0, ReceiveTimestamp);
ts.targetYawEntry.Set(0.0, ReceiveTimestamp);
ts.targetAreaEntry.Set(0.0, ReceiveTimestamp);
std::array<double, 3> poseData{0.0, 0.0, 0.0};
ts.targetPoseEntry.Set(poseData, recieveTimestamp);
ts.targetSkewEntry.Set(0.0, recieveTimestamp);
ts.targetPoseEntry.Set(poseData, ReceiveTimestamp);
ts.targetSkewEntry.Set(0.0, ReceiveTimestamp);
} else {
PhotonTrackedTarget bestTarget = result.GetBestTarget();
ts.targetPitchEntry.Set(bestTarget.GetPitch(), recieveTimestamp);
ts.targetYawEntry.Set(bestTarget.GetYaw(), recieveTimestamp);
ts.targetAreaEntry.Set(bestTarget.GetArea(), recieveTimestamp);
ts.targetSkewEntry.Set(bestTarget.GetSkew(), recieveTimestamp);
ts.targetPitchEntry.Set(bestTarget.GetPitch(), ReceiveTimestamp);
ts.targetYawEntry.Set(bestTarget.GetYaw(), ReceiveTimestamp);
ts.targetAreaEntry.Set(bestTarget.GetArea(), ReceiveTimestamp);
ts.targetSkewEntry.Set(bestTarget.GetSkew(), ReceiveTimestamp);
frc::Transform3d transform = bestTarget.GetBestCameraToTarget();
std::array<double, 4> poseData{
transform.X().to<double>(), transform.Y().to<double>(),
transform.Rotation().ToRotation2d().Degrees().to<double>()};
ts.targetPoseEntry.Set(poseData, recieveTimestamp);
ts.targetPoseEntry.Set(poseData, ReceiveTimestamp);
}
auto intrinsics = prop.GetIntrinsics();
std::vector<double> intrinsicsView{intrinsics.data(),
intrinsics.data() + intrinsics.size()};
ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp);
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> intrinsics =
prop.GetIntrinsics();
std::span<double> intrinsicsView{intrinsics.data(),
intrinsics.data() + intrinsics.size()};
ts.cameraIntrinsicsPublisher.Set(intrinsicsView, ReceiveTimestamp);
auto distortion = prop.GetDistCoeffs();
std::vector<double> distortionView{distortion.data(),
distortion.data() + distortion.size()};
ts.cameraDistortionPublisher.Set(distortionView, recieveTimestamp);
ts.cameraDistortionPublisher.Set(distortionView, ReceiveTimestamp);
ts.heartbeatPublisher.Set(heartbeatCounter, recieveTimestamp);
ts.heartbeatPublisher.Set(heartbeatCounter, ReceiveTimestamp);
ts.subTable->GetInstance().Flush();
}

2
photon-lib/src/main/native/include/photon/PhotonCamera.h
View File

@@ -39,7 +39,7 @@
#include <networktables/StringTopic.h>
#include <units/time.h>
#include "photon/targeting//PhotonPipelineResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
namespace cv {
class Mat;

2
photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
View File

@@ -34,9 +34,9 @@
#include "photon/PhotonCamera.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PnpResult.h"
namespace photon {
enum PoseStrategy {

2
photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h
View File

@@ -28,9 +28,9 @@
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PnpResult.h"
namespace photon {

2
photon-lib/src/main/native/include/photon/PhotonUtils.h
View File

@@ -34,9 +34,9 @@
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PnpResult.h"
namespace photon {
class PhotonUtils {

2
photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h
View File

@@ -91,7 +91,7 @@ class PhotonCameraSim {
void SubmitProcessedFrame(const PhotonPipelineResult& result);
void SubmitProcessedFrame(const PhotonPipelineResult& result,
uint64_t recieveTimestamp);
uint64_t ReceiveTimestamp);
SimCameraProperties prop;

12
photon-lib/src/test/java/org/photonvision/OpenCVTest.java
View File

@@ -177,7 +177,11 @@ public class OpenCVTest {
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices());
var pnpSim =
OpenCVHelp.solvePNP_SQUARE(
prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners);
prop.getIntrinsics(),
prop.getDistCoeffs(),
target.getModel().vertices,
targetCorners)
.get();
// check solvePNP estimation accuracy
assertSame(relTarget.getRotation(), pnpSim.best.getRotation());
@@ -212,7 +216,11 @@ public class OpenCVTest {
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices());
var pnpSim =
OpenCVHelp.solvePNP_SQPNP(
prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners);
prop.getIntrinsics(),
prop.getDistCoeffs(),
target.getModel().vertices,
targetCorners)
.get();
// check solvePNP estimation accuracy
assertSame(relTarget.getRotation(), pnpSim.best.getRotation());

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

@@ -37,10 +37,7 @@ class PhotonCameraTest {
var packet = new Packet(1);
var ret = new PhotonPipelineResult();
packet.setData(new byte[0]);
if (packet.getSize() < 1) {
return;
}
PhotonPipelineResult.serde.pack(packet, ret);
PhotonPipelineResult.photonStruct.pack(packet, ret);
});
}
}

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

@@ -130,7 +130,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setRecieveTimestampMicros((long) (11 * 1e6));
cameraOne.result.setReceiveTimestampMicros((long) (11 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
@@ -217,7 +217,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setRecieveTimestampMicros((long) (4 * 1e6));
cameraOne.result.setReceiveTimestampMicros((long) (4 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
@@ -306,7 +306,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setRecieveTimestampMicros((long) (17 * 1e6));
cameraOne.result.setReceiveTimestampMicros((long) (17 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
@@ -396,7 +396,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setRecieveTimestampMicros((long) (1 * 1e6));
cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
@@ -478,7 +478,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setRecieveTimestampMicros((long) (7 * 1e6));
cameraOne.result.setReceiveTimestampMicros((long) (7 * 1e6));
estimatedPose = estimator.update(cameraOne.result);
pose = estimatedPose.get().estimatedPose;
@@ -519,7 +519,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
result.setRecieveTimestampMicros((long) (20 * 1e6));
result.setReceiveTimestampMicros((long) (20 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
@@ -529,7 +529,7 @@ class PhotonPoseEstimatorTest {
// Empty result, expect empty result
cameraOne.result = new PhotonPipelineResult();
cameraOne.result.setRecieveTimestampMicros((long) (1 * 1e6));
cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6));
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
@@ -629,7 +629,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))))); // 3 3 3 ambig .4
cameraOne.result.setRecieveTimestampMicros(20 * 1000000);
cameraOne.result.setReceiveTimestampMicros(20 * 1000000);
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(

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

@@ -481,11 +481,12 @@ class VisionSystemSimTest {
visionSysSim.update(robotPose);
var results =
VisionEstimation.estimateCamPosePNP(
camera.getCameraMatrix().get(),
camera.getDistCoeffs().get(),
camera.getLatestResult().getTargets(),
layout,
TargetModel.kAprilTag16h5);
camera.getCameraMatrix().get(),
camera.getDistCoeffs().get(),
camera.getLatestResult().getTargets(),
layout,
TargetModel.kAprilTag16h5)
.get();
Pose3d pose = new Pose3d().plus(results.best);
assertEquals(5, pose.getX(), .01);
assertEquals(1, pose.getY(), .01);
@@ -500,11 +501,12 @@ class VisionSystemSimTest {
visionSysSim.update(robotPose);
results =
VisionEstimation.estimateCamPosePNP(
camera.getCameraMatrix().get(),
camera.getDistCoeffs().get(),
camera.getLatestResult().getTargets(),
layout,
TargetModel.kAprilTag16h5);
camera.getCameraMatrix().get(),
camera.getDistCoeffs().get(),
camera.getLatestResult().getTargets(),
layout,
TargetModel.kAprilTag16h5)
.get();
pose = new Pose3d().plus(results.best);
assertEquals(5, pose.getX(), .01);
assertEquals(1, pose.getY(), .01);

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

@@ -39,9 +39,9 @@
#include "photon/PhotonPoseEstimator.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PnpResult.h"
static std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
@@ -51,31 +51,33 @@ static std::vector<frc::AprilTag> tags = {
static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};
static wpi::SmallVector<std::pair<double, double>, 4> corners{
std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
static std::vector<std::pair<double, double>> detectedCorners{
std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
static std::vector<photon::TargetCorner> corners{
photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}};
static std::vector<photon::TargetCorner> detectedCorners{
photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
photon::TargetCorner{5., 6.}, photon::TargetCorner{7., 8.}};
TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1,
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
@@ -83,8 +85,9 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(11));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
@@ -93,6 +96,7 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -118,23 +122,23 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
// ID 0 at 3,3,3
// ID 1 at 5,5,5
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1,
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
@@ -142,8 +146,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(17_s);
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(17_s);
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}});
@@ -152,6 +157,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -165,23 +171,23 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1,
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
@@ -189,8 +195,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags,
photon::CLOSEST_TO_REFERENCE_POSE, {});
@@ -202,6 +209,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -214,23 +222,23 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1,
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
@@ -238,8 +246,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
{});
@@ -254,31 +263,32 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targetsThree{
std::vector<photon::PhotonTrackedTarget> targetsThree{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1,
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 0, -1, -1,
3.0, -4.0, 9.1, 6.7, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.testResult = {{0, 0_s, 2_ms, targetsThree}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(21));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targetsThree, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21));
// std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
@@ -298,23 +308,23 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1,
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
@@ -322,8 +332,9 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
{});
@@ -333,6 +344,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -345,23 +357,23 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
TEST(PhotonPoseEstimatorTest, PoseCache) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test2");
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1,
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
@@ -374,8 +386,10 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
{});
// empty input, expect empty out
cameraOne.testResult = {{0, 0_s, 2_ms, {}}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(1));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000},
std::vector<photon::PhotonTrackedTarget>{}, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
@@ -385,14 +399,15 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
EXPECT_FALSE(estimatedPose);
// Set result, and update -- expect present and timestamp to be 15
cameraOne.testResult = {{0, 0_s, 3_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15));
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 3000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_TRUE(estimatedPose);
ASSERT_TRUE(estimatedPose);
EXPECT_NEAR((15_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
@@ -403,3 +418,15 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
EXPECT_FALSE(estimatedPose);
}
TEST(PhotonPoseEstimatorTest, CopyResult) {
std::vector<photon::PhotonTrackedTarget> targets{};
auto testResult = photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt};
testResult.SetReceiveTimestamp(units::second_t(11));
auto test2 = testResult;
EXPECT_NEAR(testResult.GetTimestamp().to<double>(),
test2.GetTimestamp().to<double>(), 0.001);
}

14
photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
View File

@@ -439,9 +439,10 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
for (photon::PhotonTrackedTarget tar : targetSpan) {
targets.push_back(tar);
}
photon::PNPResult results = photon::VisionEstimation::EstimateCamPosePNP(
auto results = photon::VisionEstimation::EstimateCamPosePNP(
camEigen, distEigen, targets, layout, photon::kAprilTag16h5);
frc::Pose3d pose = frc::Pose3d{} + results.best;
ASSERT_TRUE(results);
frc::Pose3d pose = frc::Pose3d{} + results->best;
ASSERT_NEAR(5, pose.X().to<double>(), 0.01);
ASSERT_NEAR(1, pose.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose.Z().to<double>(), 0.01);
@@ -460,11 +461,12 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
for (photon::PhotonTrackedTarget tar : targetSpan2) {
targets2.push_back(tar);
}
photon::PNPResult results2 = photon::VisionEstimation::EstimateCamPosePNP(
auto results2 = photon::VisionEstimation::EstimateCamPosePNP(
camEigen, distEigen, targets2, layout, photon::kAprilTag16h5);
frc::Pose3d pose2 = frc::Pose3d{} + results2.best;
ASSERT_NEAR(5, pose2.X().to<double>(), 0.01);
ASSERT_NEAR(1, pose2.Y().to<double>(), 0.01);
ASSERT_TRUE(results2);
frc::Pose3d pose2 = frc::Pose3d{} + results2->best;
ASSERT_NEAR(robotPose.X().to<double>(), pose2.X().to<double>(), 0.01);
ASSERT_NEAR(robotPose.Y().to<double>(), pose2.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose2.Z().to<double>(), 0.01);
ASSERT_NEAR(units::degree_t{5}.convert<units::radians>().to<double>(),
pose2.Rotation().Z().to<double>(), 0.01);

24
photon-serde/README.md Normal file
View File

@@ -0,0 +1,24 @@
# Photon Serde Autocode
Like Rosmsg. But worse.
![](https://private-user-images.githubusercontent.com/29715865/350732914-ab8026ad-2861-49ad-b5b2-0fe7cf920d44.png?jwt=eyJhbGciOiJIUzI1NiIsInR5cCI6IkpXVCJ9.eyJpc3MiOiJnaXRodWIuY29tIiwiYXVkIjoicmF3LmdpdGh1YnVzZXJjb250ZW50LmNvbSIsImtleSI6ImtleTUiLCJleHAiOjE3MjIyMjY1NTIsIm5iZiI6MTcyMjIyNjI1MiwicGF0aCI6Ii8yOTcxNTg2NS8zNTA3MzI5MTQtYWI4MDI2YWQtMjg2MS00OWFkLWI1YjItMGZlN2NmOTIwZDQ0LnBuZz9YLUFtei1BbGdvcml0aG09QVdTNC1ITUFDLVNIQTI1NiZYLUFtei1DcmVkZW50aWFsPUFLSUFWQ09EWUxTQTUzUFFLNFpBJTJGMjAyNDA3MjklMkZ1cy1lYXN0LTElMkZzMyUyRmF3czRfcmVxdWVzdCZYLUFtei1EYXRlPTIwMjQwNzI5VDA0MTA1MlomWC1BbXotRXhwaXJlcz0zMDAmWC1BbXotU2lnbmF0dXJlPWI2YmQwZDQ3ZGQ3ODc5NWE0YTRhYTJkMmVmNmU4MTY2M2RiZTQ4NDIwNzQyMDdiOWJkZmMxNzQxNTgwYjE2MDYmWC1BbXotU2lnbmVkSGVhZGVycz1ob3N0JmFjdG9yX2lkPTAma2V5X2lkPTAmcmVwb19pZD0wIn0.dhfk3QkC04gIF_MKxFGKaYUNY__AmhB6wMHSZsQadZ4)
## Goals
- As fast as possible (only slightly slower than packed structs, ideally)
- Support for variable length arrays and optional types
- Allow deserialization into user-defined, possibly nested, types. See [ResultList](src/targeting/resultlist.h) for an example of this.
## Design
The code for a single type is split across 3 files. Let's look at PnpResult:
- [The struct definition](src/struct/pnpresult_struct.h): This is the data the object holds. Auto-generated. The data this object holds can be primitives or other, fully-deserialized types (like Vec2)
- [The user class](src/targeting/pnpresult_struct.h): This is the fully-deserialized PnpResult type. This contains extra functions users might need to expose like `Amgiguity`, or other computed helper things.
- [The serde interface](src/serde/pnpresult_struct.h): This is a template specilization for converting the user class to/from bytes
## Prior art
- Protobuf: slow on embedded platforms (at least quickbuf is)
- Wpi's struct: no VLAs/optionals
- Rosmsg: I'm not using ros, but I'm stealing their message hash idea

314
photon-serde/generate_messages.py Normal file
View File

@@ -0,0 +1,314 @@
#!/usr/bin/env python3
###############################################################################
## 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/>.
###############################################################################
import argparse
import copy
import hashlib
import os
import sys
from pathlib import Path
from typing import List, TypedDict, cast
import yaml
from jinja2 import Environment, FileSystemLoader
class SerdeField(TypedDict):
name: str
type: str
# optional extra args
optional: bool
vla: bool
class MessageType(TypedDict):
name: str
fields: List[SerdeField]
# will be 'shim' if shimmed, and the shims will be set
shimmed: bool
java_decode_shim: str
java_encode_shim: str
# C++ helpers
cpp_include: str
# python shim types
python_decode_shim: str
def yaml_to_dict(path: str):
script_dir = os.path.dirname(os.path.abspath(__file__))
yaml_file_path = os.path.join(script_dir, path)
with open(yaml_file_path, "r") as file:
file_dict: dict = yaml.safe_load(file)
return file_dict
data_types = yaml_to_dict("message_data_types.yaml")
# Helper to check if we need to use our own decoder
def is_intrinsic_type(type_str: str):
ret = type_str in data_types.keys()
return ret
# Deal with shimmed types
def get_shimmed_filter(message_db):
def is_shimmed(message_name: str):
# We don't (yet) support shimming intrinsic types
if is_intrinsic_type(message_name):
return False
message = get_message_by_name(message_db, message_name)
return "shimmed" in message and message["shimmed"] == True
return is_shimmed
def get_qualified_cpp_name(
message_db: List[MessageType], data_types, field: SerdeField
):
"""
Get the full name of the type encoded. Eg:
std::optional<photon::TargetCorner>
std::array<frc::Transform3d>
"""
if get_shimmed_filter(message_db)(field["type"]):
base_type = get_message_by_name(message_db, field["type"])["cpp_type"]
else:
base_type = data_types[field["type"]]["cpp_type"]
if "optional" in field and field["optional"] == True:
typestr = f"std::optional<{base_type}>"
elif "vla" in field and field["vla"] == True:
typestr = f"std::vector<{base_type}>"
else:
typestr = base_type
return typestr
def get_message_by_name(message_db: List[MessageType], message_name: str):
try:
return next(
message for message in message_db if message["name"] == message_name
)
except StopIteration as e:
raise Exception("Could not find " + message_name) from e
def get_field_by_name(message: MessageType, field_name: str):
return next(f for f in message["fields"] if f["name"] == field_name)
def get_message_hash(message_db: List[MessageType], message: MessageType):
"""
Calculate a unique message hash via MD5 sum. This is a very similar approach to rosmsg, documented:
http://wiki.ros.org/ROS/Technical%20Overview#Message_serialization_and_msg_MD5_sums
For non-intrinsic (user-defined) types, replace its type-string with the md5sum of the submessage definition
"""
# replace the non-intrinsic typename with its hash
modified_message = copy.deepcopy(message)
fields_to_hash = [
field
for field in modified_message["fields"]
if not is_intrinsic_type(field["type"])
]
for field in fields_to_hash:
sub_message = get_message_by_name(message_db, field["type"])
subhash = get_message_hash(message_db, sub_message)
# change the type to be our new md5sum
field["type"] = subhash.hexdigest()
# base case: message is all intrinsic types
# Hash a comments-stripped version for message integrity checking
cleaned_yaml = yaml.dump(modified_message, default_flow_style=False).strip()
message_hash = hashlib.md5(cleaned_yaml.encode("ascii"))
return message_hash
def get_includes(db, message: MessageType) -> str:
includes = []
for field in message["fields"]:
if not is_intrinsic_type(field["type"]):
field_msg = get_message_by_name(db, field["type"])
if "shimmed" in field_msg and field_msg["shimmed"] == True:
includes.append(field_msg["cpp_include"])
else:
# must be a photon type.
includes.append(f"\"photon/targeting/{field_msg['name']}.h\"")
if "optional" in field and field["optional"] == True:
includes.append("<optional>")
if "vla" in field and field["vla"] == True:
includes.append("<vector>")
# stdint types
includes.append("<stdint.h>")
return sorted(set(includes))
def parse_yaml():
Path(__file__).resolve().parent
config = yaml_to_dict("messages.yaml")
return config
def get_struct_schema_str(message: MessageType):
ret = ""
for field in message["fields"]:
typestr = field["type"]
if "optional" in field and field["optional"] == True:
typestr += "?"
if "vla" in field and field["vla"] == True:
typestr += "[?]"
ret += f"{typestr} {field['name']};"
return ret
def generate_photon_messages(cpp_java_root, py_root, template_root):
messages = parse_yaml()
env = Environment(
loader=FileSystemLoader(str(template_root)),
# autoescape=False,
# keep_trailing_newline=False,
)
env.filters["is_intrinsic"] = is_intrinsic_type
env.filters["is_shimmed"] = get_shimmed_filter(messages)
# add our custom types
extended_data_types = data_types.copy()
for message in messages:
name = message["name"]
extended_data_types[name] = {
"len": -1,
"java_type": name,
"cpp_type": "photon::" + name,
}
java_output_dir = Path(cpp_java_root) / "main/java/org/photonvision/struct"
java_output_dir.mkdir(parents=True, exist_ok=True)
cpp_serde_header_dir = Path(cpp_java_root) / "main/native/include/photon/serde/"
cpp_serde_header_dir.mkdir(parents=True, exist_ok=True)
cpp_serde_source_dir = Path(cpp_java_root) / "main/native/cpp/photon/serde/"
cpp_serde_source_dir.mkdir(parents=True, exist_ok=True)
cpp_struct_header_dir = Path(cpp_java_root) / "main/native/include/photon/struct/"
cpp_struct_header_dir.mkdir(parents=True, exist_ok=True)
py_serde_source_dir = Path(py_root)
py_serde_source_dir.mkdir(parents=True, exist_ok=True)
env.filters["get_qualified_name"] = lambda field: get_qualified_cpp_name(
messages, extended_data_types, field
)
for message in messages:
# don't generate shimmed types
if get_shimmed_filter(messages)(message["name"]):
continue
message = cast(MessageType, message)
java_name = f"{message['name']}Serde.java"
cpp_serde_header_name = f"{message['name']}Serde.h"
cpp_serde_source_name = f"{message['name']}Serde.cpp"
cpp_struct_header_name = f"{message['name']}Struct.h"
py_name = f"{message['name']}Serde.py"
java_template = env.get_template("Message.java.jinja")
cpp_serde_header_template = env.get_template("ThingSerde.h.jinja")
cpp_serde_source_template = env.get_template("ThingSerde.cpp.jinja")
cpp_struct_header_template = env.get_template("ThingStruct.h.jinja")
py_template = env.get_template("ThingSerde.py.jinja")
message_hash = get_message_hash(messages, message)
for output_name, template, output_folder in [
[java_name, java_template, java_output_dir],
[cpp_serde_header_name, cpp_serde_header_template, cpp_serde_header_dir],
[cpp_serde_source_name, cpp_serde_source_template, cpp_serde_source_dir],
[cpp_struct_header_name, cpp_struct_header_template, cpp_struct_header_dir],
[py_name, py_template, py_serde_source_dir],
]:
# Hack in our message getter
template.globals["get_message_by_name"] = lambda name: get_message_by_name(
messages, name
)
output_file = output_folder / output_name
output_file.write_text(
template.render(
message,
type_map=extended_data_types,
message_fmt=get_struct_schema_str(message),
message_hash=message_hash.hexdigest(),
cpp_includes=get_includes(messages, message),
),
encoding="utf-8",
)
def main(argv):
script_path = Path(__file__).resolve()
dirname = script_path.parent
parser = argparse.ArgumentParser()
parser.add_argument(
"--cpp_java_output_dir",
help="Optional. If set, will output the generated files to this directory, otherwise it will use a path relative to the script",
default=dirname.parent / "photon-targeting/src/generated",
type=Path,
)
parser.add_argument(
"--py_output_dir",
help="Optional. If set, will spit Python serde files here",
default=dirname.parent / "photon-lib/py/photonlibpy/generated",
type=Path,
)
parser.add_argument(
"--template_root",
help="Optional. If set, will use this directory as the root for the jinja templates",
default=dirname / "templates",
type=Path,
)
args = parser.parse_args(argv)
generate_photon_messages(
args.cpp_java_output_dir, args.py_output_dir, args.template_root
)
if __name__ == "__main__":
main(sys.argv[1:])

33
photon-serde/message_data_types.yaml Normal file
View File

@@ -0,0 +1,33 @@
---
bool:
# length in bytes
len: 1
java_type: bool
cpp_type: bool
java_decode_method: decodeBoolean
int16:
len: 2
java_type: short
cpp_type: int16_t
java_decode_method: decodeShort
java_list_decode_method: decodeShortList
int32:
len: 4
java_type: int
cpp_type: int32_t
java_decode_method: decodeInt
int64:
len: 8
java_type: long
cpp_type: int64_t
java_decode_method: decodeLong
float32:
len: 4
java_type: float
cpp_type: float
java_decode_method: decodeFloat
float64:
len: 8
java_type: double
cpp_type: double
java_decode_method: decodeDouble

90
photon-serde/messages.yaml Normal file
View File

@@ -0,0 +1,90 @@
---
- name: PhotonPipelineMetadata
fields:
- name: sequenceID
type: int64
- name: captureTimestampMicros
type: int64
- name: publishTimestampMicros
type: int64
- name: Transform3d
shimmed: True
java_decode_shim: PacketUtils.unpackTransform3d
java_encode_shim: PacketUtils.packTransform3d
cpp_type: frc::Transform3d
cpp_include: "<frc/geometry/Transform3d.h>"
python_decode_shim: packet.decodeTransform
# shim since we expect fields to at least exist
fields: []
- name: TargetCorner
fields:
- name: x
type: float64
- name: y
type: float64
- name: PhotonTrackedTarget
fields:
- name: yaw
type: float64
- name: pitch
type: float64
- name: area
type: float64
- name: skew
type: float64
- name: fiducialId
type: int32
- name: objDetectId
type: int32
- name: objDetectConf
type: float32
- name: bestCameraToTarget
type: Transform3d
- name: altCameraToTarget
type: Transform3d
- name: poseAmbiguity
type: float64
- name: minAreaRectCorners
type: TargetCorner
vla: True
- name: detectedCorners
type: TargetCorner
vla: True
- name: PnpResult
fields:
- name: best
type: Transform3d
comment: "This is a comment"
- name: alt
type: Transform3d
- name: bestReprojErr
type: float64
- name: altReprojErr
type: float64
- name: ambiguity
type: float64
- name: MultiTargetPNPResult
fields:
- name: estimatedPose
type: PnpResult
- name: fiducialIDsUsed
type: int16
vla: True
- name: PhotonPipelineResult
fields:
- name: metadata
type: PhotonPipelineMetadata
- name: targets
type: PhotonTrackedTarget
vla: True
- name: multitagResult
type: MultiTargetPNPResult
optional: True

103
photon-serde/templates/Message.java.jinja 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/>.
*/
// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY
package org.photonvision.struct;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.utils.PacketUtils;
// Assume that the base class lives here and we can import it
import org.photonvision.targeting.*;
/**
* Auto-generated serialization/deserialization helper for {{name}}
*/
public class {{ name }}Serde implements PacketSerde<{{name}}> {
// Message definition md5sum. See photon_packet.adoc for details
public static final String MESSAGE_VERSION = "{{ message_hash }}";
public static final String MESSAGE_FORMAT = "{{ message_fmt }}";
public final String getTypeString() { return MESSAGE_FORMAT; }
public final String getInterfaceUUID() { return MESSAGE_VERSION; }
@Override
public int getMaxByteSize() {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'");
}
@Override
public void pack(Packet packet, {{ name }} value) {
{%- for field in fields -%}
{%- if field.type | is_shimmed %}
// field is shimmed!
{{ get_message_by_name(field.type).java_encode_shim }}(packet, value.{{ field.name }});
{%- elif field.optional == True %}
// {{ field.name }} is optional! it better not be a VLA too
packet.encodeOptional(value.{{ field.name }});
{%- elif field.vla == True and field.type | is_intrinsic %}
// {{ field.name }} is a intrinsic VLA!
packet.encode(value.{{ field.name }});
{%- elif field.vla == True %}
// {{ field.name }} is a custom VLA!
packet.encodeList(value.{{ field.name }});
{%- elif field.type | is_intrinsic %}
// field {{ field.name }} is of intrinsic type {{ field.type }}
packet.encode(({{ type_map[field.type].java_type }}) value.{{ field.name }});
{%- else %}
// field {{ field.name }} is of non-intrinsic type {{ field.type }}
{{ field.type }}.photonStruct.pack(packet, value.{{ field.name }});
{%- endif %}
{%- if not loop.last %}
{% endif -%}
{% endfor%}
}
@Override
public {{ name }} unpack(Packet packet) {
var ret = new {{ name }}();
{% for field in fields -%}
{%- if field.type | is_shimmed %}
// field is shimmed!
ret.{{ field.name }} = {{ get_message_by_name(field.type).java_decode_shim }}(packet);
{%- elif field.optional == True %}
// {{ field.name }} is optional! it better not be a VLA too
ret.{{ field.name }} = packet.decodeOptional({{ field.type }}.photonStruct);
{%- elif field.vla == True and not field.type | is_intrinsic %}
// {{ field.name }} is a custom VLA!
ret.{{ field.name }} = packet.decodeList({{ field.type }}.photonStruct);
{%- elif field.vla == True and field.type | is_intrinsic %}
// {{ field.name }} is a custom VLA!
ret.{{ field.name }} = packet.decode{{ type_map[field.type].java_type.title() }}List();
{%- elif field.type | is_intrinsic %}
// {{ field.name }} is of intrinsic type {{ field.type }}
ret.{{field.name}} = packet.{{ type_map[field.type].java_decode_method }}();
{%- else %}
// {{ field.name }} is of non-intrinsic type {{ field.type }}
ret.{{field.name}} = {{ field.type }}.photonStruct.unpack(packet);
{%- endif %}
{%- if not loop.last %}
{% endif -%}
{% endfor%}
return ret;
}
}

44
photon-serde/templates/ThingSerde.cpp.jinja Normal file
View File

@@ -0,0 +1,44 @@
/*
* 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/>.
*/
// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY
#include "photon/serde/{{ name }}Serde.h"
namespace photon {
using StructType = SerdeType<{{ name }}>;
void StructType::Pack(Packet& packet, const {{ name }}& value) {
{% for field in fields -%}
packet.Pack<{{ field | get_qualified_name }}>(value.{{ field.name }});
{%- if not loop.last %}
{% endif -%}
{% endfor %}
}
{{ name }} StructType::Unpack(Packet& packet) {
return {{ name }}{ {{ name }}_PhotonStruct{
{% for field in fields -%}
.{{ field.name}} = packet.Unpack<{{ field | get_qualified_name }}>(),
{%- if not loop.last %}
{% endif -%}
{% endfor %}
}};
}
} // namespace photon

51
photon-serde/templates/ThingSerde.h.jinja Normal file
View File

@@ -0,0 +1,51 @@
/*
* 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
// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY
#include <wpi/SymbolExports.h>
// Include myself
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/{{ name }}.h"
// Includes for dependant types
{% for include in cpp_includes -%}
#include {{ include }}
{% endfor %}
namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<{{ name }}> {
static constexpr std::string_view GetSchemaHash() {
return "{{ message_hash }}";
}
static constexpr std::string_view GetSchema() {
return "{{ message_fmt }}";
}
static photon::{{ name }} Unpack(photon::Packet& packet);
static void Pack(photon::Packet& packet, const photon::{{ name }}& value);
};
static_assert(photon::PhotonStructSerializable<photon::{{ name }}>);
} // namespace photon

62
photon-serde/templates/ThingSerde.py.jinja Normal file
View File

@@ -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/>.
###############################################################################
###############################################################################
## THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py.
## --> DO NOT MODIFY <--
###############################################################################
from ..targeting import *
class {{ name }}Serde:
# Message definition md5sum. See photon_packet.adoc for details
MESSAGE_VERSION = "{{ message_hash }}"
MESSAGE_FORMAT = "{{ message_fmt }}"
@staticmethod
def unpack(packet: 'Packet') -> '{{ name }}':
ret = {{ name }}()
{% for field in fields -%}
{%- if field.type | is_shimmed %}
# field is shimmed!
ret.{{ field.name }} = {{ get_message_by_name(field.type).python_decode_shim }}()
{%- elif field.optional == True %}
# {{ field.name }} is optional! it better not be a VLA too
ret.{{ field.name }} = packet.decodeOptional({{ field.type }}.photonStruct)
{%- elif field.vla == True and not field.type | is_intrinsic %}
# {{ field.name }} is a custom VLA!
ret.{{ field.name }} = packet.decodeList({{ field.type }}.photonStruct)
{%- elif field.vla == True and field.type | is_intrinsic %}
# {{ field.name }} is a custom VLA!
ret.{{ field.name }} = packet.decode{{ type_map[field.type].java_type.title() }}List()
{%- elif field.type | is_intrinsic %}
# {{ field.name }} is of intrinsic type {{ field.type }}
ret.{{field.name}} = packet.{{ type_map[field.type].java_decode_method }}()
{%- else %}
# {{ field.name }} is of non-intrinsic type {{ field.type }}
ret.{{field.name}} = {{ field.type }}.photonStruct.unpack(packet)
{%- endif %}
{%- if not loop.last %}
{% endif -%}
{% endfor%}
return ret
# Hack ourselves into the base class
{{ name }}.photonStruct = {{ name }}Serde()

39
photon-serde/templates/ThingStruct.h.jinja Normal file
View File

@@ -0,0 +1,39 @@
/*
* 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
// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY
// Includes for dependant types
{% for include in cpp_includes -%}
#include {{ include }}
{% endfor %}
namespace photon {
struct {{ name }}_PhotonStruct {
{% for field in fields -%}
{{ field | get_qualified_name }} {{ field.name }};
{%- if not loop.last %}
{% endif -%}
{% endfor %}
friend bool operator==({{ name }}_PhotonStruct const&, {{ name }}_PhotonStruct const&) = default;
};
} // namespace photon

13
photon-targeting/build.gradle
View File

@@ -17,17 +17,19 @@ nativeUtils {
}
}
sourceSets.main.java.srcDir "${projectDir}/src/generated/main/java"
model {
components {
"${nativeName}"(NativeLibrarySpec) {
sources {
cpp {
source {
srcDirs 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp"
srcDirs 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp", 'src/generated/main/native/cpp'
include '**/*.cpp', '**/*.cc'
}
exportedHeaders {
srcDirs 'src/main/native/include', "$buildDir/generated/source/proto/main/cpp"
srcDirs 'src/main/native/include', 'src/generated/main/native/include', "$buildDir/generated/source/proto/main/cpp", 'src/generated/main/native/include'
if (project.hasProperty('generatedHeaders')) {
srcDir generatedHeaders
}
@@ -120,3 +122,10 @@ model {
}
apply from: "${rootDir}/shared/javacpp/publish.gradle"
// Add photon serde headers to our published sources
cppHeadersZip {
from('src/generated/main/native/include') {
into '/'
}
}

68
photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java 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/>.
*/
// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY
package org.photonvision.struct;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.utils.PacketUtils;
// Assume that the base class lives here and we can import it
import org.photonvision.targeting.*;
/**
* Auto-generated serialization/deserialization helper for MultiTargetPNPResult
*/
public class MultiTargetPNPResultSerde implements PacketSerde<MultiTargetPNPResult> {
// Message definition md5sum. See photon_packet.adoc for details
public static final String MESSAGE_VERSION = "ffc1cb847deb6e796a583a5b1885496b";
public static final String MESSAGE_FORMAT = "PnpResult estimatedPose;int16[?] fiducialIDsUsed;";
public final String getTypeString() { return MESSAGE_FORMAT; }
public final String getInterfaceUUID() { return MESSAGE_VERSION; }
@Override
public int getMaxByteSize() {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'");
}
@Override
public void pack(Packet packet, MultiTargetPNPResult value) {
// field estimatedPose is of non-intrinsic type PnpResult
PnpResult.photonStruct.pack(packet, value.estimatedPose);
// fiducialIDsUsed is a intrinsic VLA!
packet.encode(value.fiducialIDsUsed);
}
@Override
public MultiTargetPNPResult unpack(Packet packet) {
var ret = new MultiTargetPNPResult();
// estimatedPose is of non-intrinsic type PnpResult
ret.estimatedPose = PnpResult.photonStruct.unpack(packet);
// fiducialIDsUsed is a custom VLA!
ret.fiducialIDsUsed = packet.decodeShortList();
return ret;
}
}

74
photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java Normal file
View File

@@ -0,0 +1,74 @@
/*
* 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/>.
*/
// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY
package org.photonvision.struct;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.utils.PacketUtils;
// Assume that the base class lives here and we can import it
import org.photonvision.targeting.*;
/**
* Auto-generated serialization/deserialization helper for PhotonPipelineMetadata
*/
public class PhotonPipelineMetadataSerde implements PacketSerde<PhotonPipelineMetadata> {
// Message definition md5sum. See photon_packet.adoc for details
public static final String MESSAGE_VERSION = "2a7039527bda14d13028a1b9282d40a2";
public static final String MESSAGE_FORMAT = "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;";
public final String getTypeString() { return MESSAGE_FORMAT; }
public final String getInterfaceUUID() { return MESSAGE_VERSION; }
@Override
public int getMaxByteSize() {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'");
}
@Override
public void pack(Packet packet, PhotonPipelineMetadata value) {
// field sequenceID is of intrinsic type int64
packet.encode((long) value.sequenceID);
// field captureTimestampMicros is of intrinsic type int64
packet.encode((long) value.captureTimestampMicros);
// field publishTimestampMicros is of intrinsic type int64
packet.encode((long) value.publishTimestampMicros);
}
@Override
public PhotonPipelineMetadata unpack(Packet packet) {
var ret = new PhotonPipelineMetadata();
// sequenceID is of intrinsic type int64
ret.sequenceID = packet.decodeLong();
// captureTimestampMicros is of intrinsic type int64
ret.captureTimestampMicros = packet.decodeLong();
// publishTimestampMicros is of intrinsic type int64
ret.publishTimestampMicros = packet.decodeLong();
return ret;
}
}

74
photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java Normal file
View File

@@ -0,0 +1,74 @@
/*
* 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/>.
*/
// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY
package org.photonvision.struct;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.utils.PacketUtils;
// Assume that the base class lives here and we can import it
import org.photonvision.targeting.*;
/**
* Auto-generated serialization/deserialization helper for PhotonPipelineResult
*/
public class PhotonPipelineResultSerde implements PacketSerde<PhotonPipelineResult> {
// Message definition md5sum. See photon_packet.adoc for details
public static final String MESSAGE_VERSION = "cb3e1605048ba49325888eb797399fe2";
public static final String MESSAGE_FORMAT = "PhotonPipelineMetadata metadata;PhotonTrackedTarget[?] targets;MultiTargetPNPResult? multitagResult;";
public final String getTypeString() { return MESSAGE_FORMAT; }
public final String getInterfaceUUID() { return MESSAGE_VERSION; }
@Override
public int getMaxByteSize() {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'");
}
@Override
public void pack(Packet packet, PhotonPipelineResult value) {
// field metadata is of non-intrinsic type PhotonPipelineMetadata
PhotonPipelineMetadata.photonStruct.pack(packet, value.metadata);
// targets is a custom VLA!
packet.encodeList(value.targets);
// multitagResult is optional! it better not be a VLA too
packet.encodeOptional(value.multitagResult);
}
@Override
public PhotonPipelineResult unpack(Packet packet) {
var ret = new PhotonPipelineResult();
// metadata is of non-intrinsic type PhotonPipelineMetadata
ret.metadata = PhotonPipelineMetadata.photonStruct.unpack(packet);
// targets is a custom VLA!
ret.targets = packet.decodeList(PhotonTrackedTarget.photonStruct);
// multitagResult is optional! it better not be a VLA too
ret.multitagResult = packet.decodeOptional(MultiTargetPNPResult.photonStruct);
return ret;
}
}

128
photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java Normal file
View File

@@ -0,0 +1,128 @@
/*
* 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/>.
*/
// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY
package org.photonvision.struct;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.utils.PacketUtils;
// Assume that the base class lives here and we can import it
import org.photonvision.targeting.*;
/**
* Auto-generated serialization/deserialization helper for PhotonTrackedTarget
*/
public class PhotonTrackedTargetSerde implements PacketSerde<PhotonTrackedTarget> {
// Message definition md5sum. See photon_packet.adoc for details
public static final String MESSAGE_VERSION = "8fdada56b9162f2e32bd24f0055d7b60";
public static final String MESSAGE_FORMAT = "float64 yaw;float64 pitch;float64 area;float64 skew;int32 fiducialId;int32 objDetectId;float32 objDetectConf;Transform3d bestCameraToTarget;Transform3d altCameraToTarget;float64 poseAmbiguity;TargetCorner[?] minAreaRectCorners;TargetCorner[?] detectedCorners;";
public final String getTypeString() { return MESSAGE_FORMAT; }
public final String getInterfaceUUID() { return MESSAGE_VERSION; }
@Override
public int getMaxByteSize() {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'");
}
@Override
public void pack(Packet packet, PhotonTrackedTarget value) {
// field yaw is of intrinsic type float64
packet.encode((double) value.yaw);
// field pitch is of intrinsic type float64
packet.encode((double) value.pitch);
// field area is of intrinsic type float64
packet.encode((double) value.area);
// field skew is of intrinsic type float64
packet.encode((double) value.skew);
// field fiducialId is of intrinsic type int32
packet.encode((int) value.fiducialId);
// field objDetectId is of intrinsic type int32
packet.encode((int) value.objDetectId);
// field objDetectConf is of intrinsic type float32
packet.encode((float) value.objDetectConf);
// field is shimmed!
PacketUtils.packTransform3d(packet, value.bestCameraToTarget);
// field is shimmed!
PacketUtils.packTransform3d(packet, value.altCameraToTarget);
// field poseAmbiguity is of intrinsic type float64
packet.encode((double) value.poseAmbiguity);
// minAreaRectCorners is a custom VLA!
packet.encodeList(value.minAreaRectCorners);
// detectedCorners is a custom VLA!
packet.encodeList(value.detectedCorners);
}
@Override
public PhotonTrackedTarget unpack(Packet packet) {
var ret = new PhotonTrackedTarget();
// yaw is of intrinsic type float64
ret.yaw = packet.decodeDouble();
// pitch is of intrinsic type float64
ret.pitch = packet.decodeDouble();
// area is of intrinsic type float64
ret.area = packet.decodeDouble();
// skew is of intrinsic type float64
ret.skew = packet.decodeDouble();
// fiducialId is of intrinsic type int32
ret.fiducialId = packet.decodeInt();
// objDetectId is of intrinsic type int32
ret.objDetectId = packet.decodeInt();
// objDetectConf is of intrinsic type float32
ret.objDetectConf = packet.decodeFloat();
// field is shimmed!
ret.bestCameraToTarget = PacketUtils.unpackTransform3d(packet);
// field is shimmed!
ret.altCameraToTarget = PacketUtils.unpackTransform3d(packet);
// poseAmbiguity is of intrinsic type float64
ret.poseAmbiguity = packet.decodeDouble();
// minAreaRectCorners is a custom VLA!
ret.minAreaRectCorners = packet.decodeList(TargetCorner.photonStruct);
// detectedCorners is a custom VLA!
ret.detectedCorners = packet.decodeList(TargetCorner.photonStruct);
return ret;
}
}

86
photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.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/>.
*/
// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY
package org.photonvision.struct;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.utils.PacketUtils;
// Assume that the base class lives here and we can import it
import org.photonvision.targeting.*;
/**
* Auto-generated serialization/deserialization helper for PnpResult
*/
public class PnpResultSerde implements PacketSerde<PnpResult> {
// Message definition md5sum. See photon_packet.adoc for details
public static final String MESSAGE_VERSION = "0d1f2546b00f24718e30f38d206d4491";
public static final String MESSAGE_FORMAT = "Transform3d best;Transform3d alt;float64 bestReprojErr;float64 altReprojErr;float64 ambiguity;";
public final String getTypeString() { return MESSAGE_FORMAT; }
public final String getInterfaceUUID() { return MESSAGE_VERSION; }
@Override
public int getMaxByteSize() {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'");
}
@Override
public void pack(Packet packet, PnpResult value) {
// field is shimmed!
PacketUtils.packTransform3d(packet, value.best);
// field is shimmed!
PacketUtils.packTransform3d(packet, value.alt);
// field bestReprojErr is of intrinsic type float64
packet.encode((double) value.bestReprojErr);
// field altReprojErr is of intrinsic type float64
packet.encode((double) value.altReprojErr);
// field ambiguity is of intrinsic type float64
packet.encode((double) value.ambiguity);
}
@Override
public PnpResult unpack(Packet packet) {
var ret = new PnpResult();
// field is shimmed!
ret.best = PacketUtils.unpackTransform3d(packet);
// field is shimmed!
ret.alt = PacketUtils.unpackTransform3d(packet);
// bestReprojErr is of intrinsic type float64
ret.bestReprojErr = packet.decodeDouble();
// altReprojErr is of intrinsic type float64
ret.altReprojErr = packet.decodeDouble();
// ambiguity is of intrinsic type float64
ret.ambiguity = packet.decodeDouble();
return ret;
}
}

68
photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java 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/>.
*/
// THIS FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO NOT MODIFY
package org.photonvision.struct;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.utils.PacketUtils;
// Assume that the base class lives here and we can import it
import org.photonvision.targeting.*;
/**
* Auto-generated serialization/deserialization helper for TargetCorner
*/
public class TargetCornerSerde implements PacketSerde<TargetCorner> {
// Message definition md5sum. See photon_packet.adoc for details
public static final String MESSAGE_VERSION = "22b1ff7551d10215af6fb3672fe4eda8";
public static final String MESSAGE_FORMAT = "float64 x;float64 y;";
public final String getTypeString() { return MESSAGE_FORMAT; }
public final String getInterfaceUUID() { return MESSAGE_VERSION; }
@Override
public int getMaxByteSize() {
// TODO Auto-generated method stub
throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'");
}
@Override
public void pack(Packet packet, TargetCorner value) {
// field x is of intrinsic type float64
packet.encode((double) value.x);
// field y is of intrinsic type float64
packet.encode((double) value.y);
}
@Override
public TargetCorner unpack(Packet packet) {
var ret = new TargetCorner();
// x is of intrinsic type float64
ret.x = packet.decodeDouble();
// y is of intrinsic type float64
ret.y = packet.decodeDouble();
return ret;
}
}

39
photon-targeting/src/generated/main/native/cpp/photon/serde/MultiTargetPNPResultSerde.cpp Normal file
View File

@@ -0,0 +1,39 @@
/*
* 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/>.
*/
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
#include "photon/serde/MultiTargetPNPResultSerde.h"
namespace photon {
using StructType = SerdeType<MultiTargetPNPResult>;
void StructType::Pack(Packet& packet, const MultiTargetPNPResult& value) {
packet.Pack<photon::PnpResult>(value.estimatedPose);
packet.Pack<std::vector<int16_t>>(value.fiducialIDsUsed);
}
MultiTargetPNPResult StructType::Unpack(Packet& packet) {
return MultiTargetPNPResult{MultiTargetPNPResult_PhotonStruct{
.estimatedPose = packet.Unpack<photon::PnpResult>(),
.fiducialIDsUsed = packet.Unpack<std::vector<int16_t>>(),
}};
}
} // namespace photon

41
photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineMetadataSerde.cpp Normal file
View File

@@ -0,0 +1,41 @@
/*
* 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/>.
*/
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
#include "photon/serde/PhotonPipelineMetadataSerde.h"
namespace photon {
using StructType = SerdeType<PhotonPipelineMetadata>;
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);
}
PhotonPipelineMetadata StructType::Unpack(Packet& packet) {
return PhotonPipelineMetadata{PhotonPipelineMetadata_PhotonStruct{
.sequenceID = packet.Unpack<int64_t>(),
.captureTimestampMicros = packet.Unpack<int64_t>(),
.publishTimestampMicros = packet.Unpack<int64_t>(),
}};
}
} // namespace photon

43
photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonPipelineResultSerde.cpp 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/>.
*/
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
#include "photon/serde/PhotonPipelineResultSerde.h"
namespace photon {
using StructType = SerdeType<PhotonPipelineResult>;
void StructType::Pack(Packet& packet, const PhotonPipelineResult& value) {
packet.Pack<photon::PhotonPipelineMetadata>(value.metadata);
packet.Pack<std::vector<photon::PhotonTrackedTarget>>(value.targets);
packet.Pack<std::optional<photon::MultiTargetPNPResult>>(
value.multitagResult);
}
PhotonPipelineResult StructType::Unpack(Packet& packet) {
return PhotonPipelineResult{PhotonPipelineResult_PhotonStruct{
.metadata = packet.Unpack<photon::PhotonPipelineMetadata>(),
.targets = packet.Unpack<std::vector<photon::PhotonTrackedTarget>>(),
.multitagResult =
packet.Unpack<std::optional<photon::MultiTargetPNPResult>>(),
}};
}
} // namespace photon

59
photon-targeting/src/generated/main/native/cpp/photon/serde/PhotonTrackedTargetSerde.cpp Normal file
View File

@@ -0,0 +1,59 @@
/*
* 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/>.
*/
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
#include "photon/serde/PhotonTrackedTargetSerde.h"
namespace photon {
using StructType = SerdeType<PhotonTrackedTarget>;
void StructType::Pack(Packet& packet, const PhotonTrackedTarget& value) {
packet.Pack<double>(value.yaw);
packet.Pack<double>(value.pitch);
packet.Pack<double>(value.area);
packet.Pack<double>(value.skew);
packet.Pack<int32_t>(value.fiducialId);
packet.Pack<int32_t>(value.objDetectId);
packet.Pack<float>(value.objDetectConf);
packet.Pack<frc::Transform3d>(value.bestCameraToTarget);
packet.Pack<frc::Transform3d>(value.altCameraToTarget);
packet.Pack<double>(value.poseAmbiguity);
packet.Pack<std::vector<photon::TargetCorner>>(value.minAreaRectCorners);
packet.Pack<std::vector<photon::TargetCorner>>(value.detectedCorners);
}
PhotonTrackedTarget StructType::Unpack(Packet& packet) {
return PhotonTrackedTarget{PhotonTrackedTarget_PhotonStruct{
.yaw = packet.Unpack<double>(),
.pitch = packet.Unpack<double>(),
.area = packet.Unpack<double>(),
.skew = packet.Unpack<double>(),
.fiducialId = packet.Unpack<int32_t>(),
.objDetectId = packet.Unpack<int32_t>(),
.objDetectConf = packet.Unpack<float>(),
.bestCameraToTarget = packet.Unpack<frc::Transform3d>(),
.altCameraToTarget = packet.Unpack<frc::Transform3d>(),
.poseAmbiguity = packet.Unpack<double>(),
.minAreaRectCorners = packet.Unpack<std::vector<photon::TargetCorner>>(),
.detectedCorners = packet.Unpack<std::vector<photon::TargetCorner>>(),
}};
}
} // namespace photon

45
photon-targeting/src/generated/main/native/cpp/photon/serde/PnpResultSerde.cpp Normal file
View File

@@ -0,0 +1,45 @@
/*
* 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/>.
*/
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
#include "photon/serde/PnpResultSerde.h"
namespace photon {
using StructType = SerdeType<PnpResult>;
void StructType::Pack(Packet& packet, const PnpResult& value) {
packet.Pack<frc::Transform3d>(value.best);
packet.Pack<frc::Transform3d>(value.alt);
packet.Pack<double>(value.bestReprojErr);
packet.Pack<double>(value.altReprojErr);
packet.Pack<double>(value.ambiguity);
}
PnpResult StructType::Unpack(Packet& packet) {
return PnpResult{PnpResult_PhotonStruct{
.best = packet.Unpack<frc::Transform3d>(),
.alt = packet.Unpack<frc::Transform3d>(),
.bestReprojErr = packet.Unpack<double>(),
.altReprojErr = packet.Unpack<double>(),
.ambiguity = packet.Unpack<double>(),
}};
}
} // namespace photon

39
photon-targeting/src/generated/main/native/cpp/photon/serde/TargetCornerSerde.cpp Normal file
View File

@@ -0,0 +1,39 @@
/*
* 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/>.
*/
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
#include "photon/serde/TargetCornerSerde.h"
namespace photon {
using StructType = SerdeType<TargetCorner>;
void StructType::Pack(Packet& packet, const TargetCorner& value) {
packet.Pack<double>(value.x);
packet.Pack<double>(value.y);
}
TargetCorner StructType::Unpack(Packet& packet) {
return TargetCorner{TargetCorner_PhotonStruct{
.x = packet.Unpack<double>(),
.y = packet.Unpack<double>(),
}};
}
} // namespace photon

53
photon-targeting/src/generated/main/native/include/photon/serde/MultiTargetPNPResultSerde.h Normal file
View File

@@ -0,0 +1,53 @@
/*
* 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
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
#include <wpi/SymbolExports.h>
// Include myself
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
// Includes for dependant types
#include "photon/targeting/PnpResult.h"
#include <stdint.h>
#include <vector>
namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<MultiTargetPNPResult> {
static constexpr std::string_view GetSchemaHash() {
return "ffc1cb847deb6e796a583a5b1885496b";
}
static constexpr std::string_view GetSchema() {
return "PnpResult estimatedPose;int16[?] fiducialIDsUsed;";
}
static photon::MultiTargetPNPResult Unpack(photon::Packet& packet);
static void Pack(photon::Packet& packet,
const photon::MultiTargetPNPResult& value);
};
static_assert(photon::PhotonStructSerializable<photon::MultiTargetPNPResult>);
} // namespace photon

52
photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h Normal file
View File

@@ -0,0 +1,52 @@
/*
* 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
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
#include <wpi/SymbolExports.h>
// Include myself
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/PhotonPipelineMetadata.h"
// Includes for dependant types
#include <stdint.h>
namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<PhotonPipelineMetadata> {
static constexpr std::string_view GetSchemaHash() {
return "2a7039527bda14d13028a1b9282d40a2";
}
static constexpr std::string_view GetSchema() {
return "int64 sequenceID;int64 captureTimestampMicros;int64 "
"publishTimestampMicros;";
}
static photon::PhotonPipelineMetadata Unpack(photon::Packet& packet);
static void Pack(photon::Packet& packet,
const photon::PhotonPipelineMetadata& value);
};
static_assert(photon::PhotonStructSerializable<photon::PhotonPipelineMetadata>);
} // namespace photon

57
photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h Normal file
View File

@@ -0,0 +1,57 @@
/*
* 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
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
#include <wpi/SymbolExports.h>
// Include myself
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/PhotonPipelineResult.h"
// Includes for dependant types
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PhotonPipelineMetadata.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include <optional>
#include <stdint.h>
#include <vector>
namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<PhotonPipelineResult> {
static constexpr std::string_view GetSchemaHash() {
return "cb3e1605048ba49325888eb797399fe2";
}
static constexpr std::string_view GetSchema() {
return "PhotonPipelineMetadata metadata;PhotonTrackedTarget[?] "
"targets;MultiTargetPNPResult? multitagResult;";
}
static photon::PhotonPipelineResult Unpack(photon::Packet& packet);
static void Pack(photon::Packet& packet,
const photon::PhotonPipelineResult& value);
};
static_assert(photon::PhotonStructSerializable<photon::PhotonPipelineResult>);
} // namespace photon

58
photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h Normal file
View File

@@ -0,0 +1,58 @@
/*
* 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
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
#include <wpi/SymbolExports.h>
// Include myself
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/PhotonTrackedTarget.h"
// Includes for dependant types
#include "photon/targeting/TargetCorner.h"
#include <frc/geometry/Transform3d.h>
#include <stdint.h>
#include <vector>
namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<PhotonTrackedTarget> {
static constexpr std::string_view GetSchemaHash() {
return "8fdada56b9162f2e32bd24f0055d7b60";
}
static constexpr std::string_view GetSchema() {
return "float64 yaw;float64 pitch;float64 area;float64 skew;int32 "
"fiducialId;int32 objDetectId;float32 objDetectConf;Transform3d "
"bestCameraToTarget;Transform3d altCameraToTarget;float64 "
"poseAmbiguity;TargetCorner[?] minAreaRectCorners;TargetCorner[?] "
"detectedCorners;";
}
static photon::PhotonTrackedTarget Unpack(photon::Packet& packet);
static void Pack(photon::Packet& packet,
const photon::PhotonTrackedTarget& value);
};
static_assert(photon::PhotonStructSerializable<photon::PhotonTrackedTarget>);
} // namespace photon

52
photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h Normal file
View File

@@ -0,0 +1,52 @@
/*
* 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
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
#include <wpi/SymbolExports.h>
// Include myself
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/PnpResult.h"
// Includes for dependant types
#include <frc/geometry/Transform3d.h>
#include <stdint.h>
namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<PnpResult> {
static constexpr std::string_view GetSchemaHash() {
return "0d1f2546b00f24718e30f38d206d4491";
}
static constexpr std::string_view GetSchema() {
return "Transform3d best;Transform3d alt;float64 bestReprojErr;float64 "
"altReprojErr;float64 ambiguity;";
}
static photon::PnpResult Unpack(photon::Packet& packet);
static void Pack(photon::Packet& packet, const photon::PnpResult& value);
};
static_assert(photon::PhotonStructSerializable<photon::PnpResult>);
} // namespace photon

50
photon-targeting/src/generated/main/native/include/photon/serde/TargetCornerSerde.h Normal file
View File

@@ -0,0 +1,50 @@
/*
* 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
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
#include <wpi/SymbolExports.h>
// Include myself
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/TargetCorner.h"
// Includes for dependant types
#include <stdint.h>
namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<TargetCorner> {
static constexpr std::string_view GetSchemaHash() {
return "22b1ff7551d10215af6fb3672fe4eda8";
}
static constexpr std::string_view GetSchema() {
return "float64 x;float64 y;";
}
static photon::TargetCorner Unpack(photon::Packet& packet);
static void Pack(photon::Packet& packet, const photon::TargetCorner& value);
};
static_assert(photon::PhotonStructSerializable<photon::TargetCorner>);
} // namespace photon

40
photon-targeting/src/generated/main/native/include/photon/struct/MultiTargetPNPResultStruct.h Normal file
View File

@@ -0,0 +1,40 @@
/*
* 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
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
// Includes for dependant types
#include <stdint.h>
#include <vector>
#include "photon/targeting/PnpResult.h"
namespace photon {
struct MultiTargetPNPResult_PhotonStruct {
photon::PnpResult estimatedPose;
std::vector<int16_t> fiducialIDsUsed;
friend bool operator==(MultiTargetPNPResult_PhotonStruct const&,
MultiTargetPNPResult_PhotonStruct const&) = default;
};
} // namespace photon

24
photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp → photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineMetadataStruct.h
View File

@@ -15,11 +15,23 @@
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include "gtest/gtest.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#pragma once
// TODO
TEST(MultiTargetPNPResultTest, Equality) {}
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
// TODO
TEST(MultiTargetPNPResultTest, Inequality) {}
// Includes for dependant types
#include <stdint.h>
namespace photon {
struct PhotonPipelineMetadata_PhotonStruct {
int64_t sequenceID;
int64_t captureTimestampMicros;
int64_t publishTimestampMicros;
friend bool operator==(PhotonPipelineMetadata_PhotonStruct const&,
PhotonPipelineMetadata_PhotonStruct const&) = default;
};
} // namespace photon

44
photon-targeting/src/generated/main/native/include/photon/struct/PhotonPipelineResultStruct.h Normal file
View File

@@ -0,0 +1,44 @@
/*
* 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
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
// Includes for dependant types
#include <stdint.h>
#include <optional>
#include <vector>
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PhotonPipelineMetadata.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace photon {
struct PhotonPipelineResult_PhotonStruct {
photon::PhotonPipelineMetadata metadata;
std::vector<photon::PhotonTrackedTarget> targets;
std::optional<photon::MultiTargetPNPResult> multitagResult;
friend bool operator==(PhotonPipelineResult_PhotonStruct const&,
PhotonPipelineResult_PhotonStruct const&) = default;
};
} // namespace photon

52
photon-targeting/src/generated/main/native/include/photon/struct/PhotonTrackedTargetStruct.h Normal file
View File

@@ -0,0 +1,52 @@
/*
* 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
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
// Includes for dependant types
#include <stdint.h>
#include <vector>
#include <frc/geometry/Transform3d.h>
#include "photon/targeting/TargetCorner.h"
namespace photon {
struct PhotonTrackedTarget_PhotonStruct {
double yaw;
double pitch;
double area;
double skew;
int32_t fiducialId;
int32_t objDetectId;
float objDetectConf;
frc::Transform3d bestCameraToTarget;
frc::Transform3d altCameraToTarget;
double poseAmbiguity;
std::vector<photon::TargetCorner> minAreaRectCorners;
std::vector<photon::TargetCorner> detectedCorners;
friend bool operator==(PhotonTrackedTarget_PhotonStruct const&,
PhotonTrackedTarget_PhotonStruct const&) = default;
};
} // namespace photon

41
photon-targeting/src/generated/main/native/include/photon/struct/PnpResultStruct.h Normal file
View File

@@ -0,0 +1,41 @@
/*
* 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
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
// Includes for dependant types
#include <stdint.h>
#include <frc/geometry/Transform3d.h>
namespace photon {
struct PnpResult_PhotonStruct {
frc::Transform3d best;
frc::Transform3d alt;
double bestReprojErr;
double altReprojErr;
double ambiguity;
friend bool operator==(PnpResult_PhotonStruct const&,
PnpResult_PhotonStruct const&) = default;
};
} // namespace photon

23
photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp → photon-targeting/src/generated/main/native/include/photon/struct/TargetCornerStruct.h
View File

@@ -15,11 +15,22 @@
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include "gtest/gtest.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#pragma once
// TODO
TEST(PhotonTrackedTargetTest, Equality) {}
// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY
// TODO
TEST(PhotonTrackedTargetTest, Inequality) {}
// Includes for dependant types
#include <stdint.h>
namespace photon {
struct TargetCorner_PhotonStruct {
double x;
double y;
friend bool operator==(TargetCorner_PhotonStruct const&,
TargetCorner_PhotonStruct const&) = default;
};
} // namespace photon

187
photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
View File

@@ -17,22 +17,25 @@
package org.photonvision.common.dataflow.structures;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.Optional;
import org.photonvision.targeting.serde.PhotonStructSerializable;
/** A packet that holds byte-packed data to be sent over NetworkTables. */
public class Packet {
// Size of the packet.
int size;
// Data stored in the packet.
byte[] packetData;
// Read and write positions.
int readPos, writePos;
/**
* Constructs an empty packet.
* Constructs an empty packet. This buffer will dynamically expand if we need more data space.
*
* @param size The size of the packet buffer.
*/
public Packet(int size) {
this.size = size;
packetData = new byte[size];
}
@@ -43,27 +46,34 @@ public class Packet {
*/
public Packet(byte[] data) {
packetData = data;
size = packetData.length;
}
/** Clears the packet and resets the read and write positions. */
public void clear() {
packetData = new byte[size];
packetData = new byte[packetData.length];
readPos = 0;
writePos = 0;
}
public int getNumBytesWritten() {
return writePos + 1;
}
public int getNumBytesRead() {
return readPos + 1;
}
public int getSize() {
return size;
return packetData.length;
}
/**
* Returns the packet data.
* Returns a copy of only the packet data we've actually written to so far.
*
* @return The packet data.
*/
public byte[] getData() {
return packetData;
public byte[] getWrittenDataCopy() {
return Arrays.copyOfRange(packetData, 0, writePos);
}
/**
@@ -73,7 +83,64 @@ public class Packet {
*/
public void setData(byte[] data) {
packetData = data;
size = data.length;
}
// Logic taken from ArraysSupport, licensed under GPL V2
public static final int SOFT_MAX_ARRAY_LENGTH = Integer.MAX_VALUE - 8;
// Logic taken from ArraysSupport, licensed under GPL V2
private static int newLength(int oldLength, int minGrowth, int prefGrowth) {
// preconditions not checked because of inlining
// assert oldLength >= 0
// assert minGrowth > 0
int prefLength = oldLength + Math.max(minGrowth, prefGrowth); // might overflow
if (0 < prefLength && prefLength <= SOFT_MAX_ARRAY_LENGTH) {
return prefLength;
} else {
// put code cold in a separate method
return hugeLength(oldLength, minGrowth);
}
}
// Logic taken from ArraysSupport, licensed under GPL V2
private static int hugeLength(int oldLength, int minGrowth) {
int minLength = oldLength + minGrowth;
if (minLength < 0) { // overflow
throw new OutOfMemoryError(
"Required array length " + oldLength + " + " + minGrowth + " is too large");
} else if (minLength <= SOFT_MAX_ARRAY_LENGTH) {
return SOFT_MAX_ARRAY_LENGTH;
} else {
return minLength;
}
}
/**
* Increases the capacity to ensure that it can hold at least the number of elements specified by
* the minimum capacity argument.
*
* <p>This logic is copied from ArrayList, which is licensed GPL V2
*
* @param minCapacity the desired minimum capacity
* @return
*/
private void ensureCapacity(int bytesToAdd) {
int minCapacity = writePos + bytesToAdd;
int oldCapacity = packetData.length;
if (minCapacity <= oldCapacity) {
return;
}
if (oldCapacity > 0) {
int newCapacity =
Packet.newLength(
oldCapacity,
minCapacity - oldCapacity, /* minimum growth */
oldCapacity >> 1 /* preferred growth */);
packetData = Arrays.copyOf(packetData, newCapacity);
} else {
packetData = new byte[Math.max(256, minCapacity)];
}
}
/**
@@ -82,6 +149,7 @@ public class Packet {
* @param src The byte to encode.
*/
public void encode(byte src) {
ensureCapacity(1);
packetData[writePos++] = src;
}
@@ -91,6 +159,7 @@ public class Packet {
* @param src The short to encode.
*/
public void encode(short src) {
ensureCapacity(2);
packetData[writePos++] = (byte) (src >>> 8);
packetData[writePos++] = (byte) src;
}
@@ -101,6 +170,7 @@ public class Packet {
* @param src The integer to encode.
*/
public void encode(int src) {
ensureCapacity(4);
packetData[writePos++] = (byte) (src >>> 24);
packetData[writePos++] = (byte) (src >>> 16);
packetData[writePos++] = (byte) (src >>> 8);
@@ -113,6 +183,7 @@ public class Packet {
* @param src The float to encode.
*/
public void encode(float src) {
ensureCapacity(4);
int data = Float.floatToIntBits(src);
packetData[writePos++] = (byte) ((data >> 24) & 0xff);
packetData[writePos++] = (byte) ((data >> 16) & 0xff);
@@ -126,6 +197,7 @@ public class Packet {
* @param data The double to encode.
*/
public void encode(long data) {
ensureCapacity(8);
packetData[writePos++] = (byte) ((data >> 56) & 0xff);
packetData[writePos++] = (byte) ((data >> 48) & 0xff);
packetData[writePos++] = (byte) ((data >> 40) & 0xff);
@@ -142,6 +214,7 @@ public class Packet {
* @param src The double to encode.
*/
public void encode(double src) {
ensureCapacity(8);
long data = Double.doubleToRawLongBits(src);
packetData[writePos++] = (byte) ((data >> 56) & 0xff);
packetData[writePos++] = (byte) ((data >> 48) & 0xff);
@@ -159,9 +232,56 @@ public class Packet {
* @param src The boolean to encode.
*/
public void encode(boolean src) {
ensureCapacity(1);
packetData[writePos++] = src ? (byte) 1 : (byte) 0;
}
public void encode(List<Short> data) {
byte size = (byte) data.size();
if (data.size() > Byte.MAX_VALUE) {
throw new RuntimeException("Array too long! Got " + size);
}
// length byte
encode(size);
for (var f : data) {
encode(f);
}
}
public <T extends PhotonStructSerializable<T>> void encode(T data) {
data.getSerde().pack(this, data);
}
/**
* Encode a list of serializable structs. Lists are stored as [uint8 length, [length many] data
* structs]
*
* @param <T> the class this list will be packing
* @param data
*/
public <T extends PhotonStructSerializable<T>> void encodeList(List<T> data) {
byte size = (byte) data.size();
if (data.size() > Byte.MAX_VALUE) {
throw new RuntimeException("Array too long! Got " + size);
}
// length byte
encode(size);
for (var f : data) {
f.getSerde().pack(this, f);
}
}
public <T extends PhotonStructSerializable<T>> void encodeOptional(Optional<T> data) {
encode(data.isPresent());
if (data.isPresent()) {
data.get().getSerde().pack(this, data.get());
}
}
/**
* Returns a decoded byte from the packet.
*
@@ -275,4 +395,49 @@ public class Packet {
}
return (short) ((0xff & packetData[readPos++]) << 8 | (0xff & packetData[readPos++]));
}
/**
* Decode a list of serializable structs. Lists are stored as [uint8 length, [length many] data
* structs]. Because java sucks, we need to take the serde ref directly
*
* @param <T>
* @param serde
*/
public <T extends PhotonStructSerializable<T>> List<T> decodeList(PacketSerde<T> serde) {
byte length = decodeByte();
var ret = new ArrayList<T>();
ret.ensureCapacity(length);
for (int i = 0; i < length; i++) {
ret.add(serde.unpack(this));
}
return ret;
}
public <T extends PhotonStructSerializable<T>> Optional<T> decodeOptional(PacketSerde<T> serde) {
var present = decodeBoolean();
if (present) {
return Optional.of(serde.unpack(this));
}
return Optional.empty();
}
public List<Short> decodeShortList() {
byte length = decodeByte();
var ret = new ArrayList<Short>();
ret.ensureCapacity(length);
for (int i = 0; i < length; i++) {
ret.add(decodeShort());
}
return ret;
}
public <T extends PhotonStructSerializable<T>> T decode(PhotonStructSerializable<T> t) {
return t.getSerde().unpack(this);
}
}

4
photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java
View File

@@ -23,4 +23,8 @@ public interface PacketSerde<T> {
void pack(Packet packet, T value);
T unpack(Packet packet);
String getTypeString();
String getInterfaceUUID();
}

3
photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java
View File

@@ -77,7 +77,8 @@ public class NTTopicSet {
.getRawTopic("rawBytes")
.publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
resultPublisher = new PacketPublisher<>(rawBytesEntry, PhotonPipelineResult.serde);
resultPublisher =
new PacketPublisher<PhotonPipelineResult>(rawBytesEntry, PhotonPipelineResult.photonStruct);
protoResultPublisher =
subTable
.getProtobufTopic("result_proto", PhotonPipelineResult.proto)

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

@@ -17,27 +17,44 @@
package org.photonvision.common.networktables;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.networktables.RawPublisher;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
public class PacketPublisher<T> implements AutoCloseable {
public final RawPublisher publisher;
private final PacketSerde<T> serde;
private final PacketSerde<T> photonStruct;
public PacketPublisher(RawPublisher publisher, PacketSerde<T> serde) {
public PacketPublisher(RawPublisher publisher, PacketSerde<T> photonStruct) {
this.publisher = publisher;
this.serde = serde;
this.photonStruct = photonStruct;
var mapper = new ObjectMapper();
try {
this.publisher
.getTopic()
.setProperty("message_format", mapper.writeValueAsString(photonStruct.getTypeString()));
this.publisher
.getTopic()
.setProperty("message_uuid", mapper.writeValueAsString(photonStruct.getInterfaceUUID()));
} catch (JsonProcessingException e) {
// TODO Auto-generated catch block
e.printStackTrace();
throw new RuntimeException(e);
}
}
public void set(T value, int byteSize) {
var packet = new Packet(byteSize);
serde.pack(packet, value);
publisher.set(packet.getData());
photonStruct.pack(packet, value);
// todo: trim to only the bytes we need to send
publisher.set(packet.getWrittenDataCopy());
}
public void set(T value) {
set(value, serde.getMaxByteSize());
set(value, photonStruct.getMaxByteSize());
}
@Override

5
photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java
View File

@@ -86,6 +86,11 @@ public class PacketSubscriber<T> implements AutoCloseable {
subscriber.close();
}
// TODO - i can see an argument for moving this logic all here instead of keeping in photoncamera
public String getInterfaceUUID() {
return subscriber.getTopic().getProperty("message_uuid");
}
public List<PacketResult<T>> getAllChanges() {
List<PacketResult<T>> ret = new ArrayList<>();

18
photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View File

@@ -31,6 +31,7 @@ import edu.wpi.first.math.numbers.*;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.Optional;
import org.ejml.simple.SimpleMatrix;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.Core;
@@ -46,7 +47,7 @@ import org.opencv.core.Point3;
import org.opencv.core.Rect;
import org.opencv.core.RotatedRect;
import org.opencv.imgproc.Imgproc;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PnpResult;
import org.photonvision.targeting.TargetCorner;
public final class OpenCVHelp {
@@ -402,7 +403,7 @@ public final class OpenCVHelp {
* @return The resulting transformation that maps the camera pose to the target pose and the
* ambiguity if an alternate solution is available.
*/
public static PNPResult solvePNP_SQUARE(
public static Optional<PnpResult> solvePNP_SQUARE(
Matrix<N3, N3> cameraMatrix,
Matrix<N8, N1> distCoeffs,
List<Translation3d> modelTrls,
@@ -467,14 +468,15 @@ public final class OpenCVHelp {
// check if solvePnP failed with NaN results and retrying failed
if (Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result");
if (alt != null) return new PNPResult(best, alt, errors[0] / errors[1], errors[0], errors[1]);
else return new PNPResult(best, errors[0]);
if (alt != null)
return Optional.of(new PnpResult(best, alt, errors[0] / errors[1], errors[0], errors[1]));
else return Optional.empty();
}
// solvePnP failed
catch (Exception e) {
System.err.println("SolvePNP_SQUARE failed!");
e.printStackTrace();
return new PNPResult();
return Optional.empty();
} finally {
// release our Mats from native memory
objectMat.release();
@@ -509,7 +511,7 @@ public final class OpenCVHelp {
* model points are supplied relative to the origin, this transformation brings the camera to
* the origin.
*/
public static PNPResult solvePNP_SQPNP(
public static Optional<PnpResult> solvePNP_SQPNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N8, N1> distCoeffs,
List<Translation3d> objectTrls,
@@ -558,11 +560,11 @@ public final class OpenCVHelp {
// check if solvePnP failed with NaN results
if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result");
return new PNPResult(best, error[0]);
return Optional.of(new PnpResult(best, error[0]));
} catch (Exception e) {
System.err.println("SolvePNP_SQPNP failed!");
e.printStackTrace();
return new PNPResult();
return Optional.empty();
}
}
}

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

@@ -27,10 +27,11 @@ import edu.wpi.first.math.numbers.*;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import java.util.Optional;
import java.util.stream.Collectors;
import org.opencv.core.Point;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.PnpResult;
import org.photonvision.targeting.TargetCorner;
public class VisionEstimation {
@@ -64,9 +65,9 @@ public class VisionEstimation {
* @param visTags The visible tags reported by PV. Non-tag targets are automatically excluded.
* @param tagLayout The known tag layout on the field
* @return The transformation that maps the field origin to the camera pose. Ensure the {@link
* PNPResult} are present before utilizing them.
* PnpResult} are present before utilizing them.
*/
public static PNPResult estimateCamPosePNP(
public static Optional<PnpResult> estimateCamPosePNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N8, N1> distCoeffs,
List<PhotonTrackedTarget> visTags,
@@ -76,7 +77,7 @@ public class VisionEstimation {
|| visTags == null
|| tagLayout.getTags().isEmpty()
|| visTags.isEmpty()) {
return new PNPResult();
return Optional.empty();
}
var corners = new ArrayList<TargetCorner>();
@@ -93,7 +94,7 @@ public class VisionEstimation {
});
}
if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) {
return new PNPResult();
return Optional.empty();
}
Point[] points = OpenCVHelp.cornersToPoints(corners);
@@ -101,32 +102,34 @@ public class VisionEstimation {
if (knownTags.size() == 1) {
var camToTag =
OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, tagModel.vertices, points);
if (!camToTag.isPresent) return new PNPResult();
var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse());
if (!camToTag.isPresent()) return Optional.empty();
var bestPose = knownTags.get(0).pose.transformBy(camToTag.get().best.inverse());
var altPose = new Pose3d();
if (camToTag.ambiguity != 0)
altPose = knownTags.get(0).pose.transformBy(camToTag.alt.inverse());
if (camToTag.get().ambiguity != 0)
altPose = knownTags.get(0).pose.transformBy(camToTag.get().alt.inverse());
var o = new Pose3d();
return new PNPResult(
new Transform3d(o, bestPose),
new Transform3d(o, altPose),
camToTag.ambiguity,
camToTag.bestReprojErr,
camToTag.altReprojErr);
return Optional.of(
new PnpResult(
new Transform3d(o, bestPose),
new Transform3d(o, altPose),
camToTag.get().ambiguity,
camToTag.get().bestReprojErr,
camToTag.get().altReprojErr));
}
// multi-tag pnp
else {
var objectTrls = new ArrayList<Translation3d>();
for (var tag : knownTags) objectTrls.addAll(tagModel.getFieldVertices(tag.pose));
var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points);
if (!camToOrigin.isPresent) return new PNPResult();
return new PNPResult(
camToOrigin.best.inverse(),
camToOrigin.alt.inverse(),
camToOrigin.ambiguity,
camToOrigin.bestReprojErr,
camToOrigin.altReprojErr);
if (camToOrigin.isEmpty()) return Optional.empty();
return Optional.of(
new PnpResult(
camToOrigin.get().best.inverse(),
camToOrigin.get().alt.inverse(),
camToOrigin.get().ambiguity,
camToOrigin.get().bestReprojErr,
camToOrigin.get().altReprojErr));
}
}
}

55
photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java
View File

@@ -18,22 +18,23 @@
package org.photonvision.targeting;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.MultiTargetPNPResultSerde;
import org.photonvision.targeting.proto.MultiTargetPNPResultProto;
import org.photonvision.targeting.serde.PhotonStructSerializable;
public class MultiTargetPNPResult implements ProtobufSerializable {
public class MultiTargetPNPResult
implements ProtobufSerializable, PhotonStructSerializable<MultiTargetPNPResult> {
// Seeing 32 apriltags at once seems like a sane limit
private static final int MAX_IDS = 32;
public PNPResult estimatedPose = new PNPResult();
public List<Integer> fiducialIDsUsed = List.of();
public PnpResult estimatedPose = new PnpResult();
public List<Short> fiducialIDsUsed = List.of();
public MultiTargetPNPResult() {}
public MultiTargetPNPResult(PNPResult results, List<Integer> ids) {
public MultiTargetPNPResult(PnpResult results, List<Short> ids) {
estimatedPose = results;
fiducialIDsUsed = ids;
}
@@ -71,39 +72,13 @@ public class MultiTargetPNPResult implements ProtobufSerializable {
+ "]";
}
public static final class APacketSerde implements PacketSerde<MultiTargetPNPResult> {
@Override
public int getMaxByteSize() {
// PNPResult + MAX_IDS possible targets (arbitrary upper limit that should never be hit,
// ideally)
return PNPResult.serde.getMaxByteSize() + (Short.BYTES * MAX_IDS);
}
@Override
public void pack(Packet packet, MultiTargetPNPResult result) {
PNPResult.serde.pack(packet, result.estimatedPose);
for (int i = 0; i < MAX_IDS; i++) {
if (i < result.fiducialIDsUsed.size()) {
packet.encode((short) result.fiducialIDsUsed.get(i).byteValue());
} else {
packet.encode((short) -1);
}
}
}
@Override
public MultiTargetPNPResult unpack(Packet packet) {
var results = PNPResult.serde.unpack(packet);
var ids = new ArrayList<Integer>(MAX_IDS);
for (int i = 0; i < MAX_IDS; i++) {
int targetId = packet.decodeShort();
if (targetId > -1) ids.add(targetId);
}
return new MultiTargetPNPResult(results, ids);
}
}
public static final APacketSerde serde = new APacketSerde();
public static final MultiTargetPNPResultProto proto = new MultiTargetPNPResultProto();
// tODO!
public static final MultiTargetPNPResultSerde photonStruct = new MultiTargetPNPResultSerde();
@Override
public PacketSerde<MultiTargetPNPResult> getSerde() {
return photonStruct;
}
}

107
photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java Normal file
View File

@@ -0,0 +1,107 @@
/*
* 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.targeting;
import org.photonvision.common.dataflow.structures.PacketSerde;
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.
public long captureTimestampMicros;
public long publishTimestampMicros;
public PhotonPipelineMetadata(
long captureTimestampMicros, long publishTimestampMicros, long sequenceID) {
this.captureTimestampMicros = captureTimestampMicros;
this.publishTimestampMicros = publishTimestampMicros;
this.sequenceID = sequenceID;
}
public PhotonPipelineMetadata() {
this(-1, -1, -1);
}
/** Returns the time between image capture and publish to NT */
public double getLatencyMillis() {
return (publishTimestampMicros - captureTimestampMicros) / 1e3;
}
/** The time that this image was captured, in the coprocessor's time base. */
public long getCaptureTimestampMicros() {
return captureTimestampMicros;
}
/** The time that this result was published to NT, in the coprocessor's time base. */
public long getPublishTimestampMicros() {
return publishTimestampMicros;
}
/**
* The number of non-empty frames processed by this camera since boot. Useful to checking if a
* camera is alive.
*/
public long getSequenceID() {
return sequenceID;
}
@Override
public String toString() {
return "PhotonPipelineMetadata [sequenceID="
+ sequenceID
+ ", captureTimestampMicros="
+ captureTimestampMicros
+ ", publishTimestampMicros="
+ publishTimestampMicros
+ "]";
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + (int) (sequenceID ^ (sequenceID >>> 32));
result = prime * result + (int) (captureTimestampMicros ^ (captureTimestampMicros >>> 32));
result = prime * result + (int) (publishTimestampMicros ^ (publishTimestampMicros >>> 32));
return result;
}
@Override
public boolean equals(Object obj) {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PhotonPipelineMetadata other = (PhotonPipelineMetadata) obj;
if (sequenceID != other.sequenceID) return false;
if (captureTimestampMicros != other.captureTimestampMicros) return false;
if (publishTimestampMicros != other.publishTimestampMicros) return false;
return true;
}
public static final PhotonPipelineMetadataSerde photonStruct = new PhotonPipelineMetadataSerde();
@Override
public PacketSerde<PhotonPipelineMetadata> getSerde() {
return photonStruct;
}
}

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

@@ -20,33 +20,33 @@ package org.photonvision.targeting;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import java.util.Optional;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.PhotonPipelineResultSerde;
import org.photonvision.targeting.proto.PhotonPipelineResultProto;
import org.photonvision.targeting.serde.PhotonStructSerializable;
/** Represents a pipeline result from a PhotonCamera. */
public class PhotonPipelineResult implements ProtobufSerializable {
public class PhotonPipelineResult
implements ProtobufSerializable, PhotonStructSerializable<PhotonPipelineResult> {
private static boolean HAS_WARNED = false;
// Image capture and NT publish timestamp, in microseconds and in the coprocessor timebase. As
// reported by WPIUtilJNI::now.
private long captureTimestampMicros = -1;
private long publishTimestampMicros = -1;
// Mirror of the heartbeat entry -- monotonically increasing
private long sequenceID = -1;
// Frame capture metadata
public PhotonPipelineMetadata metadata;
// Targets to store.
public final List<PhotonTrackedTarget> targets = new ArrayList<>();
public List<PhotonTrackedTarget> targets = new ArrayList<>();
// Multi-tag result
private MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
public Optional<MultiTargetPNPResult> multitagResult;
// Since we don't trust NT time sync, keep track of when we got this packet into robot code
private long ntRecieveTimestampMicros;
// 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() {}
public PhotonPipelineResult() {
this(new PhotonPipelineMetadata(), List.of(), Optional.empty());
}
/**
* Constructs a pipeline result.
@@ -63,10 +63,10 @@ public class PhotonPipelineResult implements ProtobufSerializable {
long captureTimestamp,
long publishTimestamp,
List<PhotonTrackedTarget> targets) {
this.captureTimestampMicros = captureTimestamp;
this.publishTimestampMicros = publishTimestamp;
this.sequenceID = sequenceID;
this.targets.addAll(targets);
this(
new PhotonPipelineMetadata(captureTimestamp, publishTimestamp, sequenceID),
targets,
Optional.empty());
}
/**
@@ -85,12 +85,20 @@ public class PhotonPipelineResult implements ProtobufSerializable {
long captureTimestamp,
long publishTimestamp,
List<PhotonTrackedTarget> targets,
MultiTargetPNPResult result) {
this.captureTimestampMicros = captureTimestamp;
this.publishTimestampMicros = publishTimestamp;
this.sequenceID = sequenceID;
Optional<MultiTargetPNPResult> result) {
this(
new PhotonPipelineMetadata(captureTimestamp, publishTimestamp, sequenceID),
targets,
result);
}
public PhotonPipelineResult(
PhotonPipelineMetadata metadata,
List<PhotonTrackedTarget> targets,
Optional<MultiTargetPNPResult> result) {
this.metadata = metadata;
this.targets.addAll(targets);
this.multiTagResult = result;
this.multitagResult = result;
}
/**
@@ -99,10 +107,11 @@ public class PhotonPipelineResult implements ProtobufSerializable {
* @return The size of the packet needed to store this pipeline result.
*/
public int getPacketSize() {
return Double.BYTES // latency
+ 1 // target count
+ targets.size() * PhotonTrackedTarget.serde.getMaxByteSize()
+ MultiTargetPNPResult.serde.getMaxByteSize();
throw new RuntimeException("TODO");
// return Double.BYTES // latency
// + 1 // target count
// + targets.size() * PhotonTrackedTarget.serde.getMaxByteSize()
// + MultiTargetPNPResult.serde.getMaxByteSize();
}
/**
@@ -124,50 +133,6 @@ public class PhotonPipelineResult implements ProtobufSerializable {
return hasTargets() ? targets.get(0) : null;
}
/** Returns the time between image capture and publish to NT */
public double getLatencyMillis() {
return (publishTimestampMicros - captureTimestampMicros) / 1e3;
}
/**
* Returns the estimated time the frame was taken, in the recieved system's time base. This is
* calculated as (NT recieve time (robot base) - (publish timestamp, coproc timebase - capture
* timestamp, coproc timebase))
*
* @return The timestamp in seconds
*/
public double getTimestampSeconds() {
return (ntRecieveTimestampMicros - (publishTimestampMicros - captureTimestampMicros)) / 1e6;
}
/** The time that this image was captured, in the coprocessor's time base. */
public long getCaptureTimestampMicros() {
return captureTimestampMicros;
}
/** The time that this result was published to NT, in the coprocessor's time base. */
public long getPublishTimestampMicros() {
return publishTimestampMicros;
}
/**
* The number of non-empty frames processed by this camera since boot. Useful to checking if a
* camera is alive.
*/
public long getSequenceID() {
return sequenceID;
}
/** The time that the robot recieved this result, in the FPGA timebase. */
public long getNtRecieveTimestampMicros() {
return ntRecieveTimestampMicros;
}
/** Sets the FPGA timestamp this result was recieved by robot code */
public void setRecieveTimestampMicros(long timestampMicros) {
this.ntRecieveTimestampMicros = timestampMicros;
}
/**
* Returns whether the pipeline has targets.
*
@@ -192,22 +157,54 @@ public class PhotonPipelineResult implements ProtobufSerializable {
* Return the latest multi-target result. Be sure to check
* getMultiTagResult().estimatedPose.isPresent before using the pose estimate!
*/
public MultiTargetPNPResult getMultiTagResult() {
return multiTagResult;
public Optional<MultiTargetPNPResult> getMultiTagResult() {
return multitagResult;
}
/**
* 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))
*
* @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;
}
@Override
public String toString() {
return "PhotonPipelineResult [metadata="
+ metadata
+ ", targets="
+ targets
+ ", multitagResult="
+ multitagResult
+ ", ntReceiveTimestampMicros="
+ ntReceiveTimestampMicros
+ "]";
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + (int) (captureTimestampMicros ^ (captureTimestampMicros >>> 32));
long temp;
temp = Double.doubleToLongBits(publishTimestampMicros);
result = prime * result + (int) (temp ^ (temp >>> 32));
result = prime * result + (int) (sequenceID ^ (sequenceID >>> 32));
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) (ntRecieveTimestampMicros ^ (ntRecieveTimestampMicros >>> 32));
result = prime * result + ((multitagResult == null) ? 0 : multitagResult.hashCode());
result = prime * result + (int) (ntReceiveTimestampMicros ^ (ntReceiveTimestampMicros >>> 32));
return result;
}
@@ -217,70 +214,24 @@ public class PhotonPipelineResult implements ProtobufSerializable {
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PhotonPipelineResult other = (PhotonPipelineResult) obj;
if (captureTimestampMicros != other.captureTimestampMicros) return false;
if (Double.doubleToLongBits(publishTimestampMicros)
!= Double.doubleToLongBits(other.publishTimestampMicros)) return false;
if (sequenceID != other.sequenceID) return false;
if (metadata == null) {
if (other.metadata != null) return false;
} else if (!metadata.equals(other.metadata)) return false;
if (targets == null) {
if (other.targets != null) return false;
} else if (!targets.equals(other.targets)) return false;
if (multiTagResult == null) {
if (other.multiTagResult != null) return false;
} else if (!multiTagResult.equals(other.multiTagResult)) return false;
if (ntRecieveTimestampMicros != other.ntRecieveTimestampMicros) return false;
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;
}
@Override
public String toString() {
return "PhotonPipelineResult [captureTimestamp="
+ captureTimestampMicros
+ ", publishTimestamp="
+ publishTimestampMicros
+ ", sequenceID="
+ sequenceID
+ ", targets="
+ targets
+ ", multiTagResult="
+ multiTagResult
+ ", ntRecieveTimestamp="
+ ntRecieveTimestampMicros
+ "]";
}
public static final class APacketSerde implements PacketSerde<PhotonPipelineResult> {
@Override
public int getMaxByteSize() {
// This uses dynamic packets so it doesn't matter
return -1;
}
@Override
public void pack(Packet packet, PhotonPipelineResult value) {
packet.encode(value.sequenceID);
packet.encode(value.captureTimestampMicros);
packet.encode(value.publishTimestampMicros);
packet.encode((byte) value.targets.size());
for (var target : value.targets) PhotonTrackedTarget.serde.pack(packet, target);
MultiTargetPNPResult.serde.pack(packet, value.multiTagResult);
}
@Override
public PhotonPipelineResult unpack(Packet packet) {
var seq = packet.decodeLong();
var cap = packet.decodeLong();
var pub = packet.decodeLong();
var len = packet.decodeByte();
var targets = new ArrayList<PhotonTrackedTarget>(len);
for (int i = 0; i < len; i++) {
targets.add(PhotonTrackedTarget.serde.unpack(packet));
}
var result = MultiTargetPNPResult.serde.unpack(packet);
return new PhotonPipelineResult(seq, cap, pub, targets, result);
}
}
public static final APacketSerde serde = new APacketSerde();
public static final PhotonPipelineResultSerde photonStruct = new PhotonPipelineResultSerde();
public static final PhotonPipelineResultProto proto = new PhotonPipelineResultProto();
@Override
public PacketSerde<PhotonPipelineResult> getSerde() {
return photonStruct;
}
}

116
photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
View File

@@ -19,32 +19,32 @@ package org.photonvision.targeting;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.PhotonTrackedTargetSerde;
import org.photonvision.targeting.proto.PhotonTrackedTargetProto;
import org.photonvision.utils.PacketUtils;
import org.photonvision.targeting.serde.PhotonStructSerializable;
public class PhotonTrackedTarget implements ProtobufSerializable {
public class PhotonTrackedTarget
implements ProtobufSerializable, PhotonStructSerializable<PhotonTrackedTarget> {
private static final int MAX_CORNERS = 8;
private final double yaw;
private final double pitch;
private final double area;
private final double skew;
private final int fiducialId;
private final int classId;
private final float objDetectConf;
private final Transform3d bestCameraToTarget;
private final Transform3d altCameraToTarget;
private final double poseAmbiguity;
public double yaw;
public double pitch;
public double area;
public double skew;
public int fiducialId;
public int objDetectId;
public float objDetectConf;
public Transform3d bestCameraToTarget;
public Transform3d altCameraToTarget;
public double poseAmbiguity;
// Corners from the min-area rectangle bounding the target
private final List<TargetCorner> minAreaRectCorners;
public List<TargetCorner> minAreaRectCorners;
// Corners from whatever corner detection method was used
private final List<TargetCorner> detectedCorners;
public List<TargetCorner> detectedCorners;
/** Construct a tracked target, given exactly 4 corners */
public PhotonTrackedTarget(
@@ -71,7 +71,7 @@ public class PhotonTrackedTarget implements ProtobufSerializable {
this.area = area;
this.skew = skew;
this.fiducialId = fiducialId;
this.classId = classId;
this.objDetectId = classId;
this.objDetectConf = objDetectConf;
this.bestCameraToTarget = pose;
this.altCameraToTarget = altPose;
@@ -80,6 +80,10 @@ public class PhotonTrackedTarget implements ProtobufSerializable {
this.poseAmbiguity = ambiguity;
}
public PhotonTrackedTarget() {
// TODO Auto-generated constructor stub
}
public double getYaw() {
return yaw;
}
@@ -103,7 +107,7 @@ public class PhotonTrackedTarget implements ProtobufSerializable {
/** Get the object detection class ID number, or -1 if not set. */
public int getDetectedObjectClassID() {
return classId;
return objDetectId;
}
/**
@@ -235,75 +239,11 @@ public class PhotonTrackedTarget implements ProtobufSerializable {
+ '}';
}
public static final class APacketSerde implements PacketSerde<PhotonTrackedTarget> {
@Override
public int getMaxByteSize() {
return Double.BYTES * (5 + 7 + 2 * 4 + 1 + 1 + 4 + 7 + 2 * MAX_CORNERS);
}
@Override
public void pack(Packet packet, PhotonTrackedTarget value) {
packet.encode(value.yaw);
packet.encode(value.pitch);
packet.encode(value.area);
packet.encode(value.skew);
packet.encode(value.fiducialId);
packet.encode(value.classId);
packet.encode(value.objDetectConf);
PacketUtils.packTransform3d(packet, value.bestCameraToTarget);
PacketUtils.packTransform3d(packet, value.altCameraToTarget);
packet.encode(value.poseAmbiguity);
for (int i = 0; i < 4; i++) {
TargetCorner.serde.pack(packet, value.minAreaRectCorners.get(i));
}
packet.encode((byte) Math.min(value.detectedCorners.size(), Byte.MAX_VALUE));
for (TargetCorner targetCorner : value.detectedCorners) {
TargetCorner.serde.pack(packet, targetCorner);
}
}
@Override
public PhotonTrackedTarget unpack(Packet packet) {
var yaw = packet.decodeDouble();
var pitch = packet.decodeDouble();
var area = packet.decodeDouble();
var skew = packet.decodeDouble();
var fiducialId = packet.decodeInt();
var classId = packet.decodeInt();
var objDetectConf = packet.decodeFloat();
Transform3d best = PacketUtils.unpackTransform3d(packet);
Transform3d alt = PacketUtils.unpackTransform3d(packet);
double ambiguity = packet.decodeDouble();
var minAreaRectCorners = new ArrayList<TargetCorner>(4);
for (int i = 0; i < 4; i++) {
minAreaRectCorners.add(TargetCorner.serde.unpack(packet));
}
var len = packet.decodeByte();
var detectedCorners = new ArrayList<TargetCorner>(len);
for (int i = 0; i < len; i++) {
detectedCorners.add(TargetCorner.serde.unpack(packet));
}
return new PhotonTrackedTarget(
yaw,
pitch,
area,
skew,
fiducialId,
classId,
objDetectConf,
best,
alt,
ambiguity,
minAreaRectCorners,
detectedCorners);
}
}
public static final APacketSerde serde = new APacketSerde();
public static final PhotonTrackedTargetProto proto = new PhotonTrackedTargetProto();
public static final PhotonTrackedTargetSerde photonStruct = new PhotonTrackedTargetSerde();
@Override
public PacketSerde<PhotonTrackedTarget> getSerde() {
return photonStruct;
}
}

81
photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java → photon-targeting/src/main/java/org/photonvision/targeting/PnpResult.java
View File

@@ -19,10 +19,10 @@ package org.photonvision.targeting;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.PnpResultSerde;
import org.photonvision.targeting.proto.PNPResultProto;
import org.photonvision.utils.PacketUtils;
import org.photonvision.targeting.serde.PhotonStructSerializable;
/**
* The best estimated transformation from solvePnP, and possibly an alternate transformation
@@ -33,37 +33,30 @@ import org.photonvision.utils.PacketUtils;
* <p>Note that the coordinate frame of these transforms depends on the implementing solvePnP
* method.
*/
public class PNPResult implements ProtobufSerializable {
/**
* If this result is valid. A false value indicates there was an error in estimation, and this
* result should not be used.
*/
public final boolean isPresent;
public class PnpResult implements ProtobufSerializable, PhotonStructSerializable<PnpResult> {
/**
* The best-fit transform. The coordinate frame of this transform depends on the method which gave
* this result.
*/
public final Transform3d best;
public Transform3d best;
/** Reprojection error of the best solution, in pixels */
public final double bestReprojErr;
public double bestReprojErr;
/**
* Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal
* to the best solution.
*/
public final Transform3d alt;
public Transform3d alt;
/** If no alternate solution is found, this is bestReprojErr */
public final double altReprojErr;
public double altReprojErr;
/** If no alternate solution is found, this is 0 */
public final double ambiguity;
public double ambiguity;
/** An empty (invalid) result. */
public PNPResult() {
this.isPresent = false;
public PnpResult() {
this.best = new Transform3d();
this.alt = new Transform3d();
this.ambiguity = 0;
@@ -71,17 +64,16 @@ public class PNPResult implements ProtobufSerializable {
this.altReprojErr = 0;
}
public PNPResult(Transform3d best, double bestReprojErr) {
public PnpResult(Transform3d best, double bestReprojErr) {
this(best, best, 0, bestReprojErr, bestReprojErr);
}
public PNPResult(
public PnpResult(
Transform3d best,
Transform3d alt,
double ambiguity,
double bestReprojErr,
double altReprojErr) {
this.isPresent = true;
this.best = best;
this.alt = alt;
this.ambiguity = ambiguity;
@@ -93,7 +85,6 @@ public class PNPResult implements ProtobufSerializable {
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + (isPresent ? 1231 : 1237);
result = prime * result + ((best == null) ? 0 : best.hashCode());
long temp;
temp = Double.doubleToLongBits(bestReprojErr);
@@ -111,8 +102,7 @@ public class PNPResult implements ProtobufSerializable {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PNPResult other = (PNPResult) obj;
if (isPresent != other.isPresent) return false;
PnpResult other = (PnpResult) obj;
if (best == null) {
if (other.best != null) return false;
} else if (!best.equals(other.best)) return false;
@@ -130,9 +120,7 @@ public class PNPResult implements ProtobufSerializable {
@Override
public String toString() {
return "PNPResult [isPresent="
+ isPresent
+ ", best="
return "PnpResult [best="
+ best
+ ", bestReprojErr="
+ bestReprojErr
@@ -145,42 +133,11 @@ public class PNPResult implements ProtobufSerializable {
+ "]";
}
public static final class APacketSerde implements PacketSerde<PNPResult> {
@Override
public int getMaxByteSize() {
return 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3);
}
@Override
public void pack(Packet packet, PNPResult value) {
packet.encode(value.isPresent);
if (value.isPresent) {
PacketUtils.packTransform3d(packet, value.best);
PacketUtils.packTransform3d(packet, value.alt);
packet.encode(value.bestReprojErr);
packet.encode(value.altReprojErr);
packet.encode(value.ambiguity);
}
}
@Override
public PNPResult unpack(Packet packet) {
var present = packet.decodeBoolean();
if (!present) {
return new PNPResult();
}
var best = PacketUtils.unpackTransform3d(packet);
var alt = PacketUtils.unpackTransform3d(packet);
var bestEr = packet.decodeDouble();
var altEr = packet.decodeDouble();
var ambiguity = packet.decodeDouble();
return new PNPResult(best, alt, ambiguity, bestEr, altEr);
}
}
public static final APacketSerde serde = new APacketSerde();
public static final PNPResultProto proto = new PNPResultProto();
public static final PnpResultSerde photonStruct = new PnpResultSerde();
@Override
public PacketSerde<PnpResult> getSerde() {
return photonStruct;
}
}

38
photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java
View File

@@ -19,23 +19,28 @@ package org.photonvision.targeting;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import java.util.Objects;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.struct.TargetCornerSerde;
import org.photonvision.targeting.proto.TargetCornerProto;
import org.photonvision.targeting.serde.PhotonStructSerializable;
/**
* Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels.
* Origin at the top left, plus-x to the right, plus-y down.
*/
public class TargetCorner implements ProtobufSerializable {
public final double x;
public final double y;
public class TargetCorner implements ProtobufSerializable, PhotonStructSerializable<TargetCorner> {
public double x;
public double y;
public TargetCorner(double cx, double cy) {
this.x = cx;
this.y = cy;
}
public TargetCorner() {
this(0, 0);
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
@@ -54,24 +59,11 @@ public class TargetCorner implements ProtobufSerializable {
return "(" + x + "," + y + ')';
}
public static final class APacketSerde implements PacketSerde<TargetCorner> {
@Override
public int getMaxByteSize() {
return Double.BYTES * 2;
}
@Override
public void pack(Packet packet, TargetCorner corner) {
packet.encode(corner.x);
packet.encode(corner.y);
}
@Override
public TargetCorner unpack(Packet packet) {
return new TargetCorner(packet.decodeDouble(), packet.decodeDouble());
}
}
public static final APacketSerde serde = new APacketSerde();
public static final TargetCornerProto proto = new TargetCornerProto();
public static final TargetCornerSerde photonStruct = new TargetCornerSerde();
@Override
public PacketSerde<TargetCorner> getSerde() {
return photonStruct;
}
}

12
photon-targeting/src/main/java/org/photonvision/targeting/proto/MultiTargetPNPResultProto.java
View File

@@ -21,7 +21,7 @@ import edu.wpi.first.util.protobuf.Protobuf;
import java.util.ArrayList;
import org.photonvision.proto.Photon.ProtobufMultiTargetPNPResult;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PnpResult;
import us.hebi.quickbuf.Descriptors.Descriptor;
import us.hebi.quickbuf.RepeatedInt;
@@ -39,7 +39,7 @@ public class MultiTargetPNPResultProto
@Override
public Protobuf<?, ?>[] getNested() {
return new Protobuf<?, ?>[] {PNPResult.proto};
return new Protobuf<?, ?>[] {PnpResult.proto};
}
@Override
@@ -49,17 +49,17 @@ public class MultiTargetPNPResultProto
@Override
public MultiTargetPNPResult unpack(ProtobufMultiTargetPNPResult msg) {
ArrayList<Integer> fidIdsUsed = new ArrayList<>(msg.getFiducialIdsUsed().length());
ArrayList<Short> fidIdsUsed = new ArrayList<>(msg.getFiducialIdsUsed().length());
for (var packedFidId : msg.getFiducialIdsUsed()) {
fidIdsUsed.add(packedFidId);
fidIdsUsed.add(packedFidId.shortValue());
}
return new MultiTargetPNPResult(PNPResult.proto.unpack(msg.getEstimatedPose()), fidIdsUsed);
return new MultiTargetPNPResult(PnpResult.proto.unpack(msg.getEstimatedPose()), fidIdsUsed);
}
@Override
public void pack(ProtobufMultiTargetPNPResult msg, MultiTargetPNPResult value) {
PNPResult.proto.pack(msg.getMutableEstimatedPose(), value.estimatedPose);
PnpResult.proto.pack(msg.getMutableEstimatedPose(), value.estimatedPose);
RepeatedInt idsUsed = msg.getMutableFiducialIdsUsed().reserve(value.fiducialIDsUsed.size());
for (int i = 0; i < value.fiducialIDsUsed.size(); i++) {

21
photon-targeting/src/main/java/org/photonvision/targeting/proto/PNPResultProto.java
View File

@@ -20,13 +20,13 @@ package org.photonvision.targeting.proto;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.util.protobuf.Protobuf;
import org.photonvision.proto.Photon.ProtobufPNPResult;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PnpResult;
import us.hebi.quickbuf.Descriptors.Descriptor;
public class PNPResultProto implements Protobuf<PNPResult, ProtobufPNPResult> {
public class PNPResultProto implements Protobuf<PnpResult, ProtobufPNPResult> {
@Override
public Class<PNPResult> getTypeClass() {
return PNPResult.class;
public Class<PnpResult> getTypeClass() {
return PnpResult.class;
}
@Override
@@ -45,12 +45,8 @@ public class PNPResultProto implements Protobuf<PNPResult, ProtobufPNPResult> {
}
@Override
public PNPResult unpack(ProtobufPNPResult msg) {
if (!msg.getIsPresent()) {
return new PNPResult();
}
return new PNPResult(
public PnpResult unpack(ProtobufPNPResult msg) {
return new PnpResult(
Transform3d.proto.unpack(msg.getBest()),
Transform3d.proto.unpack(msg.getAlt()),
msg.getAmbiguity(),
@@ -59,12 +55,11 @@ public class PNPResultProto implements Protobuf<PNPResult, ProtobufPNPResult> {
}
@Override
public void pack(ProtobufPNPResult msg, PNPResult value) {
public void pack(ProtobufPNPResult msg, PnpResult value) {
Transform3d.proto.pack(msg.getMutableBest(), value.best);
Transform3d.proto.pack(msg.getMutableAlt(), value.alt);
msg.setAmbiguity(value.ambiguity)
.setBestReprojErr(value.bestReprojErr)
.setAltReprojErr(value.altReprojErr)
.setIsPresent(value.isPresent);
.setAltReprojErr(value.altReprojErr);
}
}

Some files were not shown because too many files have changed in this diff Show More