Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-24 01:31:44 +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

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