fully working everything but not

PhotonVision · Nov 6, 2023 · 29818c4 · 29818c4
1 parent 381e756
commit 29818c4
Show file tree

Hide file tree

Showing 21 changed files with 379 additions and 157 deletions.
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
@@ -128,11 +128,11 @@ public void updateCameraNickname(String newCameraNickname) {
 
     @Override
     public void accept(CVPipelineResult result) {
-        var res = new PhotonPipelineResult(
-                result.getLatencyMillis(),
-                TrackedTarget.simpleFromTrackedTargets(result.targets),
-                result.multiTagResult
-        );
+        var res =
+                new PhotonPipelineResult(
+                        result.getLatencyMillis(),
+                        TrackedTarget.simpleFromTrackedTargets(result.targets),
+                        result.multiTagResult);
 
         ts.rawBytesEntry.set(res);
 

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
@@ -71,8 +71,8 @@ private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetL
                 VisionEstimation.estimateCamPosePNP(
                         params.cameraCoefficients.cameraIntrinsics.getAsWpilibMat(),
                         params.cameraCoefficients.distCoeffs.getAsWpilibMat(),
-                        List.of(TrackedTarget.simpleFromTrackedTargets(targetList)),
-                        params.atfl);
+                        TrackedTarget.simpleFromTrackedTargets(targetList),
+                        params.atfl,
                         params.targetModel);
 
         return new MultiTargetPNPResults(estimatedPose, tagIDsUsed);

diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
@@ -113,10 +113,11 @@ public TrackedTarget(
             tvec.put(
                     0,
                     0,
-                    bestPose.getTranslation().getX(),
-                    bestPose.getTranslation().getY(),
-                    bestPose.getTranslation().getZ()
-            );
+                    new double[] {
+                        bestPose.getTranslation().getX(),
+                        bestPose.getTranslation().getY(),
+                        bestPose.getTranslation().getZ()
+                    });
             setCameraRelativeTvec(tvec);
 
             // Opencv expects a 3d vector with norm = angle and direction = axis
@@ -203,10 +204,11 @@ public TrackedTarget(
             tvec.put(
                     0,
                     0,
-                    bestPose.getTranslation().getX(),
-                    bestPose.getTranslation().getY(),
-                    bestPose.getTranslation().getZ()
-            );
+                    new double[] {
+                        bestPose.getTranslation().getX(),
+                        bestPose.getTranslation().getY(),
+                        bestPose.getTranslation().getZ()
+                    });
             setCameraRelativeTvec(tvec);
 
             var rvec = new Mat(3, 1, CvType.CV_64FC1);

diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
@@ -51,8 +51,7 @@
 import java.util.Set;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.hardware.VisionLEDMode;
-import org.photonvision.proto.PhotonTypes.PhotonPipelineResult;
-import us.hebi.quickbuf.InvalidProtocolBufferException;
+import org.photonvision.targeting.PhotonPipelineResult;
 
 /** Represents a camera that is connected to PhotonVision. */
 public class PhotonCamera implements AutoCloseable {
@@ -141,12 +140,9 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) {
         path = cameraTable.getPath();
         rawBytesEntry =
                 cameraTable
-                        .getRawTopic("result_proto")
+                        .getRawTopic("rawBytes")
                         .subscribe(
-                                "proto:" + PhotonPipelineResult.getDescriptor().getFullName(),
-                                new byte[] {},
-                                PubSubOption.periodic(0.01),
-                                PubSubOption.sendAll(true));
+                                "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
         driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
         driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false);
         inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
@@ -187,26 +183,23 @@ public PhotonCamera(String cameraName) {
      * @return The latest pipeline result.
      */
     public PhotonPipelineResult getLatestResult() {
-        // verifyVersion(); // Protobufs _should_ deal with this for us
+        verifyVersion();
 
-        var ret = PhotonPipelineResult.newInstance();
+        // Clear the packet.
+        packet.clear();
 
-        var bytes = rawBytesEntry.get(new byte[] {});
-        if (bytes.length < 1) {
-            return PhotonPipelineResult.newInstance();
-        }
+        // Create latest result.
+        var ret = new PhotonPipelineResult();
 
-        try {
-            ret = PhotonPipelineResult.parseFrom(bytes);
-        } catch (InvalidProtocolBufferException e) {
-            // TODO Auto-generated catch block
-            e.printStackTrace();
-            return ret;
-        }
+        // Populate packet and create result.
+        packet.setData(rawBytesEntry.get(new byte[] {}));
+
+        if (packet.getSize() < 1) return ret;
+        ret.createFromPacket(packet);
 
         // Set the timestamp of the result.
         // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
-        ret.setTimestampSec((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMs() / 1e3);
+        ret.setTimestampSeconds((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3);
 
         // Return result.
         return ret;

diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
@@ -43,6 +43,8 @@
 import java.util.Set;
 import org.photonvision.estimation.TargetModel;
 import org.photonvision.estimation.VisionEstimation;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
 
 /**
  * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a

diff --git a/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java b/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java
@@ -25,7 +25,7 @@
 package org.photonvision;
 
 import java.util.Comparator;
-import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.PhotonTrackedTarget;
+import org.photonvision.targeting.PhotonTrackedTarget;
 
 public enum PhotonTargetSortMode {
     Smallest(Comparator.comparingDouble(PhotonTrackedTarget::getArea)),

diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
@@ -56,6 +56,8 @@
 import org.photonvision.estimation.VisionEstimation;
 import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.targeting.PNPResults;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
 
 /**
  * A handle for simulating {@link PhotonCamera} values. Processing simulated targets through this

diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java
@@ -38,6 +38,8 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.util.ArrayList;
 import java.util.List;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
 
 /**
  * @deprecated Use {@link VisionSystemSim} instead

diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
@@ -40,9 +40,9 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
                            const std::string_view cameraName)
     : mainTable(instance.GetTable("photonvision")),
       rootTable(mainTable->GetSubTable(cameraName)),
-      rawBytesEntry(rootTable->GetRawTopic("result_proto")
-                        .Subscribe("asdfasdfasdf", {},
-                                   {.periodic = 0.01, .sendAll = true})),
+      rawBytesEntry(
+          rootTable->GetRawTopic("rawBytes")
+              .Subscribe("rawBytes", {}, {.periodic = 0.01, .sendAll = true})),
       inputSaveImgEntry(
           rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()),
       inputSaveImgSubscriber(

diff --git a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h
@@ -48,8 +48,7 @@ class SimPhotonCamera : public PhotonCamera {
     targetAreaEntry = rootTable->GetEntry("targetAreaEntry");
     targetSkewEntry = rootTable->GetEntry("targetSkewEntry");
     targetPoseEntry = rootTable->GetEntry("targetPoseEntry");
-    rawBytesPublisher =
-        rootTable->GetRawTopic("result_proto").Publish("asdfasdfasdf");
+    rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("rawBytes");
     versionEntry = instance.GetTable("photonvision")->GetEntry("version");
     // versionEntry.SetString(PhotonVersion.versionString);
   }

diff --git a/photon-lib/src/test/java/org/photonvision/PacketTest.java b/photon-lib/src/test/java/org/photonvision/PacketTest.java
@@ -32,6 +32,9 @@
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.targeting.PNPResults;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
 
 class PacketTest {
     @Test

diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java
@@ -27,6 +27,7 @@
 import org.junit.jupiter.api.Assertions;
 import org.junit.jupiter.api.Test;
 import org.photonvision.common.dataflow.structures.Packet;
+import org.photonvision.targeting.PhotonPipelineResult;
 
 class PhotonCameraTest {
     @Test

diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
@@ -47,6 +47,9 @@
 import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
 
 class PhotonPoseEstimatorTest {
     static AprilTagFieldLayout aprilTags;

diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle
@@ -4,7 +4,6 @@ plugins {
     id 'edu.wpi.first.WpilibTools' version '1.3.0'
 }
 
-
 apply from: "${rootDir}/shared/common.gradle"
 
 dependencies {

diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
@@ -30,8 +30,6 @@
 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;
@@ -172,11 +170,8 @@ public static Point[] cornersToPoints(TargetCorner... corners) {
 
     public static List<TargetCorner> pointsToCorners(Point... points) {
         var corners = new ArrayList<TargetCorner>(points.length);
-        for (Point point : points) {
-            corners.add(new TargetCorner(
-                    point.x,
-                    point.y
-            ));
+        for (int i = 0; i < points.length; i++) {
+            corners.add(new TargetCorner(points[i].x, points[i].y));
         }
         return corners;
     }
@@ -406,7 +401,7 @@ public static Point[] getConvexHull(Point[] points) {
      * @return The resulting transformation that maps the camera pose to the target pose and the
      *     ambiguity if an alternate solution is available.
      */
-    public static Optional<PNPResults> solvePNP_SQUARE(
+    public static PNPResults solvePNP_SQUARE(
             Matrix<N3, N3> cameraMatrix,
             Matrix<N5, N1> distCoeffs,
             List<Translation3d> modelTrls,
@@ -471,16 +466,15 @@ public static Optional<PNPResults> solvePNP_SQUARE(
             // 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 Optional.of(new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]));
-            else return Optional.of(new PNPResults(best, errors[0]));
+                return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]);
+            else return new PNPResults(best, errors[0]);
         }
         // solvePnP failed
         catch (Exception e) {
             System.err.println("SolvePNP_SQUARE failed!");
             e.printStackTrace();
-            return Optional.empty();
+            return new PNPResults();
         } finally {
             // release our Mats from native memory
             objectMat.release();
@@ -515,7 +509,7 @@ public static Optional<PNPResults> solvePNP_SQUARE(
      *     model points are supplied relative to the origin, this transformation brings the camera to
      *     the origin.
      */
-    public static Optional<PNPResults> solvePNP_SQPNP(
+    public static PNPResults solvePNP_SQPNP(
             Matrix<N3, N3> cameraMatrix,
             Matrix<N5, N1> distCoeffs,
             List<Translation3d> objectTrls,
@@ -564,11 +558,11 @@ public static Optional<PNPResults> solvePNP_SQPNP(
             // check if solvePnP failed with NaN results
             if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result");
 
-            return Optional.of(new PNPResults(best, error[0]));
+            return new PNPResults(best, error[0]);
         } catch (Exception e) {
             System.err.println("SolvePNP_SQPNP failed!");
             e.printStackTrace();
-            return Optional.empty();
+            return new PNPResults();
         }
     }
 }
diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
@@ -27,7 +27,6 @@
 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.PNPResults;
@@ -67,17 +66,17 @@ public static List<AprilTag> getVisibleLayoutTags(
      * @return The transformation that maps the field origin to the camera pose. Ensure the {@link
      *     PNPResults} are present before utilizing them.
      */
-    public static Optional<PNPResults> estimateCamPosePNP(
+    public static PNPResults estimateCamPosePNP(
             Matrix<N3, N3> cameraMatrix,
             Matrix<N5, N1> distCoeffs,
             List<PhotonTrackedTarget> visTags,
             AprilTagFieldLayout tagLayout,
             TargetModel tagModel) {
         if (tagLayout == null
                 || visTags == null
-                || tagLayout.getTags().isEmpty()
-                || visTags.isEmpty()) {
-            return Optional.empty();
+                || tagLayout.getTags().size() == 0
+                || visTags.size() == 0) {
+            return new PNPResults();
         }
 
         var corners = new ArrayList<TargetCorner>();
@@ -93,43 +92,41 @@ public static Optional<PNPResults> estimateCamPosePNP(
                                 corners.addAll(tgt.getDetectedCorners());
                             });
         }
-        if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) {
-            return Optional.empty();
+        if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
+            return new PNPResults();
         }
         Point[] points = OpenCVHelp.cornersToPoints(corners);
 
         // single-tag pnp
         if (knownTags.size() == 1) {
-            var camToTagOpt =
+            var camToTag =
                     OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, tagModel.vertices, points);
-            if (camToTagOpt.isEmpty()) return Optional.empty();
-            var camToTag = camToTagOpt.get();
+            if (!camToTag.isPresent) return new PNPResults();
             var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse());
             var altPose = new Pose3d();
             if (camToTag.ambiguity != 0)
                 altPose = knownTags.get(0).pose.transformBy(camToTag.alt.inverse());
 
             var o = new Pose3d();
-            return Optional.of(new PNPResults(
+            return new PNPResults(
                     new Transform3d(o, bestPose),
                     new Transform3d(o, altPose),
                     camToTag.ambiguity,
                     camToTag.bestReprojErr,
-                    camToTag.altReprojErr));
+                    camToTag.altReprojErr);
         }
         // multi-tag pnp
         else {
             var objectTrls = new ArrayList<Translation3d>();
             for (var tag : knownTags) objectTrls.addAll(tagModel.getFieldVertices(tag.pose));
-            var camToOriginOpt = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points);
-            if (camToOriginOpt.isEmpty()) return Optional.empty();
-            var camToOrigin = camToOriginOpt.get();
-            return Optional.of(new PNPResults(
+            var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points);
+            if (!camToOrigin.isPresent) return new PNPResults();
+            return new PNPResults(
                     camToOrigin.best.inverse(),
                     camToOrigin.alt.inverse(),
                     camToOrigin.ambiguity,
                     camToOrigin.bestReprojErr,
-                    camToOrigin.altReprojErr));
+                    camToOrigin.altReprojErr);
         }
     }
 }