We do all the changes at once

PhotonVision · Aug 18, 2024 · 8a5a8a5 · 8a5a8a5
1 parent 0493ef9
commit 8a5a8a5
Show file tree

Hide file tree

Showing 137 changed files with 4,374 additions and 2,039 deletions.
diff --git a/.gitignore b/.gitignore
@@ -165,3 +165,6 @@ photon-server/src/main/resources/web/index.html
 photon-lib/src/generate/native/cpp/PhotonVersion.cpp
 
 venv
+
+.venv/*
+.venv
diff --git a/build.gradle b/build.gradle
@@ -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()

diff --git a/devTools/calibrationUtils.py b/devTools/calibrationUtils.py
@@ -3,7 +3,6 @@
 from dataclasses import dataclass
 import json
 import os
-from typing import Union
 import cv2
 import numpy as np
 import mrcal

diff --git a/...on-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/...on-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
@@ -139,7 +139,8 @@ public void accept(CVPipelineResult result) {
                         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);
         }

diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java
@@ -62,13 +62,14 @@ public void accept(CVPipelineResult result) {
         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);
         }
 

diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java
@@ -20,6 +20,7 @@
 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 @@
 /** 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 @@ protected MultiTargetPNPResult process(List<TrackedTarget> targetList) {
                         "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 @@ private MultiTargetPNPResult calculateCameraInField(List<TrackedTarget> targetLi
                         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 {

diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
@@ -28,6 +28,7 @@
 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 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
         }
 
         // 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 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
                 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());

diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java
@@ -41,6 +41,7 @@
 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 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings)
         }
 
         // 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 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings)
                 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());

diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java
@@ -19,6 +19,7 @@
 
 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 CVPipelineResult(
             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,22 +52,15 @@ public CVPipelineResult(
             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(
             long sequenceID,
             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 CVPipelineResult(
             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 CVPipelineResult(
             double processingNanos,
             double fps,
             List<TrackedTarget> targets,
-            MultiTargetPNPResult multiTagResult) {
+            Optional<MultiTargetPNPResult> multiTagResult) {
         this(sequenceID, processingNanos, fps, targets, multiTagResult, null, List.of());
     }
 

diff --git a/photon-docs/build.gradle b/photon-docs/build.gradle
@@ -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

diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle
@@ -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'
+            ]
+        }
     }
 }
 

diff --git a/photon-lib/py/buildAndTest.sh b/photon-lib/py/buildAndTest.sh
@@ -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
diff --git a/photon-lib/py/photonlibpy/__init__.py b/photon-lib/py/photonlibpy/__init__.py
@@ -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
diff --git a/photon-lib/py/photonlibpy/estimatedRobotPose.py b/photon-lib/py/photonlibpy/estimatedRobotPose.py
@@ -3,7 +3,7 @@
 
 from wpimath.geometry import Pose3d
 
-from .photonTrackedTarget import PhotonTrackedTarget
+from .targeting.photonTrackedTarget import PhotonTrackedTarget
 
 if TYPE_CHECKING:
     from .photonPoseEstimator import PoseStrategy

diff --git a/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py b/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py
@@ -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()