Maybe sorta half working

PhotonVision · Oct 21, 2023 · acaefba · acaefba
1 parent 2d5ccd6
commit acaefba
Show file tree

Hide file tree

Showing 25 changed files with 838 additions and 654 deletions.
diff --git a/photon-core/build.gradle b/photon-core/build.gradle
@@ -29,6 +29,7 @@ dependencies {
     implementation "org.xerial:sqlite-jdbc:3.41.0.0"
 }
 
+
 task writeCurrentVersionJava {
     def versionFileIn = file("${rootDir}/shared/PhotonVersion.java.in")
     writePhotonVersionFile(versionFileIn, Path.of("$projectDir", "src", "main", "java", "org", "photonvision", "PhotonVersion.java"),

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
@@ -27,7 +27,7 @@
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.networktables.NTTopicSet;
-import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.proto.PhotonTypes.PhotonPipelineResult;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
 import org.photonvision.vision.target.TrackedTarget;
 
@@ -129,15 +129,22 @@ public void updateCameraNickname(String newCameraNickname) {
 
     @Override
     public void accept(CVPipelineResult result) {
-        var simplified =
-                new PhotonPipelineResult(
-                        result.getLatencyMillis(),
-                        TrackedTarget.simpleFromTrackedTargets(result.targets),
-                        result.multiTagResult);
-        Packet packet = new Packet(simplified.getPacketSize());
-        simplified.populatePacket(packet);
-
-        ts.rawBytesEntry.set(packet.getData());
+        // var simplified =
+        //         new PhotonPipelineResult(
+        //                 result.getLatencyMillis(),
+        //                 TrackedTarget.simpleFromTrackedTargets(result.targets),
+        //                 result.multiTagResult);
+        // Packet packet = new Packet(simplified.getPacketSize());
+        // simplified.populatePacket(packet);
+
+        PhotonPipelineResult resultProto = PhotonPipelineResult.newInstance();
+        resultProto.setLatencyMs(result.getLatencyMillis());
+        resultProto.addAllTargets(TrackedTarget.simpleFromTrackedTargets(result.targets));
+        if (result.multiTagResult.estimatedPose.isPresent) {
+            resultProto.setMultiTargetResult(result.multiTagResult.toProto());
+        }
+
+        ts.rawBytesEntry.set(resultProto);
 
         ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get());
         ts.driverModePublisher.set(driverModeSupplier.getAsBoolean());

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
@@ -69,7 +69,7 @@ private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetL
                 VisionEstimation.estimateCamPosePNP(
                         params.cameraCoefficients.cameraIntrinsics.getAsWpilibMat(),
                         params.cameraCoefficients.distCoeffs.getAsWpilibMat(),
-                        TrackedTarget.simpleFromTrackedTargets(targetList),
+                        List.of(TrackedTarget.simpleFromTrackedTargets(targetList)),
                         params.atfl);
 
         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
@@ -22,6 +22,8 @@
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d;
+
 import java.util.ArrayList;
 import java.util.HashMap;
 import java.util.List;
@@ -33,8 +35,8 @@
 import org.opencv.core.RotatedRect;
 import org.photonvision.common.util.SerializationUtils;
 import org.photonvision.common.util.math.MathUtils;
-import org.photonvision.targeting.PhotonTrackedTarget;
-import org.photonvision.targeting.TargetCorner;
+import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.PhotonTrackedTarget;
+import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.TargetCorner;
 import org.photonvision.vision.aruco.ArucoDetectionResult;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.CVShape;
@@ -396,37 +398,64 @@ public boolean isFiducial() {
         return this.m_fiducialId >= 0;
     }
 
-    public static List<PhotonTrackedTarget> simpleFromTrackedTargets(List<TrackedTarget> targets) {
-        var ret = new ArrayList<PhotonTrackedTarget>();
-        for (var t : targets) {
-            var minAreaRectCorners = new ArrayList<TargetCorner>();
-            var detectedCorners = new ArrayList<TargetCorner>();
+    public static PhotonTrackedTarget[] simpleFromTrackedTargets(List<TrackedTarget> targets) {
+        var ret = new PhotonTrackedTarget[targets.size()];
+        for (int tgtIdx = 0; tgtIdx < targets.size(); tgtIdx++) {
+            var t = targets.get(tgtIdx);
+            var minAreaRectCorners = new TargetCorner[4];
+            var detectedCorners = new TargetCorner[t.getTargetCorners().size()];
             {
                 var points = new Point[4];
                 t.getMinAreaRect().points(points);
                 for (int i = 0; i < 4; i++) {
-                    minAreaRectCorners.add(new TargetCorner(points[i].x, points[i].y));
+                    var c = TargetCorner.newInstance();
+                    c.setX(points[i].x);
+                    c.setY(points[i].y);
+                    minAreaRectCorners[i] = c;
                 }
             }
             {
                 var points = t.getTargetCorners();
                 for (int i = 0; i < points.size(); i++) {
-                    detectedCorners.add(new TargetCorner(points.get(i).x, points.get(i).y));
+                    var c = TargetCorner.newInstance();
+                    c.setX(points.get(i).x);
+                    c.setY(points.get(i).y);
+                    detectedCorners[i] = c;
                 }
             }
 
-            ret.add(
-                    new PhotonTrackedTarget(
-                            t.getYaw(),
-                            t.getPitch(),
-                            t.getArea(),
-                            t.getSkew(),
-                            t.getFiducialId(),
-                            t.getBestCameraToTarget3d(),
-                            t.getAltCameraToTarget3d(),
-                            t.getPoseAmbiguity(),
-                            minAreaRectCorners,
-                            detectedCorners));
+            var tt = PhotonTrackedTarget.newInstance();
+
+            var best = Transform3d.proto.createMessage();
+            var alt = Transform3d.proto.createMessage();
+            Transform3d.proto.pack(best, t.getBestCameraToTarget3d());
+            Transform3d.proto.pack(alt, t.getAltCameraToTarget3d());
+
+            tt.setYaw(t.getYaw());
+            tt.setPitch(t.getPitch());
+            tt.setArea(t.getArea());
+            tt.setSkew(t.getSkew());
+            tt.setFiducialId(t.getFiducialId());
+            tt.setBestCameraToTarget(best);
+            tt.setAltCameraToTarget(alt);
+            tt.setPoseAmbiguity(t.getPoseAmbiguity());
+            tt.addAllMinAreaRectCorners(minAreaRectCorners);
+            tt.addAllDetectedCorners(detectedCorners);
+
+            ret[tgtIdx] = tt;
+
+            // ret.add(
+            //         new PhotonTrackedTarget(
+            //                 t.getYaw(),
+            //                 t.getPitch(),
+            //                 t.getArea(),
+            //                 t.getSkew(),
+            //                 t.getFiducialId(),
+            //                 t.getBestCameraToTarget3d(),
+            //                 t.getAltCameraToTarget3d(),
+            //                 t.getPoseAmbiguity(),
+            //                 minAreaRectCorners,
+            //                 detectedCorners));
         }
         return ret;
     }

diff --git a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
@@ -27,7 +27,7 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import java.util.List;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
-import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.PhotonTrackedTarget;
 
 /** An estimated pose based on pipeline result */
 public class EstimatedRobotPose {

diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
@@ -49,11 +49,13 @@
 import edu.wpi.first.networktables.StringSubscriber;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import us.hebi.quickbuf.InvalidProtocolBufferException;
+
 import java.util.Optional;
 import java.util.Set;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.hardware.VisionLEDMode;
-import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.proto.PhotonTypes.PhotonPipelineResult;
 
 /** Represents a camera that is connected to PhotonVision. */
 public class PhotonCamera implements AutoCloseable {
@@ -139,9 +141,9 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) {
         path = cameraTable.getPath();
         rawBytesEntry =
                 cameraTable
-                        .getRawTopic("rawBytes")
+                        .getRawTopic("result_proto")
                         .subscribe(
-                                "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
+                                "proto:" + PhotonPipelineResult.getDescriptor().getFullName(), 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);
@@ -182,23 +184,26 @@ public PhotonCamera(String cameraName) {
      * @return The latest pipeline result.
      */
     public PhotonPipelineResult getLatestResult() {
-        verifyVersion();
-
-        // Clear the packet.
-        packet.clear();
+        // verifyVersion(); // Protobufs _should_ deal with this for us
 
-        // Create latest result.
-        var ret = new PhotonPipelineResult();
+        var ret = PhotonPipelineResult.newInstance();
 
-        // Populate packet and create result.
-        packet.setData(rawBytesEntry.get(new byte[] {}));
+        var bytes = rawBytesEntry.get(new byte[] {});
+        if (bytes.length < 1) {
+            return PhotonPipelineResult.newInstance();
+        }
 
-        if (packet.getSize() < 1) return ret;
-        ret.createFromPacket(packet);
+        try {
+            ret = PhotonPipelineResult.parseFrom(bytes);
+        } catch (InvalidProtocolBufferException e) {
+            // TODO Auto-generated catch block
+            e.printStackTrace();
+            return ret;
+        }
 
         // Set the timestamp of the result.
         // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
-        ret.setTimestampSeconds((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3);
+        ret.setTimestampSec((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMs() / 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
@@ -42,8 +42,6 @@
 import java.util.Optional;
 import java.util.Set;
 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,8 @@
 package org.photonvision;
 
 import java.util.Comparator;
-import org.photonvision.targeting.PhotonTrackedTarget;
+
+import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.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
@@ -53,8 +53,6 @@
 import org.photonvision.estimation.RotTrlTransform3d;
 import org.photonvision.estimation.TargetModel;
 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,8 +38,6 @@
 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
@@ -41,8 +41,8 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
     : mainTable(instance.GetTable("photonvision")),
       rootTable(mainTable->GetSubTable(cameraName)),
       rawBytesEntry(
-          rootTable->GetRawTopic("rawBytes")
-              .Subscribe("rawBytes", {}, {.periodic = 0.01, .sendAll = true})),
+          rootTable->GetRawTopic("result_proto")
+              .Subscribe("asdfasdfasdf", {}, {.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,7 +48,7 @@ class SimPhotonCamera : public PhotonCamera {
     targetAreaEntry = rootTable->GetEntry("targetAreaEntry");
     targetSkewEntry = rootTable->GetEntry("targetSkewEntry");
     targetPoseEntry = rootTable->GetEntry("targetPoseEntry");
-    rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("rawBytes");
+    rawBytesPublisher = rootTable->GetRawTopic("result_proto").Publish("asdfasdfasdf");
     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,9 +32,6 @@
 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,7 +27,6 @@
 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,9 +47,6 @@
 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
@@ -1,3 +1,6 @@
+plugins {
+    id 'com.google.protobuf' version '0.9.3'
+}
 apply plugin: "java"
 
 apply from: "${rootDir}/shared/common.gradle"
@@ -13,6 +16,16 @@ dependencies {
     testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2")
 }
 
+sourceSets {
+    main {
+        java {
+            // TODO this is a hack! I think! Why are we generating wpilib quickbufs anyways?
+            srcDirs 'build/generated/source/proto/main/quickbuf/org'
+            exclude 'build/generated/source/proto/main/quickbuf/edu/wpi/**'
+        }
+    }
+}
+
 tasks.withType(JavaCompile) {
     options.encoding = 'UTF-8'
 }
@@ -29,4 +42,29 @@ test {
     useJUnitPlatform()
 }
 
+
+protobuf {
+    protoc {
+        artifact = 'com.google.protobuf:protoc:3.21.12'
+    }
+    plugins {
+        quickbuf {
+            artifact = 'us.hebi.quickbuf:protoc-gen-quickbuf:1.3.2'
+        }
+    }
+    generateProtoTasks {
+        all().configureEach { task ->
+            task.builtins {
+                cpp {}
+                remove java
+            }
+            task.plugins {
+                quickbuf {
+                    option "gen_descriptors=true"
+                }
+            }
+        }
+    }
+}
+
 apply from: "publish.gradle"