From c95feb29cef6b03e406bfde0a988b82009a3bbaa Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 20 Oct 2023 18:57:54 -0400 Subject: [PATCH 01/74] Bump to beta 2 --- build.gradle | 2 +- photonlib-cpp-examples/aimandrange/build.gradle | 2 +- photonlib-cpp-examples/aimattarget/build.gradle | 2 +- photonlib-cpp-examples/apriltagExample/build.gradle | 2 +- photonlib-cpp-examples/getinrange/build.gradle | 2 +- photonlib-java-examples/aimandrange/build.gradle | 2 +- photonlib-java-examples/aimattarget/build.gradle | 2 +- photonlib-java-examples/getinrange/build.gradle | 2 +- photonlib-java-examples/simaimandrange/build.gradle | 2 +- photonlib-java-examples/swervedriveposeestsim/build.gradle | 2 +- 10 files changed, 10 insertions(+), 10 deletions(-) diff --git a/build.gradle b/build.gradle index 199729e75a..f593638b90 100644 --- a/build.gradle +++ b/build.gradle @@ -26,7 +26,7 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2024.1.1-beta-1" + wpilibVersion = "2024.1.1-beta-2" openCVversion = "4.8.0-1" joglVersion = "2.4.0-rc-20200307" javalinVersion = "5.6.2" diff --git a/photonlib-cpp-examples/aimandrange/build.gradle b/photonlib-cpp-examples/aimandrange/build.gradle index ecedfafb40..758d419773 100644 --- a/photonlib-cpp-examples/aimandrange/build.gradle +++ b/photonlib-cpp-examples/aimandrange/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-cpp-examples/aimattarget/build.gradle b/photonlib-cpp-examples/aimattarget/build.gradle index ecedfafb40..758d419773 100644 --- a/photonlib-cpp-examples/aimattarget/build.gradle +++ b/photonlib-cpp-examples/aimattarget/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-cpp-examples/apriltagExample/build.gradle b/photonlib-cpp-examples/apriltagExample/build.gradle index ecedfafb40..758d419773 100644 --- a/photonlib-cpp-examples/apriltagExample/build.gradle +++ b/photonlib-cpp-examples/apriltagExample/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-cpp-examples/getinrange/build.gradle b/photonlib-cpp-examples/getinrange/build.gradle index ecedfafb40..758d419773 100644 --- a/photonlib-cpp-examples/getinrange/build.gradle +++ b/photonlib-cpp-examples/getinrange/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-java-examples/aimandrange/build.gradle b/photonlib-java-examples/aimandrange/build.gradle index 7d6e58895a..21bf49feb0 100644 --- a/photonlib-java-examples/aimandrange/build.gradle +++ b/photonlib-java-examples/aimandrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/aimattarget/build.gradle b/photonlib-java-examples/aimattarget/build.gradle index d98e56b952..6d2ef660c1 100644 --- a/photonlib-java-examples/aimattarget/build.gradle +++ b/photonlib-java-examples/aimattarget/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/getinrange/build.gradle b/photonlib-java-examples/getinrange/build.gradle index 7d6e58895a..21bf49feb0 100644 --- a/photonlib-java-examples/getinrange/build.gradle +++ b/photonlib-java-examples/getinrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/simaimandrange/build.gradle b/photonlib-java-examples/simaimandrange/build.gradle index 7d6e58895a..21bf49feb0 100644 --- a/photonlib-java-examples/simaimandrange/build.gradle +++ b/photonlib-java-examples/simaimandrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/swervedriveposeestsim/build.gradle b/photonlib-java-examples/swervedriveposeestsim/build.gradle index 424e9fa54a..3d15af12d1 100644 --- a/photonlib-java-examples/swervedriveposeestsim/build.gradle +++ b/photonlib-java-examples/swervedriveposeestsim/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" } sourceCompatibility = JavaVersion.VERSION_11 From 397d28d46cf0dce9825e933b8175b3943167fba6 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 20 Oct 2023 19:27:58 -0400 Subject: [PATCH 02/74] Reduce copy paste spam going forwards --- photonlib-cpp-examples/aimandrange/build.gradle | 2 +- photonlib-cpp-examples/aimattarget/build.gradle | 2 +- photonlib-cpp-examples/apriltagExample/build.gradle | 2 +- photonlib-cpp-examples/build.gradle | 1 + photonlib-cpp-examples/getinrange/build.gradle | 2 +- photonlib-java-examples/aimandrange/build.gradle | 2 +- photonlib-java-examples/aimattarget/build.gradle | 2 +- photonlib-java-examples/build.gradle | 1 + photonlib-java-examples/getinrange/build.gradle | 2 +- photonlib-java-examples/simaimandrange/build.gradle | 2 +- .../swervedriveposeestsim/build.gradle | 2 +- shared/common.gradle | 3 +++ shared/examples_common.gradle | 8 -------- 13 files changed, 14 insertions(+), 17 deletions(-) diff --git a/photonlib-cpp-examples/aimandrange/build.gradle b/photonlib-cpp-examples/aimandrange/build.gradle index 758d419773..c1c32d0284 100644 --- a/photonlib-cpp-examples/aimandrange/build.gradle +++ b/photonlib-cpp-examples/aimandrange/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" + id "edu.wpi.first.GradleRIO" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-cpp-examples/aimattarget/build.gradle b/photonlib-cpp-examples/aimattarget/build.gradle index 758d419773..c1c32d0284 100644 --- a/photonlib-cpp-examples/aimattarget/build.gradle +++ b/photonlib-cpp-examples/aimattarget/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" + id "edu.wpi.first.GradleRIO" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-cpp-examples/apriltagExample/build.gradle b/photonlib-cpp-examples/apriltagExample/build.gradle index 758d419773..c1c32d0284 100644 --- a/photonlib-cpp-examples/apriltagExample/build.gradle +++ b/photonlib-cpp-examples/apriltagExample/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" + id "edu.wpi.first.GradleRIO" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-cpp-examples/build.gradle b/photonlib-cpp-examples/build.gradle index 0d262a1504..f86d65a4ae 100644 --- a/photonlib-cpp-examples/build.gradle +++ b/photonlib-cpp-examples/build.gradle @@ -1,5 +1,6 @@ plugins { id "com.diffplug.spotless" version "6.1.2" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" apply false } allprojects { diff --git a/photonlib-cpp-examples/getinrange/build.gradle b/photonlib-cpp-examples/getinrange/build.gradle index 758d419773..c1c32d0284 100644 --- a/photonlib-cpp-examples/getinrange/build.gradle +++ b/photonlib-cpp-examples/getinrange/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" + id "edu.wpi.first.GradleRIO" id "com.dorongold.task-tree" version "2.1.0" } diff --git a/photonlib-java-examples/aimandrange/build.gradle b/photonlib-java-examples/aimandrange/build.gradle index 21bf49feb0..7dda3b59b0 100644 --- a/photonlib-java-examples/aimandrange/build.gradle +++ b/photonlib-java-examples/aimandrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" + id "edu.wpi.first.GradleRIO" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/aimattarget/build.gradle b/photonlib-java-examples/aimattarget/build.gradle index 6d2ef660c1..1442f184c9 100644 --- a/photonlib-java-examples/aimattarget/build.gradle +++ b/photonlib-java-examples/aimattarget/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" + id "edu.wpi.first.GradleRIO" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/build.gradle b/photonlib-java-examples/build.gradle index 35f533e690..68dd2983a2 100644 --- a/photonlib-java-examples/build.gradle +++ b/photonlib-java-examples/build.gradle @@ -1,5 +1,6 @@ plugins { id "com.diffplug.spotless" version "6.1.2" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" apply false } apply from: "examples.gradle" diff --git a/photonlib-java-examples/getinrange/build.gradle b/photonlib-java-examples/getinrange/build.gradle index 21bf49feb0..7dda3b59b0 100644 --- a/photonlib-java-examples/getinrange/build.gradle +++ b/photonlib-java-examples/getinrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" + id "edu.wpi.first.GradleRIO" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/simaimandrange/build.gradle b/photonlib-java-examples/simaimandrange/build.gradle index 21bf49feb0..7dda3b59b0 100644 --- a/photonlib-java-examples/simaimandrange/build.gradle +++ b/photonlib-java-examples/simaimandrange/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" + id "edu.wpi.first.GradleRIO" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/photonlib-java-examples/swervedriveposeestsim/build.gradle b/photonlib-java-examples/swervedriveposeestsim/build.gradle index 3d15af12d1..d0be7323ec 100644 --- a/photonlib-java-examples/swervedriveposeestsim/build.gradle +++ b/photonlib-java-examples/swervedriveposeestsim/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-2" + id "edu.wpi.first.GradleRIO" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/shared/common.gradle b/shared/common.gradle index e8181507e3..cb4ce3f80b 100644 --- a/shared/common.gradle +++ b/shared/common.gradle @@ -43,6 +43,9 @@ dependencies { // test stuff testImplementation("org.junit.jupiter:junit-jupiter:5.8.2") + + // wpilib serde + implementation 'us.hebi.quickbuf:quickbuf-runtime:1.3.2' } test { diff --git a/shared/examples_common.gradle b/shared/examples_common.gradle index b908c64b5e..5212307f0f 100644 --- a/shared/examples_common.gradle +++ b/shared/examples_common.gradle @@ -1,14 +1,6 @@ task copyPhotonlib() { doFirst { // Assume publish to maven local has just be run. Kinda curst - // // first, invoke gradle to generate the vendor json - // try { - // exec { - // commandLine 'gradlew', 'photon-lib:generateVendorJson', '-x', "check" - // } - // } catch (Exception e) { - // // ignored - // } def vendorJsonSrc = new File("${rootDir}", "../photon-lib/build/generated/vendordeps/"); def vendorJsonDst = new File("${projectDir}/vendordeps/"); From 2d5ccd6715b0bed3acf5abde804c3a5ffaf10e4e Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 20 Oct 2023 19:29:53 -0400 Subject: [PATCH 03/74] Bump gradle --- gradle/wrapper/gradle-wrapper.properties | 2 +- .../apriltagExample/simgui-window.json | 14 +++++++------- .../gradle/wrapper/gradle-wrapper.properties | 2 +- .../gradle/wrapper/gradle-wrapper.properties | 2 +- .../gradle/wrapper/gradle-wrapper.properties | 2 +- 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 7f959b087a..24c4386e93 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.3-bin\.zip +distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.4-bin\.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists diff --git a/photonlib-cpp-examples/apriltagExample/simgui-window.json b/photonlib-cpp-examples/apriltagExample/simgui-window.json index 79d676b16b..0f521f36e0 100644 --- a/photonlib-cpp-examples/apriltagExample/simgui-window.json +++ b/photonlib-cpp-examples/apriltagExample/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1652", - "xpos": "268", - "ypos": "82" + "xpos": "568", + "ypos": "336" } }, "Table": { @@ -30,12 +30,12 @@ "###FMS": { "Collapsed": "0", "Pos": "36,663", - "Size": "283,146" + "Size": "336,164" }, "###Joysticks": { "Collapsed": "0", "Pos": "373,500", - "Size": "796,206" + "Size": "976,239" }, "###Keyboard 0 Settings": { "Collapsed": "0", @@ -60,12 +60,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "5,350", - "Size": "192,218" + "Size": "232,254" }, "###Timing": { "Collapsed": "0", "Pos": "5,150", - "Size": "135,127" + "Size": "162,142" }, "Debug##Default": { "Collapsed": "0", @@ -75,7 +75,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "92,99" + "Size": "109,114" } } } diff --git a/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties b/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties index 7f959b087a..24c4386e93 100644 --- a/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties +++ b/photonlib-cpp-examples/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.3-bin\.zip +distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.4-bin\.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists diff --git a/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties index 7f959b087a..24c4386e93 100644 --- a/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties +++ b/photonlib-java-examples/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.3-bin\.zip +distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.4-bin\.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists diff --git a/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties b/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties index d3fb630184..ca8f28abd3 100644 --- a/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties +++ b/photonlib-java-examples/swervedriveposeestsim/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.3-bin\.zip +distributionUrl=https\://services\.gradle\.org/distributions/gradle-8\.4-bin\.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=permwrapper/dists From acaefbaed62ef5427228184790a3098094c7a4f9 Mon Sep 17 00:00:00 2001 From: Matt Date: Sat, 21 Oct 2023 10:07:38 -0400 Subject: [PATCH 04/74] Maybe sorta half working --- photon-core/build.gradle | 1 + .../networktables/NTDataPublisher.java | 27 +- .../vision/pipe/impl/MultiTargetPNPPipe.java | 2 +- .../vision/target/TrackedTarget.java | 71 ++- .../org/photonvision/EstimatedRobotPose.java | 2 +- .../java/org/photonvision/PhotonCamera.java | 33 +- .../org/photonvision/PhotonPoseEstimator.java | 2 - .../photonvision/PhotonTargetSortMode.java | 3 +- .../simulation/PhotonCameraSim.java | 2 - .../simulation/SimVisionSystem.java | 2 - .../native/cpp/photonlib/PhotonCamera.cpp | 4 +- .../include/photonlib/SimPhotonCamera.h | 2 +- .../java/org/photonvision/PacketTest.java | 3 - .../org/photonvision/PhotonCameraTest.java | 1 - .../photonvision/PhotonPoseEstimatorTest.java | 3 - photon-targeting/build.gradle | 38 ++ .../common/networktables/NTTopicSet.java | 10 +- .../photonvision/estimation/OpenCVHelp.java | 16 +- .../estimation/VisionEstimation.java | 8 +- .../targeting/MultiTargetPNPResults.java | 22 + .../targeting/PhotonPipelineResult.java | 484 ++++++++------- .../targeting/PhotonPipelineResultProto.java | 36 ++ .../targeting/PhotonTrackedTarget.java | 586 +++++++++--------- .../photonvision/targeting/TargetCorner.java | 90 +-- .../src/main/proto/photon_types.proto | 44 ++ 25 files changed, 838 insertions(+), 654 deletions(-) create mode 100644 photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResultProto.java create mode 100644 photon-targeting/src/main/proto/photon_types.proto diff --git a/photon-core/build.gradle b/photon-core/build.gradle index 5ebef599a4..314c4adca8 100644 --- 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/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index d985c710f5..2951a96288 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-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 index 5ce9f124cc..a37c2b24c3 100644 --- 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 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 index 9be1e9162c..1a136d2ffe 100644 --- 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 simpleFromTrackedTargets(List targets) { - var ret = new ArrayList(); - for (var t : targets) { - var minAreaRectCorners = new ArrayList(); - var detectedCorners = new ArrayList(); + public static PhotonTrackedTarget[] simpleFromTrackedTargets(List 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 index a355ab00f3..2b5d65253c 100644 --- 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 index 9500629ca3..dfac7e2e97 100644 --- 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 index ed29a9c265..d75799b39f 100644 --- 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 index bf22644645..958e009abd 100644 --- 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 index 1c9ef2ce1e..ebd6a53c19 100644 --- 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 index 52756f4727..3e6147625d 100644 --- 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 index d0901b10bd..c4b55424ef 100644 --- 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 index 44ab0917a2..dac3ba8e04 100644 --- 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 index c29a629b5b..1224f0cff8 100644 --- 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 index 8a3fb5fad8..77e18a6d8a 100644 --- 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 index 1b386cc747..6f1f523686 100644 --- 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 index e4cc2d7d96..bb8c8b08a4 100644 --- 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" diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index 5a0d31eaed..c4e817c9c1 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -17,6 +17,9 @@ package org.photonvision.common.networktables; +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult; +import org.photonvision.targeting.PhotonPipelineResultProto; + import edu.wpi.first.networktables.BooleanPublisher; import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.networktables.BooleanTopic; @@ -26,6 +29,7 @@ import edu.wpi.first.networktables.IntegerSubscriber; import edu.wpi.first.networktables.IntegerTopic; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.ProtobufPublisher; import edu.wpi.first.networktables.PubSubOption; import edu.wpi.first.networktables.RawPublisher; @@ -39,7 +43,7 @@ */ public class NTTopicSet { public NetworkTable subTable; - public RawPublisher rawBytesEntry; + public ProtobufPublisher rawBytesEntry; public IntegerPublisher pipelineIndexPublisher; public IntegerSubscriber pipelineIndexRequestSub; @@ -71,8 +75,8 @@ public class NTTopicSet { public void updateEntries() { rawBytesEntry = subTable - .getRawTopic("rawBytes") - .publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); + .getProtobufTopic("result_proto", PhotonPipelineResultProto.proto) + .publish(PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish(); pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0); diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 3f89918d62..80f18943fc 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -45,8 +45,8 @@ import org.opencv.core.Rect; import org.opencv.core.RotatedRect; import org.opencv.imgproc.Imgproc; +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.TargetCorner; import org.photonvision.targeting.PNPResults; -import org.photonvision.targeting.TargetCorner; public final class OpenCVHelp { private static RotTrlTransform3d NWU_TO_EDN; @@ -160,7 +160,7 @@ public static Point[] cornersToPoints(List corners) { var points = new Point[corners.size()]; for (int i = 0; i < corners.size(); i++) { var corn = corners.get(i); - points[i] = new Point(corn.x, corn.y); + points[i] = new Point(corn.getX(), corn.getY()); } return points; } @@ -168,7 +168,7 @@ public static Point[] cornersToPoints(List corners) { public static Point[] cornersToPoints(TargetCorner... corners) { var points = new Point[corners.length]; for (int i = 0; i < corners.length; i++) { - points[i] = new Point(corners[i].x, corners[i].y); + points[i] = new Point(corners[i].getX(), corners[i].getY()); } return points; } @@ -176,7 +176,10 @@ public static Point[] cornersToPoints(TargetCorner... corners) { public static List pointsToCorners(Point... points) { var corners = new ArrayList(points.length); for (int i = 0; i < points.length; i++) { - corners.add(new TargetCorner(points[i].x, points[i].y)); + var c = TargetCorner.newInstance(); + c.setX(points[i].x); + c.setY(points[i].y); + corners.add(c); } return corners; } @@ -186,7 +189,10 @@ public static List pointsToCorners(MatOfPoint2f matInput) { float[] data = new float[(int) matInput.total() * matInput.channels()]; matInput.get(0, 0, data); for (int i = 0; i < (int) matInput.total(); i++) { - corners.add(new TargetCorner(data[0 + 2 * i], data[1 + 2 * i])); + var c = TargetCorner.newInstance(); + c.setX(data[0 + 2 * i]); + c.setY(data[1 + 2 * i]); + corners.add(c); } return corners; } diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java index 387176d57b..760c7a23fa 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -29,9 +29,9 @@ import java.util.Objects; import java.util.stream.Collectors; import org.opencv.core.Point; +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.PhotonTrackedTarget; +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.TargetCorner; import org.photonvision.targeting.PNPResults; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; public class VisionEstimation { /** Get the visible {@link AprilTag}s which are in the tag layout using the visible tag IDs. */ @@ -88,7 +88,9 @@ public static PNPResults estimateCamPosePNP( .ifPresent( pose -> { knownTags.add(new AprilTag(id, pose)); - corners.addAll(tgt.getDetectedCorners()); + for (var c : tgt.getDetectedCorners()) { + corners.add(c); + } }); } if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java index 453c0a57e5..0d2fa0b214 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java @@ -21,6 +21,8 @@ import java.util.List; import org.photonvision.common.dataflow.structures.Packet; +import edu.wpi.first.math.geometry.Transform3d; + public class MultiTargetPNPResults { // Seeing 32 apriltags at once seems like a sane limit private static final int MAX_IDS = 32; @@ -90,4 +92,24 @@ public String toString() { + fiducialIDsUsed + "]"; } + + public org.photonvision.proto.PhotonTypes.PhotonPipelineResult.MultiTargetPNPResults toProto() { + var ret = org.photonvision.proto.PhotonTypes.PhotonPipelineResult.MultiTargetPNPResults.newInstance(); + + var best = Transform3d.proto.createMessage(); + var alt = Transform3d.proto.createMessage(); + Transform3d.proto.pack(best, this.estimatedPose.best); + Transform3d.proto.pack(alt, this.estimatedPose.alt); + ret.setBest(best); + ret.setAlt(alt); + ret.setBestReprojErr(this.estimatedPose.bestReprojErr); + ret.setAltReprojErr(this.estimatedPose.altReprojErr); + ret.setAmbiguity(this.estimatedPose.ambiguity); + int[] ids = new int[this.fiducialIDsUsed.size()]; + + for (int i = 0; i < ids.length; i++) ids[i] = this.fiducialIDsUsed.get(i); + ret.addAllFiducialIdsUsed(ids); + + return ret; + } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 4472078f8a..a6f51f6241 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -1,241 +1,243 @@ -/* - * 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 . - */ - -package org.photonvision.targeting; - -import java.util.ArrayList; -import java.util.List; -import org.photonvision.common.dataflow.structures.Packet; - -/** Represents a pipeline result from a PhotonCamera. */ -public class PhotonPipelineResult { - private static boolean HAS_WARNED = false; - - // Targets to store. - public final List targets = new ArrayList<>(); - - // Latency in milliseconds. - private double latencyMillis; - - // Timestamp in milliseconds. - private double timestampSeconds = -1; - - // Multi-tag result - private MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults(); - - /** Constructs an empty pipeline result. */ - public PhotonPipelineResult() {} - - /** - * Constructs a pipeline result. - * - * @param latencyMillis The latency in the pipeline. - * @param targets The list of targets identified by the pipeline. - */ - public PhotonPipelineResult(double latencyMillis, List targets) { - this.latencyMillis = latencyMillis; - this.targets.addAll(targets); - } - - /** - * Constructs a pipeline result. - * - * @param latencyMillis The latency in the pipeline. - * @param targets The list of targets identified by the pipeline. - * @param result Result from multi-target PNP. - */ - public PhotonPipelineResult( - double latencyMillis, List targets, MultiTargetPNPResults result) { - this.latencyMillis = latencyMillis; - this.targets.addAll(targets); - this.multiTagResult = result; - } - - /** - * Returns the size of the packet needed to store this pipeline result. - * - * @return The size of the packet needed to store this pipeline result. - */ - public int getPacketSize() { - return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES - + 8 // latency - + MultiTargetPNPResults.PACK_SIZE_BYTES - + 1; // target count - } - - /** - * Returns the best target in this pipeline result. If there are no targets, this method will - * return null. The best target is determined by the target sort mode in the PhotonVision UI. - * - * @return The best target of the pipeline result. - */ - public PhotonTrackedTarget getBestTarget() { - if (!hasTargets() && !HAS_WARNED) { - String errStr = - "This PhotonPipelineResult object has no targets associated with it! Please check hasTargets() " - + "before calling this method. For more information, please review the PhotonLib " - + "documentation at http://docs.photonvision.org"; - System.err.println(errStr); - new Exception().printStackTrace(); - HAS_WARNED = true; - } - return hasTargets() ? targets.get(0) : null; - } - - /** - * Returns the latency in the pipeline. - * - * @return The latency in the pipeline. - */ - public double getLatencyMillis() { - return latencyMillis; - } - - /** - * Returns the estimated time the frame was taken, This is more accurate than using - * getLatencyMillis() - * - * @return The timestamp in seconds, or -1 if this result has no timestamp set. - */ - public double getTimestampSeconds() { - return timestampSeconds; - } - - /** - * Sets the FPGA timestamp of this result in seconds. - * - * @param timestampSeconds The timestamp in seconds. - */ - public void setTimestampSeconds(double timestampSeconds) { - this.timestampSeconds = timestampSeconds; - } - - /** - * Returns whether the pipeline has targets. - * - * @return Whether the pipeline has targets. - */ - public boolean hasTargets() { - return targets.size() > 0; - } - - /** - * Returns a copy of the vector of targets. - * - * @return A copy of the vector of targets. - */ - public List getTargets() { - return new ArrayList<>(targets); - } - - /** - * Return the latest mulit-target result. Be sure to check - * getMultiTagResult().estimatedPose.isPresent before using the pose estimate! - */ - public MultiTargetPNPResults getMultiTagResult() { - return multiTagResult; - } - - /** - * Populates the fields of the pipeline result from the packet. - * - * @param packet The incoming packet. - * @return The incoming packet. - */ - public Packet createFromPacket(Packet packet) { - // Decode latency, existence of targets, and number of targets. - latencyMillis = packet.decodeDouble(); - this.multiTagResult = MultiTargetPNPResults.createFromPacket(packet); - byte targetCount = packet.decodeByte(); - - targets.clear(); - - // Decode the information of each target. - for (int i = 0; i < (int) targetCount; ++i) { - var target = new PhotonTrackedTarget(); - target.createFromPacket(packet); - targets.add(target); - } - - return packet; - } - - /** - * Populates the outgoing packet with information from this pipeline result. - * - * @param packet The outgoing packet. - * @return The outgoing packet. - */ - public Packet populatePacket(Packet packet) { - // Encode latency, existence of targets, and number of targets. - packet.encode(latencyMillis); - multiTagResult.populatePacket(packet); - packet.encode((byte) targets.size()); - - // Encode the information of each target. - for (var target : targets) target.populatePacket(packet); - - // Return the packet. - return packet; - } - - @Override - public int hashCode() { - final int prime = 31; - int result = 1; - result = prime * result + ((targets == null) ? 0 : targets.hashCode()); - long temp; - temp = Double.doubleToLongBits(latencyMillis); - result = prime * result + (int) (temp ^ (temp >>> 32)); - temp = Double.doubleToLongBits(timestampSeconds); - result = prime * result + (int) (temp ^ (temp >>> 32)); - result = prime * result + ((multiTagResult == null) ? 0 : multiTagResult.hashCode()); - return result; - } - - @Override - public boolean equals(Object obj) { - if (this == obj) return true; - if (obj == null) return false; - if (getClass() != obj.getClass()) return false; - PhotonPipelineResult other = (PhotonPipelineResult) obj; - if (targets == null) { - if (other.targets != null) return false; - } else if (!targets.equals(other.targets)) return false; - if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis)) - return false; - if (Double.doubleToLongBits(timestampSeconds) - != Double.doubleToLongBits(other.timestampSeconds)) return false; - if (multiTagResult == null) { - if (other.multiTagResult != null) return false; - } else if (!multiTagResult.equals(other.multiTagResult)) return false; - return true; - } - - @Override - public String toString() { - return "PhotonPipelineResult [targets=" - + targets - + ", latencyMillis=" - + latencyMillis - + ", timestampSeconds=" - + timestampSeconds - + ", multiTagResult=" - + multiTagResult - + "]"; - } -} +// /* +// * 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 . +// */ + +// package org.photonvision.targeting; + +// import java.util.ArrayList; +// import java.util.List; +// import org.photonvision.common.dataflow.structures.Packet; + +// import org.photonvision.proto.PhotonTypes; + +// /** Represents a pipeline result from a PhotonCamera. */ +// public class PhotonPipelineResult { +// private static boolean HAS_WARNED = false; + +// // Targets to store. +// public final List targets = new ArrayList<>(); + +// // Latency in milliseconds. +// private double latencyMillis; + +// // Timestamp in milliseconds. +// private double timestampSeconds = -1; + +// // Multi-tag result +// private MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults(); + +// /** Constructs an empty pipeline result. */ +// public PhotonPipelineResult() {} + +// /** +// * Constructs a pipeline result. +// * +// * @param latencyMillis The latency in the pipeline. +// * @param targets The list of targets identified by the pipeline. +// */ +// public PhotonPipelineResult(double latencyMillis, List targets) { +// this.latencyMillis = latencyMillis; +// this.targets.addAll(targets); +// } + +// /** +// * Constructs a pipeline result. +// * +// * @param latencyMillis The latency in the pipeline. +// * @param targets The list of targets identified by the pipeline. +// * @param result Result from multi-target PNP. +// */ +// public PhotonPipelineResult( +// double latencyMillis, List targets, MultiTargetPNPResults result) { +// this.latencyMillis = latencyMillis; +// this.targets.addAll(targets); +// this.multiTagResult = result; +// } + +// /** +// * Returns the size of the packet needed to store this pipeline result. +// * +// * @return The size of the packet needed to store this pipeline result. +// */ +// public int getPacketSize() { +// return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES +// + 8 // latency +// + MultiTargetPNPResults.PACK_SIZE_BYTES +// + 1; // target count +// } + +// /** +// * Returns the best target in this pipeline result. If there are no targets, this method will +// * return null. The best target is determined by the target sort mode in the PhotonVision UI. +// * +// * @return The best target of the pipeline result. +// */ +// public PhotonTrackedTarget getBestTarget() { +// if (!hasTargets() && !HAS_WARNED) { +// String errStr = +// "This PhotonPipelineResult object has no targets associated with it! Please check hasTargets() " +// + "before calling this method. For more information, please review the PhotonLib " +// + "documentation at http://docs.photonvision.org"; +// System.err.println(errStr); +// new Exception().printStackTrace(); +// HAS_WARNED = true; +// } +// return hasTargets() ? targets.get(0) : null; +// } + +// /** +// * Returns the latency in the pipeline. +// * +// * @return The latency in the pipeline. +// */ +// public double getLatencyMillis() { +// return latencyMillis; +// } + +// /** +// * Returns the estimated time the frame was taken, This is more accurate than using +// * getLatencyMillis() +// * +// * @return The timestamp in seconds, or -1 if this result has no timestamp set. +// */ +// public double getTimestampSeconds() { +// return timestampSeconds; +// } + +// /** +// * Sets the FPGA timestamp of this result in seconds. +// * +// * @param timestampSeconds The timestamp in seconds. +// */ +// public void setTimestampSeconds(double timestampSeconds) { +// this.timestampSeconds = timestampSeconds; +// } + +// /** +// * Returns whether the pipeline has targets. +// * +// * @return Whether the pipeline has targets. +// */ +// public boolean hasTargets() { +// return targets.size() > 0; +// } + +// /** +// * Returns a copy of the vector of targets. +// * +// * @return A copy of the vector of targets. +// */ +// public List getTargets() { +// return new ArrayList<>(targets); +// } + +// /** +// * Return the latest mulit-target result. Be sure to check +// * getMultiTagResult().estimatedPose.isPresent before using the pose estimate! +// */ +// public MultiTargetPNPResults getMultiTagResult() { +// return multiTagResult; +// } + +// /** +// * Populates the fields of the pipeline result from the packet. +// * +// * @param packet The incoming packet. +// * @return The incoming packet. +// */ +// public Packet createFromPacket(Packet packet) { +// // Decode latency, existence of targets, and number of targets. +// latencyMillis = packet.decodeDouble(); +// this.multiTagResult = MultiTargetPNPResults.createFromPacket(packet); +// byte targetCount = packet.decodeByte(); + +// targets.clear(); + +// // Decode the information of each target. +// for (int i = 0; i < (int) targetCount; ++i) { +// var target = new PhotonTrackedTarget(); +// target.createFromPacket(packet); +// targets.add(target); +// } + +// return packet; +// } + +// /** +// * Populates the outgoing packet with information from this pipeline result. +// * +// * @param packet The outgoing packet. +// * @return The outgoing packet. +// */ +// public Packet populatePacket(Packet packet) { +// // Encode latency, existence of targets, and number of targets. +// packet.encode(latencyMillis); +// multiTagResult.populatePacket(packet); +// packet.encode((byte) targets.size()); + +// // Encode the information of each target. +// for (var target : targets) target.populatePacket(packet); + +// // Return the packet. +// return packet; +// } + +// @Override +// public int hashCode() { +// final int prime = 31; +// int result = 1; +// result = prime * result + ((targets == null) ? 0 : targets.hashCode()); +// long temp; +// temp = Double.doubleToLongBits(latencyMillis); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// temp = Double.doubleToLongBits(timestampSeconds); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// result = prime * result + ((multiTagResult == null) ? 0 : multiTagResult.hashCode()); +// return result; +// } + +// @Override +// public boolean equals(Object obj) { +// if (this == obj) return true; +// if (obj == null) return false; +// if (getClass() != obj.getClass()) return false; +// PhotonPipelineResult other = (PhotonPipelineResult) obj; +// if (targets == null) { +// if (other.targets != null) return false; +// } else if (!targets.equals(other.targets)) return false; +// if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis)) +// return false; +// if (Double.doubleToLongBits(timestampSeconds) +// != Double.doubleToLongBits(other.timestampSeconds)) return false; +// if (multiTagResult == null) { +// if (other.multiTagResult != null) return false; +// } else if (!multiTagResult.equals(other.multiTagResult)) return false; +// return true; +// } + +// @Override +// public String toString() { +// return "PhotonPipelineResult [targets=" +// + targets +// + ", latencyMillis=" +// + latencyMillis +// + ", timestampSeconds=" +// + timestampSeconds +// + ", multiTagResult=" +// + multiTagResult +// + "]"; +// } +// } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResultProto.java new file mode 100644 index 0000000000..9c13b951a9 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResultProto.java @@ -0,0 +1,36 @@ +package org.photonvision.targeting; + +import org.photonvision.proto.PhotonTypes.PhotonPipelineResult; + +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class PhotonPipelineResultProto implements Protobuf { + + @Override + public Class getTypeClass() { + return PhotonPipelineResult.class; + } + + @Override + public Descriptor getDescriptor() { + return PhotonPipelineResult.getDescriptor(); + } + + @Override + public PhotonPipelineResult createMessage() { + return PhotonPipelineResult.newInstance(); + } + + @Override + public PhotonPipelineResult unpack(PhotonPipelineResult msg) { + return msg; + } + + @Override + public void pack(PhotonPipelineResult msg, PhotonPipelineResult value) { + msg.copyFrom(value); + } + + public static final PhotonPipelineResultProto proto = new PhotonPipelineResultProto(); +} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 40263f5cc5..08a5571ef7 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -1,293 +1,293 @@ -/* - * 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 . - */ - -package org.photonvision.targeting; - -import edu.wpi.first.math.geometry.Transform3d; -import java.util.ArrayList; -import java.util.List; -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.utils.PacketUtils; - -public class PhotonTrackedTarget { - private static final int MAX_CORNERS = 8; - public static final int PACK_SIZE_BYTES = - Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS); - - private double yaw; - private double pitch; - private double area; - private double skew; - private int fiducialId; - private Transform3d bestCameraToTarget = new Transform3d(); - private Transform3d altCameraToTarget = new Transform3d(); - private double poseAmbiguity; - - // Corners from the min-area rectangle bounding the target - private List minAreaRectCorners; - - // Corners from whatever corner detection method was used - private List detectedCorners; - - public PhotonTrackedTarget() {} - - /** Construct a tracked target, given exactly 4 corners */ - public PhotonTrackedTarget( - double yaw, - double pitch, - double area, - double skew, - int id, - Transform3d pose, - Transform3d altPose, - double ambiguity, - List minAreaRectCorners, - List detectedCorners) { - assert minAreaRectCorners.size() == 4; - - if (detectedCorners.size() > MAX_CORNERS) { - detectedCorners = detectedCorners.subList(0, MAX_CORNERS); - } - - this.yaw = yaw; - this.pitch = pitch; - this.area = area; - this.skew = skew; - this.fiducialId = id; - this.bestCameraToTarget = pose; - this.altCameraToTarget = altPose; - this.minAreaRectCorners = minAreaRectCorners; - this.detectedCorners = detectedCorners; - this.poseAmbiguity = ambiguity; - } - - public double getYaw() { - return yaw; - } - - public double getPitch() { - return pitch; - } - - public double getArea() { - return area; - } - - public double getSkew() { - return skew; - } - - /** Get the Fiducial ID, or -1 if not set. */ - public int getFiducialId() { - return fiducialId; - } - - /** - * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be - * ambiguous. -1 if invalid. - */ - public double getPoseAmbiguity() { - return poseAmbiguity; - } - - /** - * Return a list of the 4 corners in image space (origin top left, x right, y down), in no - * particular order, of the minimum area bounding rectangle of this target - */ - public List getMinAreaRectCorners() { - return minAreaRectCorners; - } - - /** - * Return a list of the n corners in image space (origin top left, x right, y down), in no - * particular order, detected for this target. - * - *

For fiducials, the order is known and is always counter-clock wise around the tag, like so: - * - * 

​
-     * ⟶ +X  3 ----- 2
-     * |      |       |
-     * V      |       |
-     * +Y     0 ----- 1
-     *
- */ - public List getDetectedCorners() { - return detectedCorners; - } - - /** - * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag - * space (X forward, Y left, Z up) with the lowest reprojection error - */ - public Transform3d getBestCameraToTarget() { - return bestCameraToTarget; - } - - /** - * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag - * space (X forward, Y left, Z up) with the highest reprojection error - */ - public Transform3d getAlternateCameraToTarget() { - return altCameraToTarget; - } - - @Override - public int hashCode() { - final int prime = 31; - int result = 1; - long temp; - temp = Double.doubleToLongBits(yaw); - result = prime * result + (int) (temp ^ (temp >>> 32)); - temp = Double.doubleToLongBits(pitch); - result = prime * result + (int) (temp ^ (temp >>> 32)); - temp = Double.doubleToLongBits(area); - result = prime * result + (int) (temp ^ (temp >>> 32)); - temp = Double.doubleToLongBits(skew); - result = prime * result + (int) (temp ^ (temp >>> 32)); - result = prime * result + fiducialId; - result = prime * result + ((bestCameraToTarget == null) ? 0 : bestCameraToTarget.hashCode()); - result = prime * result + ((altCameraToTarget == null) ? 0 : altCameraToTarget.hashCode()); - temp = Double.doubleToLongBits(poseAmbiguity); - result = prime * result + (int) (temp ^ (temp >>> 32)); - result = prime * result + ((minAreaRectCorners == null) ? 0 : minAreaRectCorners.hashCode()); - result = prime * result + ((detectedCorners == null) ? 0 : detectedCorners.hashCode()); - return result; - } - - @Override - public boolean equals(Object obj) { - if (this == obj) return true; - if (obj == null) return false; - if (getClass() != obj.getClass()) return false; - PhotonTrackedTarget other = (PhotonTrackedTarget) obj; - if (Double.doubleToLongBits(yaw) != Double.doubleToLongBits(other.yaw)) return false; - if (Double.doubleToLongBits(pitch) != Double.doubleToLongBits(other.pitch)) return false; - if (Double.doubleToLongBits(area) != Double.doubleToLongBits(other.area)) return false; - if (Double.doubleToLongBits(skew) != Double.doubleToLongBits(other.skew)) return false; - if (fiducialId != other.fiducialId) return false; - if (bestCameraToTarget == null) { - if (other.bestCameraToTarget != null) return false; - } else if (!bestCameraToTarget.equals(other.bestCameraToTarget)) return false; - if (altCameraToTarget == null) { - if (other.altCameraToTarget != null) return false; - } else if (!altCameraToTarget.equals(other.altCameraToTarget)) return false; - if (Double.doubleToLongBits(poseAmbiguity) != Double.doubleToLongBits(other.poseAmbiguity)) - return false; - if (minAreaRectCorners == null) { - if (other.minAreaRectCorners != null) return false; - } else if (!minAreaRectCorners.equals(other.minAreaRectCorners)) return false; - if (detectedCorners == null) { - if (other.detectedCorners != null) return false; - } else if (!detectedCorners.equals(other.detectedCorners)) return false; - return true; - } - - private static void encodeList(Packet packet, List list) { - packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE)); - for (int i = 0; i < list.size(); i++) { - packet.encode(list.get(i).x); - packet.encode(list.get(i).y); - } - } - - private static List decodeList(Packet p) { - byte len = p.decodeByte(); - var ret = new ArrayList(); - for (int i = 0; i < len; i++) { - double cx = p.decodeDouble(); - double cy = p.decodeDouble(); - ret.add(new TargetCorner(cx, cy)); - } - return ret; - } - - /** - * Populates the fields of this class with information from the incoming packet. - * - * @param packet The incoming packet. - * @return The incoming packet. - */ - public Packet createFromPacket(Packet packet) { - this.yaw = packet.decodeDouble(); - this.pitch = packet.decodeDouble(); - this.area = packet.decodeDouble(); - this.skew = packet.decodeDouble(); - this.fiducialId = packet.decodeInt(); - - this.bestCameraToTarget = PacketUtils.decodeTransform(packet); - this.altCameraToTarget = PacketUtils.decodeTransform(packet); - - this.poseAmbiguity = packet.decodeDouble(); - - this.minAreaRectCorners = new ArrayList<>(4); - for (int i = 0; i < 4; i++) { - double cx = packet.decodeDouble(); - double cy = packet.decodeDouble(); - minAreaRectCorners.add(new TargetCorner(cx, cy)); - } - - detectedCorners = decodeList(packet); - - return packet; - } - - /** - * Populates the outgoing packet with information from the current target. - * - * @param packet The outgoing packet. - * @return The outgoing packet. - */ - public Packet populatePacket(Packet packet) { - packet.encode(yaw); - packet.encode(pitch); - packet.encode(area); - packet.encode(skew); - packet.encode(fiducialId); - PacketUtils.encodeTransform(packet, bestCameraToTarget); - PacketUtils.encodeTransform(packet, altCameraToTarget); - packet.encode(poseAmbiguity); - - for (int i = 0; i < 4; i++) { - packet.encode(minAreaRectCorners.get(i).x); - packet.encode(minAreaRectCorners.get(i).y); - } - - encodeList(packet, detectedCorners); - - return packet; - } - - @Override - public String toString() { - return "PhotonTrackedTarget{" - + "yaw=" - + yaw - + ", pitch=" - + pitch - + ", area=" - + area - + ", skew=" - + skew - + ", fiducialId=" - + fiducialId - + ", cameraToTarget=" - + bestCameraToTarget - + ", targetCorners=" - + minAreaRectCorners - + '}'; - } -} +// /* +// * 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 . +// */ + +// package org.photonvision.targeting; + +// import edu.wpi.first.math.geometry.Transform3d; +// import java.util.ArrayList; +// import java.util.List; +// import org.photonvision.common.dataflow.structures.Packet; +// import org.photonvision.utils.PacketUtils; + +// public class PhotonTrackedTarget { +// private static final int MAX_CORNERS = 8; +// public static final int PACK_SIZE_BYTES = +// Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS); + +// private double yaw; +// private double pitch; +// private double area; +// private double skew; +// private int fiducialId; +// private Transform3d bestCameraToTarget = new Transform3d(); +// private Transform3d altCameraToTarget = new Transform3d(); +// private double poseAmbiguity; + +// // Corners from the min-area rectangle bounding the target +// private List minAreaRectCorners; + +// // Corners from whatever corner detection method was used +// private List detectedCorners; + +// public PhotonTrackedTarget() {} + +// /** Construct a tracked target, given exactly 4 corners */ +// public PhotonTrackedTarget( +// double yaw, +// double pitch, +// double area, +// double skew, +// int id, +// Transform3d pose, +// Transform3d altPose, +// double ambiguity, +// List minAreaRectCorners, +// List detectedCorners) { +// assert minAreaRectCorners.size() == 4; + +// if (detectedCorners.size() > MAX_CORNERS) { +// detectedCorners = detectedCorners.subList(0, MAX_CORNERS); +// } + +// this.yaw = yaw; +// this.pitch = pitch; +// this.area = area; +// this.skew = skew; +// this.fiducialId = id; +// this.bestCameraToTarget = pose; +// this.altCameraToTarget = altPose; +// this.minAreaRectCorners = minAreaRectCorners; +// this.detectedCorners = detectedCorners; +// this.poseAmbiguity = ambiguity; +// } + +// public double getYaw() { +// return yaw; +// } + +// public double getPitch() { +// return pitch; +// } + +// public double getArea() { +// return area; +// } + +// public double getSkew() { +// return skew; +// } + +// /** Get the Fiducial ID, or -1 if not set. */ +// public int getFiducialId() { +// return fiducialId; +// } + +// /** +// * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be +// * ambiguous. -1 if invalid. +// */ +// public double getPoseAmbiguity() { +// return poseAmbiguity; +// } + +// /** +// * Return a list of the 4 corners in image space (origin top left, x right, y down), in no +// * particular order, of the minimum area bounding rectangle of this target +// */ +// public List getMinAreaRectCorners() { +// return minAreaRectCorners; +// } + +// /** +// * Return a list of the n corners in image space (origin top left, x right, y down), in no +// * particular order, detected for this target. +// * +// *

For fiducials, the order is known and is always counter-clock wise around the tag, like so: +// * +// * 

​
+//      * ⟶ +X  3 ----- 2
+//      * |      |       |
+//      * V      |       |
+//      * +Y     0 ----- 1
+//      *
+// */ +// public List getDetectedCorners() { +// return detectedCorners; +// } + +// /** +// * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag +// * space (X forward, Y left, Z up) with the lowest reprojection error +// */ +// public Transform3d getBestCameraToTarget() { +// return bestCameraToTarget; +// } + +// /** +// * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag +// * space (X forward, Y left, Z up) with the highest reprojection error +// */ +// public Transform3d getAlternateCameraToTarget() { +// return altCameraToTarget; +// } + +// @Override +// public int hashCode() { +// final int prime = 31; +// int result = 1; +// long temp; +// temp = Double.doubleToLongBits(yaw); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// temp = Double.doubleToLongBits(pitch); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// temp = Double.doubleToLongBits(area); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// temp = Double.doubleToLongBits(skew); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// result = prime * result + fiducialId; +// result = prime * result + ((bestCameraToTarget == null) ? 0 : bestCameraToTarget.hashCode()); +// result = prime * result + ((altCameraToTarget == null) ? 0 : altCameraToTarget.hashCode()); +// temp = Double.doubleToLongBits(poseAmbiguity); +// result = prime * result + (int) (temp ^ (temp >>> 32)); +// result = prime * result + ((minAreaRectCorners == null) ? 0 : minAreaRectCorners.hashCode()); +// result = prime * result + ((detectedCorners == null) ? 0 : detectedCorners.hashCode()); +// return result; +// } + +// @Override +// public boolean equals(Object obj) { +// if (this == obj) return true; +// if (obj == null) return false; +// if (getClass() != obj.getClass()) return false; +// PhotonTrackedTarget other = (PhotonTrackedTarget) obj; +// if (Double.doubleToLongBits(yaw) != Double.doubleToLongBits(other.yaw)) return false; +// if (Double.doubleToLongBits(pitch) != Double.doubleToLongBits(other.pitch)) return false; +// if (Double.doubleToLongBits(area) != Double.doubleToLongBits(other.area)) return false; +// if (Double.doubleToLongBits(skew) != Double.doubleToLongBits(other.skew)) return false; +// if (fiducialId != other.fiducialId) return false; +// if (bestCameraToTarget == null) { +// if (other.bestCameraToTarget != null) return false; +// } else if (!bestCameraToTarget.equals(other.bestCameraToTarget)) return false; +// if (altCameraToTarget == null) { +// if (other.altCameraToTarget != null) return false; +// } else if (!altCameraToTarget.equals(other.altCameraToTarget)) return false; +// if (Double.doubleToLongBits(poseAmbiguity) != Double.doubleToLongBits(other.poseAmbiguity)) +// return false; +// if (minAreaRectCorners == null) { +// if (other.minAreaRectCorners != null) return false; +// } else if (!minAreaRectCorners.equals(other.minAreaRectCorners)) return false; +// if (detectedCorners == null) { +// if (other.detectedCorners != null) return false; +// } else if (!detectedCorners.equals(other.detectedCorners)) return false; +// return true; +// } + +// private static void encodeList(Packet packet, List list) { +// packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE)); +// for (int i = 0; i < list.size(); i++) { +// packet.encode(list.get(i).x); +// packet.encode(list.get(i).y); +// } +// } + +// private static List decodeList(Packet p) { +// byte len = p.decodeByte(); +// var ret = new ArrayList(); +// for (int i = 0; i < len; i++) { +// double cx = p.decodeDouble(); +// double cy = p.decodeDouble(); +// ret.add(new TargetCorner(cx, cy)); +// } +// return ret; +// } + +// /** +// * Populates the fields of this class with information from the incoming packet. +// * +// * @param packet The incoming packet. +// * @return The incoming packet. +// */ +// public Packet createFromPacket(Packet packet) { +// this.yaw = packet.decodeDouble(); +// this.pitch = packet.decodeDouble(); +// this.area = packet.decodeDouble(); +// this.skew = packet.decodeDouble(); +// this.fiducialId = packet.decodeInt(); + +// this.bestCameraToTarget = PacketUtils.decodeTransform(packet); +// this.altCameraToTarget = PacketUtils.decodeTransform(packet); + +// this.poseAmbiguity = packet.decodeDouble(); + +// this.minAreaRectCorners = new ArrayList<>(4); +// for (int i = 0; i < 4; i++) { +// double cx = packet.decodeDouble(); +// double cy = packet.decodeDouble(); +// minAreaRectCorners.add(new TargetCorner(cx, cy)); +// } + +// detectedCorners = decodeList(packet); + +// return packet; +// } + +// /** +// * Populates the outgoing packet with information from the current target. +// * +// * @param packet The outgoing packet. +// * @return The outgoing packet. +// */ +// public Packet populatePacket(Packet packet) { +// packet.encode(yaw); +// packet.encode(pitch); +// packet.encode(area); +// packet.encode(skew); +// packet.encode(fiducialId); +// PacketUtils.encodeTransform(packet, bestCameraToTarget); +// PacketUtils.encodeTransform(packet, altCameraToTarget); +// packet.encode(poseAmbiguity); + +// for (int i = 0; i < 4; i++) { +// packet.encode(minAreaRectCorners.get(i).x); +// packet.encode(minAreaRectCorners.get(i).y); +// } + +// encodeList(packet, detectedCorners); + +// return packet; +// } + +// @Override +// public String toString() { +// return "PhotonTrackedTarget{" +// + "yaw=" +// + yaw +// + ", pitch=" +// + pitch +// + ", area=" +// + area +// + ", skew=" +// + skew +// + ", fiducialId=" +// + fiducialId +// + ", cameraToTarget=" +// + bestCameraToTarget +// + ", targetCorners=" +// + minAreaRectCorners +// + '}'; +// } +// } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index 9808079a5d..36eb04b258 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -1,52 +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 . - */ +// /* +// * 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 . +// */ -package org.photonvision.targeting; +// package org.photonvision.targeting; -import java.util.Objects; +// import java.util.Objects; -/** - * 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 { - public final double x; - public final double y; +// /** +// * 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 { +// public final double x; +// public final double y; - public TargetCorner(double cx, double cy) { - this.x = cx; - this.y = cy; - } +// public TargetCorner(double cx, double cy) { +// this.x = cx; +// this.y = cy; +// } - @Override - public boolean equals(Object o) { - if (this == o) return true; - if (o == null || getClass() != o.getClass()) return false; - TargetCorner that = (TargetCorner) o; - return Double.compare(that.x, x) == 0 && Double.compare(that.y, y) == 0; - } +// @Override +// public boolean equals(Object o) { +// if (this == o) return true; +// if (o == null || getClass() != o.getClass()) return false; +// TargetCorner that = (TargetCorner) o; +// return Double.compare(that.x, x) == 0 && Double.compare(that.y, y) == 0; +// } - @Override - public int hashCode() { - return Objects.hash(x, y); - } +// @Override +// public int hashCode() { +// return Objects.hash(x, y); +// } - @Override - public String toString() { - return "(" + x + "," + y + ')'; - } -} +// @Override +// public String toString() { +// return "(" + x + "," + y + ')'; +// } +// } diff --git a/photon-targeting/src/main/proto/photon_types.proto b/photon-targeting/src/main/proto/photon_types.proto new file mode 100644 index 0000000000..b328bed18e --- /dev/null +++ b/photon-targeting/src/main/proto/photon_types.proto @@ -0,0 +1,44 @@ +syntax = "proto3"; + +package photonvision.proto; + +option java_package = "org.photonvision.proto"; + +import "geometry3d.proto"; + +message PhotonPipelineResult { + double latency_ms = 1; + double timestamp_sec = 2; + + message TargetCorner { + double x = 1; + double y = 2; + } + + message PhotonTrackedTarget { + double yaw = 1; + double pitch = 2; + double area = 3; + double skew = 4; + int32 fiducialId = 5; + wpi.proto.ProtobufTransform3d bestCameraToTarget = 6; + wpi.proto.ProtobufTransform3d altCameraToTarget = 7; + double poseAmbiguity = 8; + repeated TargetCorner minAreaRectCorners = 9; + repeated TargetCorner detectedCorners = 10; + } + + message MultiTargetPNPResults { + wpi.proto.ProtobufTransform3d best = 1; + double bestReprojErr = 2; + optional wpi.proto.ProtobufTransform3d alt = 3; + optional double altReprojErr = 4; + double ambiguity = 5; + + repeated int32 fiducial_ids_used = 6; + } + + repeated PhotonTrackedTarget targets = 3; + MultiTargetPNPResults multi_target_result = 4; +} + From a5f69e17c0961c1a5f17c2fd33613322a6ad8186 Mon Sep 17 00:00:00 2001 From: Matt Date: Tue, 24 Oct 2023 21:45:08 -0400 Subject: [PATCH 05/74] Remove optional --- photon-targeting/src/main/proto/photon_types.proto | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/photon-targeting/src/main/proto/photon_types.proto b/photon-targeting/src/main/proto/photon_types.proto index b328bed18e..42ef3429cf 100644 --- a/photon-targeting/src/main/proto/photon_types.proto +++ b/photon-targeting/src/main/proto/photon_types.proto @@ -31,8 +31,8 @@ message PhotonPipelineResult { message MultiTargetPNPResults { wpi.proto.ProtobufTransform3d best = 1; double bestReprojErr = 2; - optional wpi.proto.ProtobufTransform3d alt = 3; - optional double altReprojErr = 4; + wpi.proto.ProtobufTransform3d alt = 3; + double altReprojErr = 4; double ambiguity = 5; repeated int32 fiducial_ids_used = 6; From 20d7580b73a4d50935df56dbdf592d5ece62c9ca Mon Sep 17 00:00:00 2001 From: Matt Date: Wed, 25 Oct 2023 21:17:33 -0400 Subject: [PATCH 06/74] bump wpilib --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index f593638b90..a58191cec8 100644 --- a/build.gradle +++ b/build.gradle @@ -26,7 +26,7 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2024.1.1-beta-2" + wpilibVersion = "2024.1.1-beta-2-19-gad80eb3" openCVversion = "4.8.0-1" joglVersion = "2.4.0-rc-20200307" javalinVersion = "5.6.2" From aebcfa08d2a72372b4ca124ed2b501fe3535fa88 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 5 Nov 2023 19:15:14 -0500 Subject: [PATCH 07/74] Change targeting classes --- .../targeting/MultiTargetPNPResults.java | 93 ++- .../photonvision/targeting/PNPResults.java | 83 +-- .../targeting/PhotonPipelineResult.java | 451 +++++++------- .../targeting/PhotonPipelineResultProto.java | 36 -- .../targeting/PhotonTrackedTarget.java | 585 +++++++++--------- .../photonvision/targeting/TargetCorner.java | 118 +++- .../src/main/proto/photon_types.proto | 70 ++- 7 files changed, 708 insertions(+), 728 deletions(-) delete mode 100644 photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResultProto.java diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java index 0d2fa0b214..71c47c2765 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java @@ -17,49 +17,22 @@ package org.photonvision.targeting; -import java.util.ArrayList; +import edu.wpi.first.util.protobuf.Protobuf; +import org.photonvision.proto.PhotonTypes.ProtobufMultiTargetPNPResults; +import us.hebi.quickbuf.Descriptors.Descriptor; +import java.util.Arrays; import java.util.List; -import org.photonvision.common.dataflow.structures.Packet; - -import edu.wpi.first.math.geometry.Transform3d; +import java.util.stream.Collectors; public class MultiTargetPNPResults { - // Seeing 32 apriltags at once seems like a sane limit - private static final int MAX_IDS = 32; - // pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally) - public static final int PACK_SIZE_BYTES = PNPResults.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS); - - public PNPResults estimatedPose = new PNPResults(); - public List fiducialIDsUsed = List.of(); - - public MultiTargetPNPResults() {} + public final PNPResults estimatedPose; + public final List fiducialIDsUsed; public MultiTargetPNPResults(PNPResults results, List ids) { estimatedPose = results; fiducialIDsUsed = ids; } - public static MultiTargetPNPResults createFromPacket(Packet packet) { - var results = PNPResults.createFromPacket(packet); - var ids = new ArrayList(MAX_IDS); - for (int i = 0; i < MAX_IDS; i++) { - int targetId = (int) packet.decodeShort(); - if (targetId > -1) ids.add(targetId); - } - return new MultiTargetPNPResults(results, ids); - } - - public void populatePacket(Packet packet) { - estimatedPose.populatePacket(packet); - for (int i = 0; i < MAX_IDS; i++) { - if (i < fiducialIDsUsed.size()) { - packet.encode((short) fiducialIDsUsed.get(i).byteValue()); - } else { - packet.encode((short) -1); - } - } - } - @Override public int hashCode() { final int prime = 31; @@ -93,23 +66,45 @@ public String toString() { + "]"; } - public org.photonvision.proto.PhotonTypes.PhotonPipelineResult.MultiTargetPNPResults toProto() { - var ret = org.photonvision.proto.PhotonTypes.PhotonPipelineResult.MultiTargetPNPResults.newInstance(); + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return MultiTargetPNPResults.class; + } - var best = Transform3d.proto.createMessage(); - var alt = Transform3d.proto.createMessage(); - Transform3d.proto.pack(best, this.estimatedPose.best); - Transform3d.proto.pack(alt, this.estimatedPose.alt); - ret.setBest(best); - ret.setAlt(alt); - ret.setBestReprojErr(this.estimatedPose.bestReprojErr); - ret.setAltReprojErr(this.estimatedPose.altReprojErr); - ret.setAmbiguity(this.estimatedPose.ambiguity); - int[] ids = new int[this.fiducialIDsUsed.size()]; + @Override + public Descriptor getDescriptor() { + return ProtobufMultiTargetPNPResults.getDescriptor(); + } - for (int i = 0; i < ids.length; i++) ids[i] = this.fiducialIDsUsed.get(i); - ret.addAllFiducialIdsUsed(ids); + @Override + public ProtobufMultiTargetPNPResults createMessage() { + return ProtobufMultiTargetPNPResults.newInstance(); + } + + @Override + public MultiTargetPNPResults unpack(ProtobufMultiTargetPNPResults msg) { + return new MultiTargetPNPResults( + PNPResults.proto.unpack(msg.getEstimatedPose()), + // TODO better way of doing this + Arrays.stream(msg.getFiducialIdsUsed().array()) + .boxed() + .collect(Collectors.toList()) + ); + } - return ret; + @Override + public void pack(ProtobufMultiTargetPNPResults msg, MultiTargetPNPResults value) { + PNPResults.proto.pack(msg.getMutableEstimatedPose(), value.estimatedPose); + + // TODO better way of doing this + int[] ids = new int[value.fiducialIDsUsed.size()]; + for(int i = 0; i < ids.length; i++) { + ids[i] = value.fiducialIDsUsed.get(i); + } + msg.addAllFiducialIdsUsed(ids); + } } + + public static final AProto proto = new AProto(); } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java index 11a77547fc..a07e75a6e2 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java @@ -18,8 +18,9 @@ package org.photonvision.targeting; import edu.wpi.first.math.geometry.Transform3d; -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.utils.PacketUtils; +import edu.wpi.first.util.protobuf.Protobuf; +import org.photonvision.proto.PhotonTypes.ProtobufPNPResults; +import us.hebi.quickbuf.Descriptors.Descriptor; /** * The best estimated transformation from solvePnP, and possibly an alternate transformation @@ -58,20 +59,6 @@ public class PNPResults { /** If no alternate solution is found, this is 0 */ public final double ambiguity; - /** An empty (invalid) result. */ - public PNPResults() { - this.isPresent = false; - this.best = new Transform3d(); - this.alt = new Transform3d(); - this.ambiguity = 0; - this.bestReprojErr = 0; - this.altReprojErr = 0; - } - - public PNPResults(Transform3d best, double bestReprojErr) { - this(best, best, 0, bestReprojErr, bestReprojErr); - } - public PNPResults( Transform3d best, Transform3d alt, @@ -86,30 +73,8 @@ public PNPResults( this.altReprojErr = altReprojErr; } - public static final int PACK_SIZE_BYTES = 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3); - - public static PNPResults createFromPacket(Packet packet) { - var present = packet.decodeBoolean(); - var best = PacketUtils.decodeTransform(packet); - var alt = PacketUtils.decodeTransform(packet); - var bestEr = packet.decodeDouble(); - var altEr = packet.decodeDouble(); - var ambiguity = packet.decodeDouble(); - if (present) { - return new PNPResults(best, alt, ambiguity, bestEr, altEr); - } else { - return new PNPResults(); - } - } - - public Packet populatePacket(Packet packet) { - packet.encode(isPresent); - PacketUtils.encodeTransform(packet, best); - PacketUtils.encodeTransform(packet, alt); - packet.encode(bestReprojErr); - packet.encode(altReprojErr); - packet.encode(ambiguity); - return packet; + public PNPResults(Transform3d best, double bestReprojErr) { + this(best, best, 0, bestReprojErr, bestReprojErr); } @Override @@ -167,4 +132,42 @@ public String toString() { + ambiguity + "]"; } + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return PNPResults.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPNPResults.getDescriptor(); + } + + @Override + public ProtobufPNPResults createMessage() { + return ProtobufPNPResults.newInstance(); + } + + @Override + public PNPResults unpack(ProtobufPNPResults msg) { + return new PNPResults( + Transform3d.proto.unpack(msg.getBest()), + Transform3d.proto.unpack(msg.getAlt()), + msg.getAmbiguity(), + msg.getBestReprojErr(), + msg.getAltReprojErr() + ); + } + + @Override + public void pack(ProtobufPNPResults msg, PNPResults 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); + + } + } + + public static final AProto proto = new AProto(); } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index a6f51f6241..f789738195 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -1,243 +1,208 @@ -// /* -// * 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 . -// */ - -// package org.photonvision.targeting; - -// import java.util.ArrayList; -// import java.util.List; -// import org.photonvision.common.dataflow.structures.Packet; - -// import org.photonvision.proto.PhotonTypes; - -// /** Represents a pipeline result from a PhotonCamera. */ -// public class PhotonPipelineResult { -// private static boolean HAS_WARNED = false; - -// // Targets to store. -// public final List targets = new ArrayList<>(); - -// // Latency in milliseconds. -// private double latencyMillis; - -// // Timestamp in milliseconds. -// private double timestampSeconds = -1; - -// // Multi-tag result -// private MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults(); - -// /** Constructs an empty pipeline result. */ -// public PhotonPipelineResult() {} - -// /** -// * Constructs a pipeline result. -// * -// * @param latencyMillis The latency in the pipeline. -// * @param targets The list of targets identified by the pipeline. -// */ -// public PhotonPipelineResult(double latencyMillis, List targets) { -// this.latencyMillis = latencyMillis; -// this.targets.addAll(targets); -// } - -// /** -// * Constructs a pipeline result. -// * -// * @param latencyMillis The latency in the pipeline. -// * @param targets The list of targets identified by the pipeline. -// * @param result Result from multi-target PNP. -// */ -// public PhotonPipelineResult( -// double latencyMillis, List targets, MultiTargetPNPResults result) { -// this.latencyMillis = latencyMillis; -// this.targets.addAll(targets); -// this.multiTagResult = result; -// } - -// /** -// * Returns the size of the packet needed to store this pipeline result. -// * -// * @return The size of the packet needed to store this pipeline result. -// */ -// public int getPacketSize() { -// return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES -// + 8 // latency -// + MultiTargetPNPResults.PACK_SIZE_BYTES -// + 1; // target count -// } - -// /** -// * Returns the best target in this pipeline result. If there are no targets, this method will -// * return null. The best target is determined by the target sort mode in the PhotonVision UI. -// * -// * @return The best target of the pipeline result. -// */ -// public PhotonTrackedTarget getBestTarget() { -// if (!hasTargets() && !HAS_WARNED) { -// String errStr = -// "This PhotonPipelineResult object has no targets associated with it! Please check hasTargets() " -// + "before calling this method. For more information, please review the PhotonLib " -// + "documentation at http://docs.photonvision.org"; -// System.err.println(errStr); -// new Exception().printStackTrace(); -// HAS_WARNED = true; -// } -// return hasTargets() ? targets.get(0) : null; -// } - -// /** -// * Returns the latency in the pipeline. -// * -// * @return The latency in the pipeline. -// */ -// public double getLatencyMillis() { -// return latencyMillis; -// } - -// /** -// * Returns the estimated time the frame was taken, This is more accurate than using -// * getLatencyMillis() -// * -// * @return The timestamp in seconds, or -1 if this result has no timestamp set. -// */ -// public double getTimestampSeconds() { -// return timestampSeconds; -// } - -// /** -// * Sets the FPGA timestamp of this result in seconds. -// * -// * @param timestampSeconds The timestamp in seconds. -// */ -// public void setTimestampSeconds(double timestampSeconds) { -// this.timestampSeconds = timestampSeconds; -// } - -// /** -// * Returns whether the pipeline has targets. -// * -// * @return Whether the pipeline has targets. -// */ -// public boolean hasTargets() { -// return targets.size() > 0; -// } - -// /** -// * Returns a copy of the vector of targets. -// * -// * @return A copy of the vector of targets. -// */ -// public List getTargets() { -// return new ArrayList<>(targets); -// } - -// /** -// * Return the latest mulit-target result. Be sure to check -// * getMultiTagResult().estimatedPose.isPresent before using the pose estimate! -// */ -// public MultiTargetPNPResults getMultiTagResult() { -// return multiTagResult; -// } - -// /** -// * Populates the fields of the pipeline result from the packet. -// * -// * @param packet The incoming packet. -// * @return The incoming packet. -// */ -// public Packet createFromPacket(Packet packet) { -// // Decode latency, existence of targets, and number of targets. -// latencyMillis = packet.decodeDouble(); -// this.multiTagResult = MultiTargetPNPResults.createFromPacket(packet); -// byte targetCount = packet.decodeByte(); - -// targets.clear(); - -// // Decode the information of each target. -// for (int i = 0; i < (int) targetCount; ++i) { -// var target = new PhotonTrackedTarget(); -// target.createFromPacket(packet); -// targets.add(target); -// } - -// return packet; -// } - -// /** -// * Populates the outgoing packet with information from this pipeline result. -// * -// * @param packet The outgoing packet. -// * @return The outgoing packet. -// */ -// public Packet populatePacket(Packet packet) { -// // Encode latency, existence of targets, and number of targets. -// packet.encode(latencyMillis); -// multiTagResult.populatePacket(packet); -// packet.encode((byte) targets.size()); - -// // Encode the information of each target. -// for (var target : targets) target.populatePacket(packet); - -// // Return the packet. -// return packet; -// } - -// @Override -// public int hashCode() { -// final int prime = 31; -// int result = 1; -// result = prime * result + ((targets == null) ? 0 : targets.hashCode()); -// long temp; -// temp = Double.doubleToLongBits(latencyMillis); -// result = prime * result + (int) (temp ^ (temp >>> 32)); -// temp = Double.doubleToLongBits(timestampSeconds); -// result = prime * result + (int) (temp ^ (temp >>> 32)); -// result = prime * result + ((multiTagResult == null) ? 0 : multiTagResult.hashCode()); -// return result; -// } - -// @Override -// public boolean equals(Object obj) { -// if (this == obj) return true; -// if (obj == null) return false; -// if (getClass() != obj.getClass()) return false; -// PhotonPipelineResult other = (PhotonPipelineResult) obj; -// if (targets == null) { -// if (other.targets != null) return false; -// } else if (!targets.equals(other.targets)) return false; -// if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis)) -// return false; -// if (Double.doubleToLongBits(timestampSeconds) -// != Double.doubleToLongBits(other.timestampSeconds)) return false; -// if (multiTagResult == null) { -// if (other.multiTagResult != null) return false; -// } else if (!multiTagResult.equals(other.multiTagResult)) return false; -// return true; -// } - -// @Override -// public String toString() { -// return "PhotonPipelineResult [targets=" -// + targets -// + ", latencyMillis=" -// + latencyMillis -// + ", timestampSeconds=" -// + timestampSeconds -// + ", multiTagResult=" -// + multiTagResult -// + "]"; -// } -// } +/* + * 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 . + */ + +package org.photonvision.targeting; + +import edu.wpi.first.util.protobuf.Protobuf; +import org.photonvision.proto.PhotonTypes.ProtobufPhotonTrackedTarget; +import org.photonvision.proto.PhotonTypes.ProtobufPhotonPipelineResult; +import us.hebi.quickbuf.Descriptors.Descriptor; +import java.util.ArrayList; +import java.util.List; + +/** Represents a pipeline result from a PhotonCamera. */ +public class PhotonPipelineResult { + private static boolean HAS_WARNED = false; + + public final List targets = new ArrayList<>(); + private final double latencyMillis; + private final MultiTargetPNPResults multiTagResult; + + private double timestampSeconds = -1; + + /** + * Constructs a pipeline result. + * + * @param latencyMillis The latency in the pipeline. + * @param targets The list of targets identified by the pipeline. + * @param result Result from multi-target PNP. + */ + public PhotonPipelineResult( + double latencyMillis, List targets, MultiTargetPNPResults result) { + this.latencyMillis = latencyMillis; + this.targets.addAll(targets); + this.multiTagResult = result; + } + + /** + * Returns the best target in this pipeline result. If there are no targets, this method will + * return null. The best target is determined by the target sort mode in the PhotonVision UI. + * + * @return The best target of the pipeline result. + */ + public PhotonTrackedTarget getBestTarget() { + if (!hasTargets() && !HAS_WARNED) { + String errStr = + "This PhotonPipelineResult object has no targets associated with it! Please check hasTargets() " + + "before calling this method. For more information, please review the PhotonLib " + + "documentation at http://docs.photonvision.org"; + System.err.println(errStr); + new Exception().printStackTrace(); + HAS_WARNED = true; + } + return hasTargets() ? targets.get(0) : null; + } + + /** + * Returns the latency in the pipeline. + * + * @return The latency in the pipeline. + */ + public double getLatencyMillis() { + return latencyMillis; + } + + /** + * Sets the FPGA timestamp of this result in seconds. + * + * @param timestampSeconds The timestamp in seconds. + */ + public void setTimestampSeconds(double timestampSeconds) { + this.timestampSeconds = timestampSeconds; + } + + /** + * Returns the estimated time the frame was taken, This is more accurate than using + * getLatencyMillis() + * + * @return The timestamp in seconds, or -1 if this result has no timestamp set. + */ + public double getTimestampSeconds() { + return timestampSeconds; + } + + /** + * Returns whether the pipeline has targets. + * + * @return Whether the pipeline has targets. + */ + public boolean hasTargets() { + return !targets.isEmpty(); + } + + /** + * Returns a copy of the vector of targets. + * + * @return A copy of the vector of targets. + */ + public List getTargets() { + return new ArrayList<>(targets); + } + + /** + * Return the latest mulit-target result. Be sure to check + * getMultiTagResult().estimatedPose.isPresent before using the pose estimate! + */ + public MultiTargetPNPResults getMultiTagResult() { + return multiTagResult; + } + + @Override + public int hashCode() { + final int prime = 31; + int result = 1; + result = prime * result + ((targets == null) ? 0 : targets.hashCode()); + long temp; + temp = Double.doubleToLongBits(latencyMillis); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(timestampSeconds); + result = prime * result + (int) (temp ^ (temp >>> 32)); + result = prime * result + ((multiTagResult == null) ? 0 : multiTagResult.hashCode()); + return result; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null) return false; + if (getClass() != obj.getClass()) return false; + PhotonPipelineResult other = (PhotonPipelineResult) obj; + if (targets == null) { + if (other.targets != null) return false; + } else if (!targets.equals(other.targets)) return false; + if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis)) + return false; + if (Double.doubleToLongBits(timestampSeconds) + != Double.doubleToLongBits(other.timestampSeconds)) return false; + if (multiTagResult == null) { + if (other.multiTagResult != null) return false; + } else if (!multiTagResult.equals(other.multiTagResult)) return false; + return true; + } + + @Override + public String toString() { + return "PhotonPipelineResult [targets=" + + targets + + ", latencyMillis=" + + latencyMillis + + ", timestampSeconds=" + + timestampSeconds + + ", multiTagResult=" + + multiTagResult + + "]"; + } + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return PhotonPipelineResult.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPhotonPipelineResult.getDescriptor(); + } + + @Override + public ProtobufPhotonPipelineResult createMessage() { + return ProtobufPhotonPipelineResult.newInstance(); + } + + @Override + public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) { + return new PhotonPipelineResult( + msg.getLatencyMs(), + PhotonTrackedTarget.proto.unpack(msg.getTargets()), + MultiTargetPNPResults.proto.unpack(msg.getMultiTargetResult()) + ); + } + + @Override + public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) { + ProtobufPhotonTrackedTarget[] targets = new ProtobufPhotonTrackedTarget[value.targets.size()]; + PhotonTrackedTarget.proto.pack(targets, value.targets); + msg.addAllTargets(targets); + + MultiTargetPNPResults.proto.pack(msg.getMutableMultiTargetResult(), value.multiTagResult); + + msg.setLatencyMs(value.getLatencyMillis()); + } + } + + public static final AProto proto = new AProto(); +} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResultProto.java deleted file mode 100644 index 9c13b951a9..0000000000 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResultProto.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.photonvision.targeting; - -import org.photonvision.proto.PhotonTypes.PhotonPipelineResult; - -import edu.wpi.first.util.protobuf.Protobuf; -import us.hebi.quickbuf.Descriptors.Descriptor; - -public class PhotonPipelineResultProto implements Protobuf { - - @Override - public Class getTypeClass() { - return PhotonPipelineResult.class; - } - - @Override - public Descriptor getDescriptor() { - return PhotonPipelineResult.getDescriptor(); - } - - @Override - public PhotonPipelineResult createMessage() { - return PhotonPipelineResult.newInstance(); - } - - @Override - public PhotonPipelineResult unpack(PhotonPipelineResult msg) { - return msg; - } - - @Override - public void pack(PhotonPipelineResult msg, PhotonPipelineResult value) { - msg.copyFrom(value); - } - - public static final PhotonPipelineResultProto proto = new PhotonPipelineResultProto(); -} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 08a5571ef7..ed32cc1037 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -1,293 +1,292 @@ -// /* -// * 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 . -// */ - -// package org.photonvision.targeting; - -// import edu.wpi.first.math.geometry.Transform3d; -// import java.util.ArrayList; -// import java.util.List; -// import org.photonvision.common.dataflow.structures.Packet; -// import org.photonvision.utils.PacketUtils; - -// public class PhotonTrackedTarget { -// private static final int MAX_CORNERS = 8; -// public static final int PACK_SIZE_BYTES = -// Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS); - -// private double yaw; -// private double pitch; -// private double area; -// private double skew; -// private int fiducialId; -// private Transform3d bestCameraToTarget = new Transform3d(); -// private Transform3d altCameraToTarget = new Transform3d(); -// private double poseAmbiguity; - -// // Corners from the min-area rectangle bounding the target -// private List minAreaRectCorners; - -// // Corners from whatever corner detection method was used -// private List detectedCorners; - -// public PhotonTrackedTarget() {} - -// /** Construct a tracked target, given exactly 4 corners */ -// public PhotonTrackedTarget( -// double yaw, -// double pitch, -// double area, -// double skew, -// int id, -// Transform3d pose, -// Transform3d altPose, -// double ambiguity, -// List minAreaRectCorners, -// List detectedCorners) { -// assert minAreaRectCorners.size() == 4; - -// if (detectedCorners.size() > MAX_CORNERS) { -// detectedCorners = detectedCorners.subList(0, MAX_CORNERS); -// } - -// this.yaw = yaw; -// this.pitch = pitch; -// this.area = area; -// this.skew = skew; -// this.fiducialId = id; -// this.bestCameraToTarget = pose; -// this.altCameraToTarget = altPose; -// this.minAreaRectCorners = minAreaRectCorners; -// this.detectedCorners = detectedCorners; -// this.poseAmbiguity = ambiguity; -// } - -// public double getYaw() { -// return yaw; -// } - -// public double getPitch() { -// return pitch; -// } - -// public double getArea() { -// return area; -// } - -// public double getSkew() { -// return skew; -// } - -// /** Get the Fiducial ID, or -1 if not set. */ -// public int getFiducialId() { -// return fiducialId; -// } - -// /** -// * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be -// * ambiguous. -1 if invalid. -// */ -// public double getPoseAmbiguity() { -// return poseAmbiguity; -// } - -// /** -// * Return a list of the 4 corners in image space (origin top left, x right, y down), in no -// * particular order, of the minimum area bounding rectangle of this target -// */ -// public List getMinAreaRectCorners() { -// return minAreaRectCorners; -// } - -// /** -// * Return a list of the n corners in image space (origin top left, x right, y down), in no -// * particular order, detected for this target. -// * -// *

For fiducials, the order is known and is always counter-clock wise around the tag, like so: -// * -// * 

​
-//      * ⟶ +X  3 ----- 2
-//      * |      |       |
-//      * V      |       |
-//      * +Y     0 ----- 1
-//      *
-// */ -// public List getDetectedCorners() { -// return detectedCorners; -// } - -// /** -// * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag -// * space (X forward, Y left, Z up) with the lowest reprojection error -// */ -// public Transform3d getBestCameraToTarget() { -// return bestCameraToTarget; -// } - -// /** -// * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag -// * space (X forward, Y left, Z up) with the highest reprojection error -// */ -// public Transform3d getAlternateCameraToTarget() { -// return altCameraToTarget; -// } - -// @Override -// public int hashCode() { -// final int prime = 31; -// int result = 1; -// long temp; -// temp = Double.doubleToLongBits(yaw); -// result = prime * result + (int) (temp ^ (temp >>> 32)); -// temp = Double.doubleToLongBits(pitch); -// result = prime * result + (int) (temp ^ (temp >>> 32)); -// temp = Double.doubleToLongBits(area); -// result = prime * result + (int) (temp ^ (temp >>> 32)); -// temp = Double.doubleToLongBits(skew); -// result = prime * result + (int) (temp ^ (temp >>> 32)); -// result = prime * result + fiducialId; -// result = prime * result + ((bestCameraToTarget == null) ? 0 : bestCameraToTarget.hashCode()); -// result = prime * result + ((altCameraToTarget == null) ? 0 : altCameraToTarget.hashCode()); -// temp = Double.doubleToLongBits(poseAmbiguity); -// result = prime * result + (int) (temp ^ (temp >>> 32)); -// result = prime * result + ((minAreaRectCorners == null) ? 0 : minAreaRectCorners.hashCode()); -// result = prime * result + ((detectedCorners == null) ? 0 : detectedCorners.hashCode()); -// return result; -// } - -// @Override -// public boolean equals(Object obj) { -// if (this == obj) return true; -// if (obj == null) return false; -// if (getClass() != obj.getClass()) return false; -// PhotonTrackedTarget other = (PhotonTrackedTarget) obj; -// if (Double.doubleToLongBits(yaw) != Double.doubleToLongBits(other.yaw)) return false; -// if (Double.doubleToLongBits(pitch) != Double.doubleToLongBits(other.pitch)) return false; -// if (Double.doubleToLongBits(area) != Double.doubleToLongBits(other.area)) return false; -// if (Double.doubleToLongBits(skew) != Double.doubleToLongBits(other.skew)) return false; -// if (fiducialId != other.fiducialId) return false; -// if (bestCameraToTarget == null) { -// if (other.bestCameraToTarget != null) return false; -// } else if (!bestCameraToTarget.equals(other.bestCameraToTarget)) return false; -// if (altCameraToTarget == null) { -// if (other.altCameraToTarget != null) return false; -// } else if (!altCameraToTarget.equals(other.altCameraToTarget)) return false; -// if (Double.doubleToLongBits(poseAmbiguity) != Double.doubleToLongBits(other.poseAmbiguity)) -// return false; -// if (minAreaRectCorners == null) { -// if (other.minAreaRectCorners != null) return false; -// } else if (!minAreaRectCorners.equals(other.minAreaRectCorners)) return false; -// if (detectedCorners == null) { -// if (other.detectedCorners != null) return false; -// } else if (!detectedCorners.equals(other.detectedCorners)) return false; -// return true; -// } - -// private static void encodeList(Packet packet, List list) { -// packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE)); -// for (int i = 0; i < list.size(); i++) { -// packet.encode(list.get(i).x); -// packet.encode(list.get(i).y); -// } -// } - -// private static List decodeList(Packet p) { -// byte len = p.decodeByte(); -// var ret = new ArrayList(); -// for (int i = 0; i < len; i++) { -// double cx = p.decodeDouble(); -// double cy = p.decodeDouble(); -// ret.add(new TargetCorner(cx, cy)); -// } -// return ret; -// } - -// /** -// * Populates the fields of this class with information from the incoming packet. -// * -// * @param packet The incoming packet. -// * @return The incoming packet. -// */ -// public Packet createFromPacket(Packet packet) { -// this.yaw = packet.decodeDouble(); -// this.pitch = packet.decodeDouble(); -// this.area = packet.decodeDouble(); -// this.skew = packet.decodeDouble(); -// this.fiducialId = packet.decodeInt(); - -// this.bestCameraToTarget = PacketUtils.decodeTransform(packet); -// this.altCameraToTarget = PacketUtils.decodeTransform(packet); - -// this.poseAmbiguity = packet.decodeDouble(); - -// this.minAreaRectCorners = new ArrayList<>(4); -// for (int i = 0; i < 4; i++) { -// double cx = packet.decodeDouble(); -// double cy = packet.decodeDouble(); -// minAreaRectCorners.add(new TargetCorner(cx, cy)); -// } - -// detectedCorners = decodeList(packet); - -// return packet; -// } - -// /** -// * Populates the outgoing packet with information from the current target. -// * -// * @param packet The outgoing packet. -// * @return The outgoing packet. -// */ -// public Packet populatePacket(Packet packet) { -// packet.encode(yaw); -// packet.encode(pitch); -// packet.encode(area); -// packet.encode(skew); -// packet.encode(fiducialId); -// PacketUtils.encodeTransform(packet, bestCameraToTarget); -// PacketUtils.encodeTransform(packet, altCameraToTarget); -// packet.encode(poseAmbiguity); - -// for (int i = 0; i < 4; i++) { -// packet.encode(minAreaRectCorners.get(i).x); -// packet.encode(minAreaRectCorners.get(i).y); -// } - -// encodeList(packet, detectedCorners); - -// return packet; -// } - -// @Override -// public String toString() { -// return "PhotonTrackedTarget{" -// + "yaw=" -// + yaw -// + ", pitch=" -// + pitch -// + ", area=" -// + area -// + ", skew=" -// + skew -// + ", fiducialId=" -// + fiducialId -// + ", cameraToTarget=" -// + bestCameraToTarget -// + ", targetCorners=" -// + minAreaRectCorners -// + '}'; -// } -// } +/* + * 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 . + */ + +package org.photonvision.targeting; + +import edu.wpi.first.math.geometry.Transform3d; + +import java.util.ArrayList; +import java.util.List; +import edu.wpi.first.util.protobuf.Protobuf; +import org.photonvision.proto.PhotonTypes.ProtobufPhotonTrackedTarget; +import org.photonvision.proto.PhotonTypes.ProtobufTargetCorner; +import us.hebi.quickbuf.Descriptors.Descriptor; +import us.hebi.quickbuf.RepeatedMessage; + +public class 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 Transform3d bestCameraToTarget; + private final Transform3d altCameraToTarget; + private final double poseAmbiguity; + + // Corners from the min-area rectangle bounding the target + private final List minAreaRectCorners; + // Corners from whatever corner detection method was used + private final List detectedCorners; + + /** Construct a tracked target, given exactly 4 corners */ + public PhotonTrackedTarget( + double yaw, + double pitch, + double area, + double skew, + int id, + Transform3d pose, + Transform3d altPose, + double ambiguity, + List minAreaRectCorners, + List detectedCorners) { + assert minAreaRectCorners.size() == 4; + + if (detectedCorners.size() > MAX_CORNERS) { + detectedCorners = detectedCorners.subList(0, MAX_CORNERS); + } + + this.yaw = yaw; + this.pitch = pitch; + this.area = area; + this.skew = skew; + this.fiducialId = id; + this.bestCameraToTarget = pose; + this.altCameraToTarget = altPose; + this.minAreaRectCorners = minAreaRectCorners; + this.detectedCorners = detectedCorners; + this.poseAmbiguity = ambiguity; + } + + public double getYaw() { + return yaw; + } + + public double getPitch() { + return pitch; + } + + public double getArea() { + return area; + } + + public double getSkew() { + return skew; + } + + /** Get the Fiducial ID, or -1 if not set. */ + public int getFiducialId() { + return fiducialId; + } + + /** + * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be + * ambiguous. -1 if invalid. + */ + public double getPoseAmbiguity() { + return poseAmbiguity; + } + + /** + * Return a list of the 4 corners in image space (origin top left, x right, y down), in no + * particular order, of the minimum area bounding rectangle of this target + */ + public List getMinAreaRectCorners() { + return minAreaRectCorners; + } + + /** + * Return a list of the n corners in image space (origin top left, x right, y down), in no + * particular order, detected for this target. + * + *

For fiducials, the order is known and is always counter-clock wise around the tag, like so: + * + * 

​
+     * ⟶ +X  3 ----- 2
+     * |      |       |
+     * V      |       |
+     * +Y     0 ----- 1
+     *
+ */ + public List getDetectedCorners() { + return detectedCorners; + } + + /** + * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag + * space (X forward, Y left, Z up) with the lowest reprojection error + */ + public Transform3d getBestCameraToTarget() { + return bestCameraToTarget; + } + + /** + * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag + * space (X forward, Y left, Z up) with the highest reprojection error + */ + public Transform3d getAlternateCameraToTarget() { + return altCameraToTarget; + } + + @Override + public int hashCode() { + final int prime = 31; + int result = 1; + long temp; + temp = Double.doubleToLongBits(yaw); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(pitch); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(area); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(skew); + result = prime * result + (int) (temp ^ (temp >>> 32)); + result = prime * result + fiducialId; + result = prime * result + ((bestCameraToTarget == null) ? 0 : bestCameraToTarget.hashCode()); + result = prime * result + ((altCameraToTarget == null) ? 0 : altCameraToTarget.hashCode()); + temp = Double.doubleToLongBits(poseAmbiguity); + result = prime * result + (int) (temp ^ (temp >>> 32)); + result = prime * result + ((minAreaRectCorners == null) ? 0 : minAreaRectCorners.hashCode()); + result = prime * result + ((detectedCorners == null) ? 0 : detectedCorners.hashCode()); + return result; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null) return false; + if (getClass() != obj.getClass()) return false; + PhotonTrackedTarget other = (PhotonTrackedTarget) obj; + if (Double.doubleToLongBits(yaw) != Double.doubleToLongBits(other.yaw)) return false; + if (Double.doubleToLongBits(pitch) != Double.doubleToLongBits(other.pitch)) return false; + if (Double.doubleToLongBits(area) != Double.doubleToLongBits(other.area)) return false; + if (Double.doubleToLongBits(skew) != Double.doubleToLongBits(other.skew)) return false; + if (fiducialId != other.fiducialId) return false; + if (bestCameraToTarget == null) { + if (other.bestCameraToTarget != null) return false; + } else if (!bestCameraToTarget.equals(other.bestCameraToTarget)) return false; + if (altCameraToTarget == null) { + if (other.altCameraToTarget != null) return false; + } else if (!altCameraToTarget.equals(other.altCameraToTarget)) return false; + if (Double.doubleToLongBits(poseAmbiguity) != Double.doubleToLongBits(other.poseAmbiguity)) + return false; + if (minAreaRectCorners == null) { + if (other.minAreaRectCorners != null) return false; + } else if (!minAreaRectCorners.equals(other.minAreaRectCorners)) return false; + if (detectedCorners == null) { + if (other.detectedCorners != null) return false; + } else if (!detectedCorners.equals(other.detectedCorners)) return false; + return true; + } + + @Override + public String toString() { + return "PhotonTrackedTarget{" + + "yaw=" + + yaw + + ", pitch=" + + pitch + + ", area=" + + area + + ", skew=" + + skew + + ", fiducialId=" + + fiducialId + + ", cameraToTarget=" + + bestCameraToTarget + + ", targetCorners=" + + minAreaRectCorners + + '}'; + } + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return PhotonTrackedTarget.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPhotonTrackedTarget.getDescriptor(); + } + + + @Override + public ProtobufPhotonTrackedTarget createMessage() { + return ProtobufPhotonTrackedTarget.newInstance(); + } + + @Override + public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) { + return new PhotonTrackedTarget( + msg.getYaw(), + msg.getPitch(), + msg.getArea(), + msg.getSkew(), + msg.getFiducialId(), + Transform3d.proto.unpack(msg.getBestCameraToTarget()), + Transform3d.proto.unpack(msg.getAltCameraToTarget()), + msg.getPoseAmbiguity(), + TargetCorner.proto.unpack(msg.getMinAreaRectCorners()), + TargetCorner.proto.unpack(msg.getDetectedCorners()) + ); + } + + public List unpack(RepeatedMessage msg) { + ArrayList targets = new ArrayList<>(msg.length()); + for(ProtobufPhotonTrackedTarget target : msg) { + targets.add(unpack(target)); + } + return targets; + } + + @Override + public void pack(ProtobufPhotonTrackedTarget msg, PhotonTrackedTarget value) { + msg + .setYaw(value.getYaw()) + .setPitch(value.getPitch()) + .setSkew(value.getSkew()) + .setArea(value.getArea()) + .setFiducialId(value.getFiducialId()) + .setPoseAmbiguity(value.getPoseAmbiguity()); + + Transform3d.proto.pack(msg.getMutableBestCameraToTarget(), value.bestCameraToTarget); + Transform3d.proto.pack(msg.getMutableAltCameraToTarget(), value.altCameraToTarget); + + ProtobufTargetCorner[] minAreaRectCorners = new ProtobufTargetCorner[value.minAreaRectCorners.size()]; + ProtobufTargetCorner[] detectedCorners = new ProtobufTargetCorner[value.minAreaRectCorners.size()]; + + TargetCorner.proto.pack(minAreaRectCorners, value.minAreaRectCorners); + TargetCorner.proto.pack(detectedCorners, value.detectedCorners); + + msg.addAllMinAreaRectCorners(minAreaRectCorners); + msg.addAllDetectedCorners(detectedCorners); + } + + public void pack(ProtobufPhotonTrackedTarget[] buffer, List targets) { + for(int i = 0; i < targets.size(); i++) { + var protoTarget = createMessage(); + pack(protoTarget, targets.get(i)); + buffer[i] = protoTarget; + } + } + } + + public static final AProto proto = new AProto(); +} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index 36eb04b258..2759423213 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -15,38 +15,90 @@ // * along with this program. If not, see . // */ -// package org.photonvision.targeting; +package org.photonvision.targeting; -// import java.util.Objects; +import java.util.ArrayList; +import java.util.List; +import java.util.Objects; -// /** -// * 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 { -// public final double x; -// public final double y; - -// public TargetCorner(double cx, double cy) { -// this.x = cx; -// this.y = cy; -// } - -// @Override -// public boolean equals(Object o) { -// if (this == o) return true; -// if (o == null || getClass() != o.getClass()) return false; -// TargetCorner that = (TargetCorner) o; -// return Double.compare(that.x, x) == 0 && Double.compare(that.y, y) == 0; -// } - -// @Override -// public int hashCode() { -// return Objects.hash(x, y); -// } - -// @Override -// public String toString() { -// return "(" + x + "," + y + ')'; -// } -// } +import edu.wpi.first.util.protobuf.Protobuf; +import org.photonvision.proto.PhotonTypes.ProtobufTargetCorner; +import us.hebi.quickbuf.Descriptors.Descriptor; +import us.hebi.quickbuf.RepeatedMessage; + +/** + * 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 { + public final double x; + public final double y; + + public TargetCorner(double cx, double cy) { + this.x = cx; + this.y = cy; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (o == null || getClass() != o.getClass()) return false; + TargetCorner that = (TargetCorner) o; + return Double.compare(that.x, x) == 0 && Double.compare(that.y, y) == 0; + } + + @Override + public int hashCode() { + return Objects.hash(x, y); + } + + @Override + public String toString() { + return "(" + x + "," + y + ')'; + } + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return TargetCorner.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTargetCorner.getDescriptor(); + } + + @Override + public ProtobufTargetCorner createMessage() { + return ProtobufTargetCorner.newInstance(); + } + + @Override + public TargetCorner unpack(ProtobufTargetCorner msg) { + return new TargetCorner(msg.getX(), msg.getY()); + } + + public List unpack(RepeatedMessage msg) { + ArrayList corners = new ArrayList<>(msg.length()); + for(ProtobufTargetCorner corner : msg) { + corners.add(unpack(corner)); + } + return corners; + } + + @Override + public void pack(ProtobufTargetCorner msg, TargetCorner value) { + msg.setX(value.x).setY(value.y); + } + + public void pack(ProtobufTargetCorner[] buffer, List corners) { + for(int i = 0; i < corners.size(); i++) { + var protoCorner = createMessage(); + pack(protoCorner, corners.get(i)); + buffer[i] = protoCorner; + } + } + } + + public static final AProto proto = new AProto(); +} diff --git a/photon-targeting/src/main/proto/photon_types.proto b/photon-targeting/src/main/proto/photon_types.proto index b328bed18e..4eb8eb95e1 100644 --- a/photon-targeting/src/main/proto/photon_types.proto +++ b/photon-targeting/src/main/proto/photon_types.proto @@ -6,39 +6,41 @@ option java_package = "org.photonvision.proto"; import "geometry3d.proto"; -message PhotonPipelineResult { - double latency_ms = 1; - double timestamp_sec = 2; - - message TargetCorner { - double x = 1; - double y = 2; - } - - message PhotonTrackedTarget { - double yaw = 1; - double pitch = 2; - double area = 3; - double skew = 4; - int32 fiducialId = 5; - wpi.proto.ProtobufTransform3d bestCameraToTarget = 6; - wpi.proto.ProtobufTransform3d altCameraToTarget = 7; - double poseAmbiguity = 8; - repeated TargetCorner minAreaRectCorners = 9; - repeated TargetCorner detectedCorners = 10; - } - - message MultiTargetPNPResults { - wpi.proto.ProtobufTransform3d best = 1; - double bestReprojErr = 2; - optional wpi.proto.ProtobufTransform3d alt = 3; - optional double altReprojErr = 4; - double ambiguity = 5; - - repeated int32 fiducial_ids_used = 6; - } - - repeated PhotonTrackedTarget targets = 3; - MultiTargetPNPResults multi_target_result = 4; +message ProtobufTargetCorner { + double x = 1; + double y = 2; +} + +message ProtobufPNPResults { + bool isPresent = 1; + wpi.proto.ProtobufTransform3d best = 2; + double bestReprojErr = 3; + optional wpi.proto.ProtobufTransform3d alt = 4; + optional double altReprojErr = 5; + double ambiguity = 6; +} + +message ProtobufMultiTargetPNPResults { + ProtobufPNPResults estimatedPose = 1; + repeated int32 fiducial_ids_used = 2; } +message ProtobufPhotonTrackedTarget { + double yaw = 1; + double pitch = 2; + double area = 3; + double skew = 4; + int32 fiducialId = 5; + wpi.proto.ProtobufTransform3d bestCameraToTarget = 6; + wpi.proto.ProtobufTransform3d altCameraToTarget = 7; + double poseAmbiguity = 8; + repeated ProtobufTargetCorner minAreaRectCorners = 9; + repeated ProtobufTargetCorner detectedCorners = 10; +} + +message ProtobufPhotonPipelineResult { + double latency_ms = 1; + + repeated ProtobufPhotonTrackedTarget targets = 3; + ProtobufMultiTargetPNPResults multi_target_result = 4; +} From 5345b1ba3ea8910af04dde932718866594c245c3 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 5 Nov 2023 19:20:46 -0500 Subject: [PATCH 08/74] Update OpenCVHelp.java --- .../photonvision/estimation/OpenCVHelp.java | 23 ++++++++----------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 80f18943fc..45b46d85e1 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -45,8 +45,8 @@ import org.opencv.core.Rect; import org.opencv.core.RotatedRect; import org.opencv.imgproc.Imgproc; -import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.TargetCorner; import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.TargetCorner; public final class OpenCVHelp { private static RotTrlTransform3d NWU_TO_EDN; @@ -73,7 +73,7 @@ public final class OpenCVHelp { } public static Mat matrixToMat(SimpleMatrix matrix) { - var mat = new Mat(matrix.numRows(), matrix.numCols(), CvType.CV_64F); + var mat = new Mat(matrix.getNumRows(), matrix.getNumCols(), CvType.CV_64F); mat.put(0, 0, matrix.getDDRM().getData()); return mat; } @@ -160,7 +160,7 @@ public static Point[] cornersToPoints(List corners) { var points = new Point[corners.size()]; for (int i = 0; i < corners.size(); i++) { var corn = corners.get(i); - points[i] = new Point(corn.getX(), corn.getY()); + points[i] = new Point(corn.x, corn.y); } return points; } @@ -168,18 +168,18 @@ public static Point[] cornersToPoints(List corners) { public static Point[] cornersToPoints(TargetCorner... corners) { var points = new Point[corners.length]; for (int i = 0; i < corners.length; i++) { - points[i] = new Point(corners[i].getX(), corners[i].getY()); + points[i] = new Point(corners[i].x, corners[i].y); } return points; } public static List pointsToCorners(Point... points) { var corners = new ArrayList(points.length); - for (int i = 0; i < points.length; i++) { - var c = TargetCorner.newInstance(); - c.setX(points[i].x); - c.setY(points[i].y); - corners.add(c); + for (Point point : points) { + corners.add(new TargetCorner( + point.x, + point.y + )); } return corners; } @@ -189,10 +189,7 @@ public static List pointsToCorners(MatOfPoint2f matInput) { float[] data = new float[(int) matInput.total() * matInput.channels()]; matInput.get(0, 0, data); for (int i = 0; i < (int) matInput.total(); i++) { - var c = TargetCorner.newInstance(); - c.setX(data[0 + 2 * i]); - c.setY(data[1 + 2 * i]); - corners.add(c); + corners.add(new TargetCorner(data[0 + 2 * i], data[1 + 2 * i])); } return corners; } From 42f1c4b1fefd7020dd66b3fe39c6d67bb99f5ebf Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 5 Nov 2023 19:23:09 -0500 Subject: [PATCH 09/74] Update build.gradle --- photon-core/build.gradle | 1 - 1 file changed, 1 deletion(-) diff --git a/photon-core/build.gradle b/photon-core/build.gradle index 2529c0cd33..6fbf407797 100644 --- a/photon-core/build.gradle +++ b/photon-core/build.gradle @@ -29,7 +29,6 @@ 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"), From fd920fd2e968cf7db8daa59245db5c3022330221 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 5 Nov 2023 19:24:41 -0500 Subject: [PATCH 10/74] fix licence --- .../photonvision/targeting/TargetCorner.java | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index 2759423213..f7f923d8ef 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -1,19 +1,19 @@ -// /* -// * 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 . -// */ +/* + * 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 . + */ package org.photonvision.targeting; From bfb84d44ea96c7d7b7c0b78d3b78f148c0ef3dcb Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 5 Nov 2023 20:00:41 -0500 Subject: [PATCH 11/74] keep cleaning up --- .../networktables/NTDataPublisher.java | 26 ++--- .../vision/target/TrackedTarget.java | 101 +++++++----------- .../photonvision/PhotonTargetSortMode.java | 1 - .../common/networktables/NTTopicSet.java | 7 +- .../photonvision/estimation/OpenCVHelp.java | 17 +-- .../estimation/VisionEstimation.java | 39 +++---- .../photonvision/targeting/PNPResults.java | 13 +-- .../photonvision/targeting/TargetCorner.java | 1 - 8 files changed, 78 insertions(+), 127 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 2951a96288..19d43ebde5 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -19,15 +19,16 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEvent; + +import java.util.List; import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.Supplier; import org.photonvision.common.dataflow.CVPipelineResultConsumer; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.networktables.NTTopicSet; -import org.photonvision.proto.PhotonTypes.PhotonPipelineResult; +import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.TrackedTarget; @@ -129,22 +130,13 @@ 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); - - PhotonPipelineResult resultProto = PhotonPipelineResult.newInstance(); - resultProto.setLatencyMs(result.getLatencyMillis()); - resultProto.addAllTargets(TrackedTarget.simpleFromTrackedTargets(result.targets)); - if (result.multiTagResult.estimatedPose.isPresent) { - resultProto.setMultiTargetResult(result.multiTagResult.toProto()); - } + var res = new PhotonPipelineResult( + result.getLatencyMillis(), + TrackedTarget.simpleFromTrackedTargets(result.targets), + result.multiTagResult + ); - ts.rawBytesEntry.set(resultProto); + ts.rawBytesEntry.set(res); ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get()); ts.driverModePublisher.set(driverModeSupplier.getAsBoolean()); 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 index 25676aebc2..bbe63d68e3 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java @@ -19,8 +19,6 @@ import edu.wpi.first.apriltag.AprilTagDetection; import edu.wpi.first.apriltag.AprilTagPoseEstimate; 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; @@ -32,8 +30,8 @@ import org.opencv.core.RotatedRect; import org.photonvision.common.util.SerializationUtils; import org.photonvision.common.util.math.MathUtils; -import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.PhotonTrackedTarget; -import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.TargetCorner; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; import org.photonvision.vision.aruco.ArucoDetectionResult; import org.photonvision.vision.frame.FrameStaticProperties; import org.photonvision.vision.opencv.CVShape; @@ -116,9 +114,9 @@ public TrackedTarget( 0, 0, new double[] { - bestPose.getTranslation().getX(), - bestPose.getTranslation().getY(), - bestPose.getTranslation().getZ() + bestPose.getTranslation().getX(), + bestPose.getTranslation().getY(), + bestPose.getTranslation().getZ() }); setCameraRelativeTvec(tvec); @@ -131,10 +129,10 @@ public TrackedTarget( double[] corners = tagDetection.getCorners(); Point[] cornerPoints = new Point[] { - new Point(corners[0], corners[1]), - new Point(corners[2], corners[3]), - new Point(corners[4], corners[5]), - new Point(corners[6], corners[7]) + new Point(corners[0], corners[1]), + new Point(corners[2], corners[3]), + new Point(corners[4], corners[5]), + new Point(corners[6], corners[7]) }; m_targetCorners = List.of(cornerPoints); MatOfPoint contourMat = new MatOfPoint(cornerPoints); @@ -170,10 +168,10 @@ public TrackedTarget( Point[] cornerPoints = new Point[] { - new Point(xCorners[0], yCorners[0]), - new Point(xCorners[1], yCorners[1]), - new Point(xCorners[2], yCorners[2]), - new Point(xCorners[3], yCorners[3]) + new Point(xCorners[0], yCorners[0]), + new Point(xCorners[1], yCorners[1]), + new Point(xCorners[2], yCorners[2]), + new Point(xCorners[3], yCorners[3]) }; m_targetCorners = List.of(cornerPoints); MatOfPoint contourMat = new MatOfPoint(cornerPoints); @@ -207,9 +205,9 @@ public TrackedTarget( 0, 0, new double[] { - bestPose.getTranslation().getX(), - bestPose.getTranslation().getY(), - bestPose.getTranslation().getZ() + bestPose.getTranslation().getX(), + bestPose.getTranslation().getY(), + bestPose.getTranslation().getZ() }); setCameraRelativeTvec(tvec); @@ -395,64 +393,37 @@ public boolean isFiducial() { return this.m_fiducialId >= 0; } - public static PhotonTrackedTarget[] simpleFromTrackedTargets(List 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()]; + public static List simpleFromTrackedTargets(List targets) { + var ret = new ArrayList(); + for (var t : targets) { + var minAreaRectCorners = new ArrayList(); + var detectedCorners = new ArrayList(); { var points = new Point[4]; t.getMinAreaRect().points(points); for (int i = 0; i < 4; i++) { - var c = TargetCorner.newInstance(); - c.setX(points[i].x); - c.setY(points[i].y); - minAreaRectCorners[i] = c; + minAreaRectCorners.add(new TargetCorner(points[i].x, points[i].y)); } } { var points = t.getTargetCorners(); - for (int i = 0; i < points.size(); i++) { - var c = TargetCorner.newInstance(); - c.setX(points.get(i).x); - c.setY(points.get(i).y); - detectedCorners[i] = c; + for (Point point : points) { + detectedCorners.add(new TargetCorner(point.x, point.y)); } } - 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)); + 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/PhotonTargetSortMode.java b/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java index 958e009abd..46c48ceb8a 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java @@ -25,7 +25,6 @@ package org.photonvision; import java.util.Comparator; - import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.PhotonTrackedTarget; public enum PhotonTargetSortMode { diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index c4e817c9c1..154d939121 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -17,9 +17,6 @@ package org.photonvision.common.networktables; -import org.photonvision.proto.PhotonTypes.PhotonPipelineResult; -import org.photonvision.targeting.PhotonPipelineResultProto; - import edu.wpi.first.networktables.BooleanPublisher; import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.networktables.BooleanTopic; @@ -31,7 +28,7 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.ProtobufPublisher; import edu.wpi.first.networktables.PubSubOption; -import edu.wpi.first.networktables.RawPublisher; +import org.photonvision.targeting.PhotonPipelineResult; /** * This class is a wrapper around all per-pipeline NT topics that PhotonVision should be publishing @@ -75,7 +72,7 @@ public class NTTopicSet { public void updateEntries() { rawBytesEntry = subTable - .getProtobufTopic("result_proto", PhotonPipelineResultProto.proto) + .getProtobufTopic("result_proto", PhotonPipelineResult.proto) .publish(PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish(); diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 38b05d3adb..c253a198c2 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -30,6 +30,8 @@ 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; @@ -404,7 +406,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 PNPResults solvePNP_SQUARE( + public static Optional solvePNP_SQUARE( Matrix cameraMatrix, Matrix distCoeffs, List modelTrls, @@ -469,15 +471,16 @@ public static 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 new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]); - else return new PNPResults(best, errors[0]); + return Optional.of(new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1])); + else return Optional.of(new PNPResults(best, errors[0])); } // solvePnP failed catch (Exception e) { System.err.println("SolvePNP_SQUARE failed!"); e.printStackTrace(); - return new PNPResults(); + return Optional.empty(); } finally { // release our Mats from native memory objectMat.release(); @@ -512,7 +515,7 @@ public static PNPResults solvePNP_SQUARE( * model points are supplied relative to the origin, this transformation brings the camera to * the origin. */ - public static PNPResults solvePNP_SQPNP( + public static Optional solvePNP_SQPNP( Matrix cameraMatrix, Matrix distCoeffs, List objectTrls, @@ -561,11 +564,11 @@ public static PNPResults solvePNP_SQPNP( // check if solvePnP failed with NaN results if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result"); - return new PNPResults(best, error[0]); + return Optional.of(new PNPResults(best, error[0])); } catch (Exception e) { System.err.println("SolvePNP_SQPNP failed!"); e.printStackTrace(); - return new PNPResults(); + return Optional.empty(); } } } diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java index c737f7f4dc..2b13537be6 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -27,11 +27,12 @@ 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.proto.PhotonTypes.PhotonPipelineResult.PhotonTrackedTarget; -import org.photonvision.proto.PhotonTypes.PhotonPipelineResult.TargetCorner; import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; public class VisionEstimation { /** Get the visible {@link AprilTag}s which are in the tag layout using the visible tag IDs. */ @@ -66,7 +67,7 @@ public static List getVisibleLayoutTags( * @return The transformation that maps the field origin to the camera pose. Ensure the {@link * PNPResults} are present before utilizing them. */ - public static PNPResults estimateCamPosePNP( + public static Optional estimateCamPosePNP( Matrix cameraMatrix, Matrix distCoeffs, List visTags, @@ -74,9 +75,9 @@ public static PNPResults estimateCamPosePNP( TargetModel tagModel) { if (tagLayout == null || visTags == null - || tagLayout.getTags().size() == 0 - || visTags.size() == 0) { - return new PNPResults(); + || tagLayout.getTags().isEmpty() + || visTags.isEmpty()) { + return Optional.empty(); } var corners = new ArrayList(); @@ -89,46 +90,46 @@ public static PNPResults estimateCamPosePNP( .ifPresent( pose -> { knownTags.add(new AprilTag(id, pose)); - for (var c : tgt.getDetectedCorners()) { - corners.add(c); - } + corners.addAll(tgt.getDetectedCorners()); }); } - if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { - return new PNPResults(); + if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) { + return Optional.empty(); } Point[] points = OpenCVHelp.cornersToPoints(corners); // single-tag pnp if (knownTags.size() == 1) { - var camToTag = + var camToTagOpt = OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, tagModel.vertices, points); - if (!camToTag.isPresent) return new PNPResults(); + if (camToTagOpt.isEmpty()) return Optional.empty(); + var camToTag = camToTagOpt.get(); 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 new PNPResults( + return Optional.of(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(); for (var tag : knownTags) objectTrls.addAll(tagModel.getFieldVertices(tag.pose)); - var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points); - if (!camToOrigin.isPresent) return new PNPResults(); - return new PNPResults( + var camToOriginOpt = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points); + if (camToOriginOpt.isEmpty()) return Optional.empty(); + var camToOrigin = camToOriginOpt.get(); + return Optional.of(new PNPResults( camToOrigin.best.inverse(), camToOrigin.alt.inverse(), camToOrigin.ambiguity, camToOrigin.bestReprojErr, - camToOrigin.altReprojErr); + camToOrigin.altReprojErr)); } } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java index a07e75a6e2..f2f4f853d2 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java @@ -32,12 +32,6 @@ * method. */ public class PNPResults { - /** - * 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; - /** * The best-fit transform. The coordinate frame of this transform depends on the method which gave * this result. @@ -65,7 +59,6 @@ public PNPResults( double ambiguity, double bestReprojErr, double altReprojErr) { - this.isPresent = true; this.best = best; this.alt = alt; this.ambiguity = ambiguity; @@ -81,7 +74,6 @@ public PNPResults(Transform3d best, double bestReprojErr) { 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); @@ -100,7 +92,6 @@ public boolean equals(Object obj) { if (obj == null) return false; if (getClass() != obj.getClass()) return false; PNPResults other = (PNPResults) obj; - if (isPresent != other.isPresent) return false; if (best == null) { if (other.best != null) return false; } else if (!best.equals(other.best)) return false; @@ -118,9 +109,7 @@ public boolean equals(Object obj) { @Override public String toString() { - return "PNPResults [isPresent=" - + isPresent - + ", best=" + return "PNPResults [best=" + best + ", bestReprojErr=" + bestReprojErr diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index f7f923d8ef..1a655215f1 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -20,7 +20,6 @@ import java.util.ArrayList; import java.util.List; import java.util.Objects; - import edu.wpi.first.util.protobuf.Protobuf; import org.photonvision.proto.PhotonTypes.ProtobufTargetCorner; import us.hebi.quickbuf.Descriptors.Descriptor; From 030f54324d3586f3c367d205df16f06722387666 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 5 Nov 2023 20:06:56 -0500 Subject: [PATCH 12/74] more formatting fixes --- .../vision/target/TrackedTarget.java | 34 +++++++++---------- .../org/photonvision/EstimatedRobotPose.java | 2 +- .../java/org/photonvision/PhotonCamera.java | 8 +++-- .../native/cpp/photonlib/PhotonCamera.cpp | 6 ++-- .../include/photonlib/SimPhotonCamera.h | 3 +- 5 files changed, 27 insertions(+), 26 deletions(-) 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 index bbe63d68e3..335e889464 100644 --- 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,11 +113,10 @@ public TrackedTarget( tvec.put( 0, 0, - new double[] { - bestPose.getTranslation().getX(), - bestPose.getTranslation().getY(), - bestPose.getTranslation().getZ() - }); + bestPose.getTranslation().getX(), + bestPose.getTranslation().getY(), + bestPose.getTranslation().getZ() + ); setCameraRelativeTvec(tvec); // Opencv expects a 3d vector with norm = angle and direction = axis @@ -129,10 +128,10 @@ public TrackedTarget( double[] corners = tagDetection.getCorners(); Point[] cornerPoints = new Point[] { - new Point(corners[0], corners[1]), - new Point(corners[2], corners[3]), - new Point(corners[4], corners[5]), - new Point(corners[6], corners[7]) + new Point(corners[0], corners[1]), + new Point(corners[2], corners[3]), + new Point(corners[4], corners[5]), + new Point(corners[6], corners[7]) }; m_targetCorners = List.of(cornerPoints); MatOfPoint contourMat = new MatOfPoint(cornerPoints); @@ -168,10 +167,10 @@ public TrackedTarget( Point[] cornerPoints = new Point[] { - new Point(xCorners[0], yCorners[0]), - new Point(xCorners[1], yCorners[1]), - new Point(xCorners[2], yCorners[2]), - new Point(xCorners[3], yCorners[3]) + new Point(xCorners[0], yCorners[0]), + new Point(xCorners[1], yCorners[1]), + new Point(xCorners[2], yCorners[2]), + new Point(xCorners[3], yCorners[3]) }; m_targetCorners = List.of(cornerPoints); MatOfPoint contourMat = new MatOfPoint(cornerPoints); @@ -204,11 +203,10 @@ public TrackedTarget( tvec.put( 0, 0, - new double[] { - bestPose.getTranslation().getX(), - bestPose.getTranslation().getY(), - bestPose.getTranslation().getZ() - }); + 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/EstimatedRobotPose.java b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java index 2b5d65253c..a355ab00f3 100644 --- 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.proto.PhotonTypes.PhotonPipelineResult.PhotonTrackedTarget; +import org.photonvision.targeting.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 index 19bc478140..e2265dcead 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -47,13 +47,12 @@ 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.proto.PhotonTypes.PhotonPipelineResult; +import us.hebi.quickbuf.InvalidProtocolBufferException; /** Represents a camera that is connected to PhotonVision. */ public class PhotonCamera implements AutoCloseable { @@ -144,7 +143,10 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { cameraTable .getRawTopic("result_proto") .subscribe( - "proto:" + PhotonPipelineResult.getDescriptor().getFullName(), 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); diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp index c4b55424ef..9c3fc733ce 100644 --- 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("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 index dac3ba8e04..9d491b1fc3 100644 --- a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h @@ -48,7 +48,8 @@ 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("result_proto").Publish("asdfasdfasdf"); versionEntry = instance.GetTable("photonvision")->GetEntry("version"); // versionEntry.SetString(PhotonVersion.versionString); } From 381e7566426c0e8c7585bf4edae700919f2a41f5 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 5 Nov 2023 20:07:37 -0500 Subject: [PATCH 13/74] Update NTDataPublisher.java --- .../common/dataflow/networktables/NTDataPublisher.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 19d43ebde5..814decb804 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -19,8 +19,6 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEvent; - -import java.util.List; import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.Supplier; From c362730cd09153149d05c6f0e23dc3405fb9fcc8 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 5 Nov 2023 23:39:16 -0500 Subject: [PATCH 14/74] Basic Protobuf Implementation --- .../networktables/NTDataPublisher.java | 10 +- .../vision/pipe/impl/MultiTargetPNPPipe.java | 4 +- .../vision/target/TrackedTarget.java | 18 ++- .../java/org/photonvision/PhotonCamera.java | 30 ++-- .../org/photonvision/PhotonPoseEstimator.java | 2 + .../photonvision/PhotonTargetSortMode.java | 2 +- .../simulation/PhotonCameraSim.java | 7 +- .../simulation/SimVisionSystem.java | 2 + .../native/cpp/photonlib/PhotonCamera.cpp | 6 +- .../include/photonlib/SimPhotonCamera.h | 3 +- .../java/org/photonvision/PacketTest.java | 3 + .../org/photonvision/PhotonCameraTest.java | 1 + .../photonvision/PhotonPoseEstimatorTest.java | 3 + photon-targeting/build.gradle | 1 - .../photonvision/estimation/OpenCVHelp.java | 24 ++- .../estimation/VisionEstimation.java | 31 ++-- .../targeting/MultiTargetPNPResults.java | 48 ++++-- .../photonvision/targeting/PNPResults.java | 63 +++++++- .../targeting/PhotonPipelineResult.java | 122 ++++++++++++--- .../targeting/PhotonTrackedTarget.java | 144 +++++++++++++----- .../photonvision/targeting/TargetCorner.java | 12 +- 21 files changed, 379 insertions(+), 157 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 814decb804..7baf0a2f27 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-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 index d243e37e6a..313d8a19e0 100644 --- 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 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 index 335e889464..2a28d05e56 100644 --- 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 index e2265dcead..b4a8f85a51 100644 --- 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 { @@ -143,7 +142,7 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { cameraTable .getRawTopic("result_proto") .subscribe( - "proto:" + PhotonPipelineResult.getDescriptor().getFullName(), + "proto:" + PhotonPipelineResult.proto.getDescriptor().getFullName(), new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); @@ -187,26 +186,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 index 67e2d11f2d..32b4c7ba1f 100644 --- 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 index 46c48ceb8a..bf22644645 100644 --- 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 index 9511300ab0..0e317afd47 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -47,7 +47,6 @@ import org.opencv.imgproc.Imgproc; import org.photonvision.PhotonCamera; import org.photonvision.PhotonTargetSortMode; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.networktables.NTTopicSet; import org.photonvision.estimation.CameraTargetRelation; import org.photonvision.estimation.OpenCVHelp; @@ -56,6 +55,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 @@ -561,9 +562,7 @@ public void submitProcessedFrame(PhotonPipelineResult result) { public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimestamp) { ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp); - var newPacket = new Packet(result.getPacketSize()); - result.populatePacket(newPacket); - ts.rawBytesEntry.set(newPacket.getData(), receiveTimestamp); + ts.rawBytesEntry.set(result, receiveTimestamp); boolean hasTargets = result.hasTargets(); ts.hasTargetEntry.set(hasTargets, receiveTimestamp); diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java index 3e6147625d..52756f4727 100644 --- 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 index 9c3fc733ce..d0901b10bd 100644 --- 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 index 9d491b1fc3..44ab0917a2 100644 --- 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 index 1224f0cff8..c29a629b5b 100644 --- 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 index 77e18a6d8a..8a3fb5fad8 100644 --- 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 index 6f1f523686..1b386cc747 100644 --- 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 index 05c368b2c6..5c85d4bcb5 100644 --- 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 index c253a198c2..84e5d8d7cc 100644 --- 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 pointsToCorners(Point... points) { var corners = new ArrayList(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 solvePNP_SQUARE( + public static PNPResults solvePNP_SQUARE( Matrix cameraMatrix, Matrix distCoeffs, List modelTrls, @@ -471,16 +466,15 @@ public static Optional 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 solvePNP_SQUARE( * model points are supplied relative to the origin, this transformation brings the camera to * the origin. */ - public static Optional solvePNP_SQPNP( + public static PNPResults solvePNP_SQPNP( Matrix cameraMatrix, Matrix distCoeffs, List objectTrls, @@ -564,11 +558,11 @@ public static Optional 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 index 2b13537be6..9ad6e0fc4e 100644 --- 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,7 +66,7 @@ public static List 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 estimateCamPosePNP( + public static PNPResults estimateCamPosePNP( Matrix cameraMatrix, Matrix distCoeffs, List visTags, @@ -75,9 +74,9 @@ public static Optional estimateCamPosePNP( 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(); @@ -93,43 +92,41 @@ public static Optional 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(); 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); } } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java index 71c47c2765..f53353e820 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java @@ -18,21 +18,51 @@ package org.photonvision.targeting; import edu.wpi.first.util.protobuf.Protobuf; -import org.photonvision.proto.PhotonTypes.ProtobufMultiTargetPNPResults; -import us.hebi.quickbuf.Descriptors.Descriptor; +import java.util.ArrayList; import java.util.Arrays; import java.util.List; import java.util.stream.Collectors; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.proto.PhotonTypes.ProtobufMultiTargetPNPResults; +import us.hebi.quickbuf.Descriptors.Descriptor; public class MultiTargetPNPResults { - public final PNPResults estimatedPose; - public final List fiducialIDsUsed; + // Seeing 32 apriltags at once seems like a sane limit + private static final int MAX_IDS = 32; + // pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally) + public static final int PACK_SIZE_BYTES = PNPResults.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS); + + public PNPResults estimatedPose = new PNPResults(); + public List fiducialIDsUsed = List.of(); + + public MultiTargetPNPResults() {} public MultiTargetPNPResults(PNPResults results, List ids) { estimatedPose = results; fiducialIDsUsed = ids; } + public static MultiTargetPNPResults createFromPacket(Packet packet) { + var results = PNPResults.createFromPacket(packet); + var ids = new ArrayList(MAX_IDS); + for (int i = 0; i < MAX_IDS; i++) { + int targetId = (int) packet.decodeShort(); + if (targetId > -1) ids.add(targetId); + } + return new MultiTargetPNPResults(results, ids); + } + + public void populatePacket(Packet packet) { + estimatedPose.populatePacket(packet); + for (int i = 0; i < MAX_IDS; i++) { + if (i < fiducialIDsUsed.size()) { + packet.encode((short) fiducialIDsUsed.get(i).byteValue()); + } else { + packet.encode((short) -1); + } + } + } + @Override public int hashCode() { final int prime = 31; @@ -66,7 +96,8 @@ public String toString() { + "]"; } - public static final class AProto implements Protobuf { + public static final class AProto + implements Protobuf { @Override public Class getTypeClass() { return MultiTargetPNPResults.class; @@ -87,10 +118,7 @@ public MultiTargetPNPResults unpack(ProtobufMultiTargetPNPResults msg) { return new MultiTargetPNPResults( PNPResults.proto.unpack(msg.getEstimatedPose()), // TODO better way of doing this - Arrays.stream(msg.getFiducialIdsUsed().array()) - .boxed() - .collect(Collectors.toList()) - ); + Arrays.stream(msg.getFiducialIdsUsed().array()).boxed().collect(Collectors.toList())); } @Override @@ -99,7 +127,7 @@ public void pack(ProtobufMultiTargetPNPResults msg, MultiTargetPNPResults value) // TODO better way of doing this int[] ids = new int[value.fiducialIDsUsed.size()]; - for(int i = 0; i < ids.length; i++) { + for (int i = 0; i < ids.length; i++) { ids[i] = value.fiducialIDsUsed.get(i); } msg.addAllFiducialIdsUsed(ids); diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java index f2f4f853d2..819d48c577 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java @@ -19,7 +19,9 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.util.protobuf.Protobuf; +import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.proto.PhotonTypes.ProtobufPNPResults; +import org.photonvision.utils.PacketUtils; import us.hebi.quickbuf.Descriptors.Descriptor; /** @@ -32,6 +34,12 @@ * method. */ public class PNPResults { + /** + * 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; + /** * The best-fit transform. The coordinate frame of this transform depends on the method which gave * this result. @@ -53,12 +61,27 @@ public class PNPResults { /** If no alternate solution is found, this is 0 */ public final double ambiguity; + /** An empty (invalid) result. */ + public PNPResults() { + this.isPresent = false; + this.best = new Transform3d(); + this.alt = new Transform3d(); + this.ambiguity = 0; + this.bestReprojErr = 0; + this.altReprojErr = 0; + } + + public PNPResults(Transform3d best, double bestReprojErr) { + this(best, best, 0, bestReprojErr, bestReprojErr); + } + public PNPResults( Transform3d best, Transform3d alt, double ambiguity, double bestReprojErr, double altReprojErr) { + this.isPresent = true; this.best = best; this.alt = alt; this.ambiguity = ambiguity; @@ -66,14 +89,37 @@ public PNPResults( this.altReprojErr = altReprojErr; } - public PNPResults(Transform3d best, double bestReprojErr) { - this(best, best, 0, bestReprojErr, bestReprojErr); + public static final int PACK_SIZE_BYTES = 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3); + + public static PNPResults createFromPacket(Packet packet) { + var present = packet.decodeBoolean(); + var best = PacketUtils.decodeTransform(packet); + var alt = PacketUtils.decodeTransform(packet); + var bestEr = packet.decodeDouble(); + var altEr = packet.decodeDouble(); + var ambiguity = packet.decodeDouble(); + if (present) { + return new PNPResults(best, alt, ambiguity, bestEr, altEr); + } else { + return new PNPResults(); + } + } + + public Packet populatePacket(Packet packet) { + packet.encode(isPresent); + PacketUtils.encodeTransform(packet, best); + PacketUtils.encodeTransform(packet, alt); + packet.encode(bestReprojErr); + packet.encode(altReprojErr); + packet.encode(ambiguity); + return packet; } @Override 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); @@ -92,6 +138,7 @@ public boolean equals(Object obj) { if (obj == null) return false; if (getClass() != obj.getClass()) return false; PNPResults other = (PNPResults) obj; + if (isPresent != other.isPresent) return false; if (best == null) { if (other.best != null) return false; } else if (!best.equals(other.best)) return false; @@ -109,7 +156,9 @@ public boolean equals(Object obj) { @Override public String toString() { - return "PNPResults [best=" + return "PNPResults [isPresent=" + + isPresent + + ", best=" + best + ", bestReprojErr=" + bestReprojErr @@ -145,16 +194,16 @@ public PNPResults unpack(ProtobufPNPResults msg) { Transform3d.proto.unpack(msg.getAlt()), msg.getAmbiguity(), msg.getBestReprojErr(), - msg.getAltReprojErr() - ); + msg.getAltReprojErr()); } @Override public void pack(ProtobufPNPResults msg, PNPResults 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); - + msg.setAmbiguity(value.ambiguity) + .setBestReprojErr(value.bestReprojErr) + .setAltReprojErr(value.altReprojErr); } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index f789738195..31bc7f2ea3 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -18,22 +18,44 @@ package org.photonvision.targeting; import edu.wpi.first.util.protobuf.Protobuf; -import org.photonvision.proto.PhotonTypes.ProtobufPhotonTrackedTarget; -import org.photonvision.proto.PhotonTypes.ProtobufPhotonPipelineResult; -import us.hebi.quickbuf.Descriptors.Descriptor; import java.util.ArrayList; import java.util.List; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.proto.PhotonTypes.ProtobufPhotonPipelineResult; +import org.photonvision.proto.PhotonTypes.ProtobufPhotonTrackedTarget; +import us.hebi.quickbuf.Descriptors.Descriptor; +import us.hebi.quickbuf.RepeatedMessage; /** Represents a pipeline result from a PhotonCamera. */ public class PhotonPipelineResult { private static boolean HAS_WARNED = false; + // Targets to store. public final List targets = new ArrayList<>(); - private final double latencyMillis; - private final MultiTargetPNPResults multiTagResult; + // Latency in milliseconds. + private double latencyMillis; + + // Timestamp in milliseconds. private double timestampSeconds = -1; + // Multi-tag result + private MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults(); + + /** Constructs an empty pipeline result. */ + public PhotonPipelineResult() {} + + /** + * Constructs a pipeline result. + * + * @param latencyMillis The latency in the pipeline. + * @param targets The list of targets identified by the pipeline. + */ + public PhotonPipelineResult(double latencyMillis, List targets) { + this.latencyMillis = latencyMillis; + this.targets.addAll(targets); + } + /** * Constructs a pipeline result. * @@ -48,6 +70,18 @@ public PhotonPipelineResult( this.multiTagResult = result; } + /** + * Returns the size of the packet needed to store this pipeline result. + * + * @return The size of the packet needed to store this pipeline result. + */ + public int getPacketSize() { + return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES + + 8 // latency + + MultiTargetPNPResults.PACK_SIZE_BYTES + + 1; // target count + } + /** * Returns the best target in this pipeline result. If there are no targets, this method will * return null. The best target is determined by the target sort mode in the PhotonVision UI. @@ -76,15 +110,6 @@ public double getLatencyMillis() { return latencyMillis; } - /** - * Sets the FPGA timestamp of this result in seconds. - * - * @param timestampSeconds The timestamp in seconds. - */ - public void setTimestampSeconds(double timestampSeconds) { - this.timestampSeconds = timestampSeconds; - } - /** * Returns the estimated time the frame was taken, This is more accurate than using * getLatencyMillis() @@ -95,13 +120,22 @@ public double getTimestampSeconds() { return timestampSeconds; } + /** + * Sets the FPGA timestamp of this result in seconds. + * + * @param timestampSeconds The timestamp in seconds. + */ + public void setTimestampSeconds(double timestampSeconds) { + this.timestampSeconds = timestampSeconds; + } + /** * Returns whether the pipeline has targets. * * @return Whether the pipeline has targets. */ public boolean hasTargets() { - return !targets.isEmpty(); + return targets.size() > 0; } /** @@ -121,6 +155,49 @@ public MultiTargetPNPResults getMultiTagResult() { return multiTagResult; } + /** + * Populates the fields of the pipeline result from the packet. + * + * @param packet The incoming packet. + * @return The incoming packet. + */ + public Packet createFromPacket(Packet packet) { + // Decode latency, existence of targets, and number of targets. + latencyMillis = packet.decodeDouble(); + this.multiTagResult = MultiTargetPNPResults.createFromPacket(packet); + byte targetCount = packet.decodeByte(); + + targets.clear(); + + // Decode the information of each target. + for (int i = 0; i < (int) targetCount; ++i) { + var target = new PhotonTrackedTarget(); + target.createFromPacket(packet); + targets.add(target); + } + + return packet; + } + + /** + * Populates the outgoing packet with information from this pipeline result. + * + * @param packet The outgoing packet. + * @return The outgoing packet. + */ + public Packet populatePacket(Packet packet) { + // Encode latency, existence of targets, and number of targets. + packet.encode(latencyMillis); + multiTagResult.populatePacket(packet); + packet.encode((byte) targets.size()); + + // Encode the information of each target. + for (var target : targets) target.populatePacket(packet); + + // Return the packet. + return packet; + } + @Override public int hashCode() { final int prime = 31; @@ -167,7 +244,8 @@ public String toString() { + "]"; } - public static final class AProto implements Protobuf { + public static final class AProto + implements Protobuf { @Override public Class getTypeClass() { return PhotonPipelineResult.class; @@ -188,15 +266,17 @@ public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) { return new PhotonPipelineResult( msg.getLatencyMs(), PhotonTrackedTarget.proto.unpack(msg.getTargets()), - MultiTargetPNPResults.proto.unpack(msg.getMultiTargetResult()) - ); + MultiTargetPNPResults.proto.unpack(msg.getMultiTargetResult())); } @Override public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) { - ProtobufPhotonTrackedTarget[] targets = new ProtobufPhotonTrackedTarget[value.targets.size()]; - PhotonTrackedTarget.proto.pack(targets, value.targets); - msg.addAllTargets(targets); + RepeatedMessage targets = + msg.getMutableTargets().reserve(value.targets.size()); + for (int i = 0; i < value.targets.size(); i++) { + ProtobufPhotonTrackedTarget corner = targets.next(); + PhotonTrackedTarget.proto.pack(corner, value.targets.get(i)); + } MultiTargetPNPResults.proto.pack(msg.getMutableMultiTargetResult(), value.multiTagResult); diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index ed32cc1037..8fa4b75700 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -18,31 +18,37 @@ package org.photonvision.targeting; import edu.wpi.first.math.geometry.Transform3d; - +import edu.wpi.first.util.protobuf.Protobuf; import java.util.ArrayList; import java.util.List; -import edu.wpi.first.util.protobuf.Protobuf; +import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.proto.PhotonTypes.ProtobufPhotonTrackedTarget; import org.photonvision.proto.PhotonTypes.ProtobufTargetCorner; +import org.photonvision.utils.PacketUtils; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.RepeatedMessage; public class 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 Transform3d bestCameraToTarget; - private final Transform3d altCameraToTarget; - private final double poseAmbiguity; + public static final int PACK_SIZE_BYTES = + Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS); + + private double yaw; + private double pitch; + private double area; + private double skew; + private int fiducialId; + private Transform3d bestCameraToTarget = new Transform3d(); + private Transform3d altCameraToTarget = new Transform3d(); + private double poseAmbiguity; // Corners from the min-area rectangle bounding the target - private final List minAreaRectCorners; + private List minAreaRectCorners; + // Corners from whatever corner detection method was used - private final List detectedCorners; + private List detectedCorners; + + public PhotonTrackedTarget() {} /** Construct a tracked target, given exactly 4 corners */ public PhotonTrackedTarget( @@ -195,6 +201,81 @@ public boolean equals(Object obj) { return true; } + private static void encodeList(Packet packet, List list) { + packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE)); + for (int i = 0; i < list.size(); i++) { + packet.encode(list.get(i).x); + packet.encode(list.get(i).y); + } + } + + private static List decodeList(Packet p) { + byte len = p.decodeByte(); + var ret = new ArrayList(); + for (int i = 0; i < len; i++) { + double cx = p.decodeDouble(); + double cy = p.decodeDouble(); + ret.add(new TargetCorner(cx, cy)); + } + return ret; + } + + /** + * Populates the fields of this class with information from the incoming packet. + * + * @param packet The incoming packet. + * @return The incoming packet. + */ + public Packet createFromPacket(Packet packet) { + this.yaw = packet.decodeDouble(); + this.pitch = packet.decodeDouble(); + this.area = packet.decodeDouble(); + this.skew = packet.decodeDouble(); + this.fiducialId = packet.decodeInt(); + + this.bestCameraToTarget = PacketUtils.decodeTransform(packet); + this.altCameraToTarget = PacketUtils.decodeTransform(packet); + + this.poseAmbiguity = packet.decodeDouble(); + + this.minAreaRectCorners = new ArrayList<>(4); + for (int i = 0; i < 4; i++) { + double cx = packet.decodeDouble(); + double cy = packet.decodeDouble(); + minAreaRectCorners.add(new TargetCorner(cx, cy)); + } + + detectedCorners = decodeList(packet); + + return packet; + } + + /** + * Populates the outgoing packet with information from the current target. + * + * @param packet The outgoing packet. + * @return The outgoing packet. + */ + public Packet populatePacket(Packet packet) { + packet.encode(yaw); + packet.encode(pitch); + packet.encode(area); + packet.encode(skew); + packet.encode(fiducialId); + PacketUtils.encodeTransform(packet, bestCameraToTarget); + PacketUtils.encodeTransform(packet, altCameraToTarget); + packet.encode(poseAmbiguity); + + for (int i = 0; i < 4; i++) { + packet.encode(minAreaRectCorners.get(i).x); + packet.encode(minAreaRectCorners.get(i).y); + } + + encodeList(packet, detectedCorners); + + return packet; + } + @Override public String toString() { return "PhotonTrackedTarget{" @@ -215,7 +296,8 @@ public String toString() { + '}'; } - public static final class AProto implements Protobuf { + public static final class AProto + implements Protobuf { @Override public Class getTypeClass() { return PhotonTrackedTarget.class; @@ -226,7 +308,6 @@ public Descriptor getDescriptor() { return ProtobufPhotonTrackedTarget.getDescriptor(); } - @Override public ProtobufPhotonTrackedTarget createMessage() { return ProtobufPhotonTrackedTarget.newInstance(); @@ -244,13 +325,12 @@ public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) { Transform3d.proto.unpack(msg.getAltCameraToTarget()), msg.getPoseAmbiguity(), TargetCorner.proto.unpack(msg.getMinAreaRectCorners()), - TargetCorner.proto.unpack(msg.getDetectedCorners()) - ); + TargetCorner.proto.unpack(msg.getDetectedCorners())); } public List unpack(RepeatedMessage msg) { ArrayList targets = new ArrayList<>(msg.length()); - for(ProtobufPhotonTrackedTarget target : msg) { + for (ProtobufPhotonTrackedTarget target : msg) { targets.add(unpack(target)); } return targets; @@ -258,8 +338,7 @@ public List unpack(RepeatedMessage minAreaRectCorners = + msg.getMutableMinAreaRectCorners().reserve(value.minAreaRectCorners.size()); + for (int i = 0; i < value.minAreaRectCorners.size(); i++) { + ProtobufTargetCorner corner = minAreaRectCorners.next(); + TargetCorner.proto.pack(corner, value.minAreaRectCorners.get(i)); + } - public void pack(ProtobufPhotonTrackedTarget[] buffer, List targets) { - for(int i = 0; i < targets.size(); i++) { - var protoTarget = createMessage(); - pack(protoTarget, targets.get(i)); - buffer[i] = protoTarget; + RepeatedMessage detectedCorners = + msg.getMutableDetectedCorners().reserve(value.detectedCorners.size()); + for (int i = 0; i < value.detectedCorners.size(); i++) { + ProtobufTargetCorner corner = detectedCorners.next(); + TargetCorner.proto.pack(corner, value.detectedCorners.get(i)); } } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index 1a655215f1..2cd1cfcc54 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -17,10 +17,10 @@ package org.photonvision.targeting; +import edu.wpi.first.util.protobuf.Protobuf; import java.util.ArrayList; import java.util.List; import java.util.Objects; -import edu.wpi.first.util.protobuf.Protobuf; import org.photonvision.proto.PhotonTypes.ProtobufTargetCorner; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.RepeatedMessage; @@ -79,7 +79,7 @@ public TargetCorner unpack(ProtobufTargetCorner msg) { public List unpack(RepeatedMessage msg) { ArrayList corners = new ArrayList<>(msg.length()); - for(ProtobufTargetCorner corner : msg) { + for (ProtobufTargetCorner corner : msg) { corners.add(unpack(corner)); } return corners; @@ -89,14 +89,6 @@ public List unpack(RepeatedMessage msg) { public void pack(ProtobufTargetCorner msg, TargetCorner value) { msg.setX(value.x).setY(value.y); } - - public void pack(ProtobufTargetCorner[] buffer, List corners) { - for(int i = 0; i < corners.size(); i++) { - var protoCorner = createMessage(); - pack(protoCorner, corners.get(i)); - buffer[i] = protoCorner; - } - } } public static final AProto proto = new AProto(); From 6f5b24239d99a7b03e8130f90f00af4fb9d936b0 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 6 Nov 2023 00:01:50 -0500 Subject: [PATCH 15/74] Update SimPhotonCamera.java --- .../java/org/photonvision/simulation/SimPhotonCamera.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java index 58e3e2fcf7..d1c4b148db 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java @@ -143,9 +143,7 @@ public void submitProcessedFrame( PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResults()); - var newPacket = new Packet(newResult.getPacketSize()); - newResult.populatePacket(newPacket); - ts.rawBytesEntry.set(newPacket.getData()); + ts.rawBytesEntry.set(newResult); boolean hasTargets = newResult.hasTargets(); ts.hasTargetEntry.set(hasTargets); From db6ad1062f3b1a60bb41a9c4b1f48f61fa62e049 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 6 Nov 2023 00:13:15 -0500 Subject: [PATCH 16/74] rename rawBytesEntry -> pipelineResults pub/sub --- .../networktables/NTDataPublisher.java | 2 +- .../java/org/photonvision/PhotonCamera.java | 34 ++++++------------- .../simulation/PhotonCameraSim.java | 2 +- .../simulation/SimPhotonCamera.java | 3 +- .../common/networktables/NTTopicSet.java | 6 ++-- 5 files changed, 17 insertions(+), 30 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index 7baf0a2f27..24f5d0ad8d 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -134,7 +134,7 @@ public void accept(CVPipelineResult result) { TrackedTarget.simpleFromTrackedTargets(result.targets), result.multiTagResult); - ts.rawBytesEntry.set(res); + ts.pipelineResultsPublisher.set(res); ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get()); ts.driverModePublisher.set(driverModeSupplier.getAsBoolean()); diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index b4a8f85a51..b9f5c1b7d5 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -41,8 +41,8 @@ import edu.wpi.first.networktables.MultiSubscriber; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.ProtobufSubscriber; import edu.wpi.first.networktables.PubSubOption; -import edu.wpi.first.networktables.RawSubscriber; import edu.wpi.first.networktables.StringPublisher; import edu.wpi.first.networktables.StringSubscriber; import edu.wpi.first.wpilibj.DriverStation; @@ -58,7 +58,7 @@ public class PhotonCamera implements AutoCloseable { public static final String kTableName = "photonvision"; private final NetworkTable cameraTable; - RawSubscriber rawBytesEntry; + ProtobufSubscriber pipelineResultsSubscriber; BooleanPublisher driverModePublisher; BooleanSubscriber driverModeSubscriber; DoublePublisher latencyMillisEntry; @@ -79,7 +79,7 @@ public class PhotonCamera implements AutoCloseable { @Override public void close() { - rawBytesEntry.close(); + pipelineResultsSubscriber.close(); driverModePublisher.close(); driverModeSubscriber.close(); latencyMillisEntry.close(); @@ -138,14 +138,11 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { var photonvision_root_table = instance.getTable(kTableName); this.cameraTable = photonvision_root_table.getSubTable(cameraName); path = cameraTable.getPath(); - rawBytesEntry = + pipelineResultsSubscriber = cameraTable - .getRawTopic("result_proto") - .subscribe( - "proto:" + PhotonPipelineResult.proto.getDescriptor().getFullName(), - new byte[] {}, - PubSubOption.periodic(0.01), - PubSubOption.sendAll(true)); + .getProtobufTopic("result_proto", PhotonPipelineResult.proto) + .subscribe(null, PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); + driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish(); driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false); inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0); @@ -186,23 +183,14 @@ public PhotonCamera(String cameraName) { * @return The latest pipeline result. */ public PhotonPipelineResult getLatestResult() { - verifyVersion(); - - // Clear the packet. - packet.clear(); - - // Create latest result. - var ret = new PhotonPipelineResult(); - - // Populate packet and create result. - packet.setData(rawBytesEntry.get(new byte[] {})); + var ret = pipelineResultsSubscriber.get(); - if (packet.getSize() < 1) return ret; - ret.createFromPacket(packet); + // TOOD: handle no results (ret == null) // 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.setTimestampSeconds( + (pipelineResultsSubscriber.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3); // Return result. return ret; diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index 0e317afd47..bf5eb911fb 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -562,7 +562,7 @@ public void submitProcessedFrame(PhotonPipelineResult result) { public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimestamp) { ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp); - ts.rawBytesEntry.set(result, receiveTimestamp); + ts.pipelineResultsPublisher.set(result, receiveTimestamp); boolean hasTargets = result.hasTargets(); ts.hasTargetEntry.set(hasTargets, receiveTimestamp); diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java index d1c4b148db..8ccd59a7e3 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java @@ -33,7 +33,6 @@ import java.util.List; import org.photonvision.PhotonCamera; import org.photonvision.PhotonTargetSortMode; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.networktables.NTTopicSet; import org.photonvision.targeting.MultiTargetPNPResults; import org.photonvision.targeting.PhotonPipelineResult; @@ -143,7 +142,7 @@ public void submitProcessedFrame( PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResults()); - ts.rawBytesEntry.set(newResult); + ts.pipelineResultsPublisher.set(newResult); boolean hasTargets = newResult.hasTargets(); ts.hasTargetEntry.set(hasTargets); diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index 154d939121..5e995f2c32 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -40,7 +40,7 @@ */ public class NTTopicSet { public NetworkTable subTable; - public ProtobufPublisher rawBytesEntry; + public ProtobufPublisher pipelineResultsPublisher; public IntegerPublisher pipelineIndexPublisher; public IntegerSubscriber pipelineIndexRequestSub; @@ -70,7 +70,7 @@ public class NTTopicSet { public DoubleArrayPublisher cameraDistortionPublisher; public void updateEntries() { - rawBytesEntry = + pipelineResultsPublisher = subTable .getProtobufTopic("result_proto", PhotonPipelineResult.proto) .publish(PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); @@ -105,7 +105,7 @@ public void updateEntries() { @SuppressWarnings("DuplicatedCode") public void removeEntries() { - if (rawBytesEntry != null) rawBytesEntry.close(); + if (pipelineResultsPublisher != null) pipelineResultsPublisher.close(); if (pipelineIndexPublisher != null) pipelineIndexPublisher.close(); if (pipelineIndexRequestSub != null) pipelineIndexRequestSub.close(); From 6d5759cf0f9fe1259f49adde1660eafecf562231 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 6 Nov 2023 01:32:06 -0500 Subject: [PATCH 17/74] handle crap --- photon-lib/src/main/java/org/photonvision/PhotonCamera.java | 4 +--- .../java/org/photonvision/simulation/PhotonCameraSim.java | 4 ++-- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index b9f5c1b7d5..43f8d33e60 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -141,7 +141,7 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { pipelineResultsSubscriber = cameraTable .getProtobufTopic("result_proto", PhotonPipelineResult.proto) - .subscribe(null, PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); + .subscribe(new PhotonPipelineResult(), PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish(); driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false); @@ -185,8 +185,6 @@ public PhotonCamera(String cameraName) { public PhotonPipelineResult getLatestResult() { var ret = pipelineResultsSubscriber.get(); - // TOOD: handle no results (ret == null) - // Set the timestamp of the result. // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds. ret.setTimestampSeconds( diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index bf5eb911fb..c152140d3e 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -560,10 +560,10 @@ public void submitProcessedFrame(PhotonPipelineResult result) { * @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.pipelineResultsPublisher.set(result, receiveTimestamp); + ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp); + boolean hasTargets = result.hasTargets(); ts.hasTargetEntry.set(hasTargets, receiveTimestamp); if (!hasTargets) { From fafe163d4b887659a8c4ea8911a56e9a21328470 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 6 Nov 2023 13:28:28 -0500 Subject: [PATCH 18/74] Formatting fixes --- photon-lib/src/main/java/org/photonvision/PhotonCamera.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 43f8d33e60..d2db3e6d9d 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -141,7 +141,10 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { pipelineResultsSubscriber = cameraTable .getProtobufTopic("result_proto", PhotonPipelineResult.proto) - .subscribe(new PhotonPipelineResult(), PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); + .subscribe( + new PhotonPipelineResult(), + PubSubOption.periodic(0.01), + PubSubOption.sendAll(true)); driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish(); driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false); From d9c32dedd54cd72fe28db62c07bdf10b8db30102 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 6 Nov 2023 13:28:39 -0500 Subject: [PATCH 19/74] Add protobuf tests --- .../targeting/MultiTargetPNPResultsTest.java | 30 +++++ .../targeting/PNPResultsTest.java | 24 ++++ .../targeting/PhotonPipelineResultTest.java | 121 ++++++++++++++++++ .../targeting/PhotonTrackedTargetTest.java | 45 +++++++ .../targeting/TargetCornerTest.java | 16 +++ 5 files changed, 236 insertions(+) create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultsTest.java create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/PNPResultsTest.java create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java create mode 100644 photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultsTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultsTest.java new file mode 100644 index 0000000000..fb7e051b08 --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultsTest.java @@ -0,0 +1,30 @@ +package org.photonvision.targeting; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class MultiTargetPNPResultsTest { + @Test + public void protobufTest() { + var result = new MultiTargetPNPResults(); + var serializedResult = MultiTargetPNPResults.proto.createMessage(); + MultiTargetPNPResults.proto.pack(serializedResult, result); + var unpackedResult = MultiTargetPNPResults.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + + result = + new MultiTargetPNPResults( + new PNPResults( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3)); + serializedResult = MultiTargetPNPResults.proto.createMessage(); + MultiTargetPNPResults.proto.pack(serializedResult, result); + unpackedResult = MultiTargetPNPResults.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultsTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultsTest.java new file mode 100644 index 0000000000..bc3e449c25 --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultsTest.java @@ -0,0 +1,24 @@ +package org.photonvision.targeting; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import org.junit.jupiter.api.Test; + +public class PNPResultsTest { + @Test + public void protobufTest() { + var pnpRes = new PNPResults(); + var serializedPNPRes = PNPResults.proto.createMessage(); + PNPResults.proto.pack(serializedPNPRes, pnpRes); + var unpackedPNPRes = PNPResults.proto.unpack(serializedPNPRes); + assertEquals(pnpRes, unpackedPNPRes); + + pnpRes = new PNPResults(new Transform3d(1, 2, 3, new Rotation3d(1, 2, 3)), 0.1); + serializedPNPRes = PNPResults.proto.createMessage(); + PNPResults.proto.pack(serializedPNPRes, pnpRes); + unpackedPNPRes = PNPResults.proto.unpack(serializedPNPRes); + assertEquals(pnpRes, unpackedPNPRes); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java new file mode 100644 index 0000000000..92442cbff7 --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java @@ -0,0 +1,121 @@ +package org.photonvision.targeting; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class PhotonPipelineResultTest { + @Test + public void protobufTest() { + // Empty Result + var result = new PhotonPipelineResult(); + var serializedResult = PhotonPipelineResult.proto.createMessage(); + PhotonPipelineResult.proto.pack(serializedResult, result); + var unpackedResult = PhotonPipelineResult.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + + // non multitag result + result = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + serializedResult = PhotonPipelineResult.proto.createMessage(); + PhotonPipelineResult.proto.pack(serializedResult, result); + unpackedResult = PhotonPipelineResult.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + + // multitag result + result = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResults( + new PNPResults( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3))); + serializedResult = PhotonPipelineResult.proto.createMessage(); + PhotonPipelineResult.proto.pack(serializedResult, result); + unpackedResult = PhotonPipelineResult.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java new file mode 100644 index 0000000000..20e436db6b --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java @@ -0,0 +1,45 @@ +package org.photonvision.targeting; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import org.junit.jupiter.api.Test; + +public class PhotonTrackedTargetTest { + @Test + public void protobufTest() { + var target = new PhotonTrackedTarget(); + var serializedTarget = PhotonTrackedTarget.proto.createMessage(); + PhotonTrackedTarget.proto.pack(serializedTarget, target); + var unpackedTarget = PhotonTrackedTarget.proto.unpack(serializedTarget); + assertEquals(target, unpackedTarget); + + target = + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + serializedTarget = PhotonTrackedTarget.proto.createMessage(); + PhotonTrackedTarget.proto.pack(serializedTarget, target); + unpackedTarget = PhotonTrackedTarget.proto.unpack(serializedTarget); + assertEquals(target, unpackedTarget); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java new file mode 100644 index 0000000000..9ade583956 --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java @@ -0,0 +1,16 @@ +package org.photonvision.targeting; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +public class TargetCornerTest { + @Test + public void protobufTest() { + var corner = new TargetCorner(0, 1); + var serializedCorner = TargetCorner.proto.createMessage(); + TargetCorner.proto.pack(serializedCorner, corner); + var unpackedCorner = TargetCorner.proto.unpack(serializedCorner); + assertEquals(corner, unpackedCorner); + } +} From 7246a8187671a6a977527168f321981f79fd1c76 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 6 Nov 2023 13:28:56 -0500 Subject: [PATCH 20/74] Fix bugs with packing and unpacking --- .../photonvision/targeting/MultiTargetPNPResults.java | 9 ++++----- .../main/java/org/photonvision/targeting/PNPResults.java | 7 ++++++- .../org/photonvision/targeting/PhotonTrackedTarget.java | 8 ++++++-- 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java index f53353e820..2da2994924 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java @@ -25,6 +25,7 @@ import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.proto.PhotonTypes.ProtobufMultiTargetPNPResults; import us.hebi.quickbuf.Descriptors.Descriptor; +import us.hebi.quickbuf.RepeatedInt; public class MultiTargetPNPResults { // Seeing 32 apriltags at once seems like a sane limit @@ -125,12 +126,10 @@ public MultiTargetPNPResults unpack(ProtobufMultiTargetPNPResults msg) { public void pack(ProtobufMultiTargetPNPResults msg, MultiTargetPNPResults value) { PNPResults.proto.pack(msg.getMutableEstimatedPose(), value.estimatedPose); - // TODO better way of doing this - int[] ids = new int[value.fiducialIDsUsed.size()]; - for (int i = 0; i < ids.length; i++) { - ids[i] = value.fiducialIDsUsed.get(i); + RepeatedInt idsUsed = msg.getMutableFiducialIdsUsed().reserve(value.fiducialIDsUsed.size()); + for (int i = 0; i < value.fiducialIDsUsed.size(); i++) { + idsUsed.add(value.fiducialIDsUsed.get(i)); } - msg.addAllFiducialIdsUsed(ids); } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java index 819d48c577..8598eb0312 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java @@ -189,6 +189,10 @@ public ProtobufPNPResults createMessage() { @Override public PNPResults unpack(ProtobufPNPResults msg) { + if (!msg.getIsPresent()) { + return new PNPResults(); + } + return new PNPResults( Transform3d.proto.unpack(msg.getBest()), Transform3d.proto.unpack(msg.getAlt()), @@ -203,7 +207,8 @@ public void pack(ProtobufPNPResults msg, PNPResults value) { Transform3d.proto.pack(msg.getMutableAlt(), value.alt); msg.setAmbiguity(value.ambiguity) .setBestReprojErr(value.bestReprojErr) - .setAltReprojErr(value.altReprojErr); + .setAltReprojErr(value.altReprojErr) + .setIsPresent(value.isPresent); } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 8fa4b75700..96ece1c277 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -43,10 +43,10 @@ public class PhotonTrackedTarget { private double poseAmbiguity; // Corners from the min-area rectangle bounding the target - private List minAreaRectCorners; + private List minAreaRectCorners = List.of(); // Corners from whatever corner detection method was used - private List detectedCorners; + private List detectedCorners = List.of(); public PhotonTrackedTarget() {} @@ -315,6 +315,10 @@ public ProtobufPhotonTrackedTarget createMessage() { @Override public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) { + if (msg.getMinAreaRectCorners().length() != 4) { + return new PhotonTrackedTarget(); + } + return new PhotonTrackedTarget( msg.getYaw(), msg.getPitch(), From 5d100cef668180c333d5f1c311e26e2dbb38f6ae Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 6 Nov 2023 15:05:53 -0500 Subject: [PATCH 21/74] Formatting fixes --- .../targeting/MultiTargetPNPResultsTest.java | 17 +++++++++++++++++ .../photonvision/targeting/PNPResultsTest.java | 17 +++++++++++++++++ .../targeting/PhotonPipelineResultTest.java | 17 +++++++++++++++++ .../targeting/PhotonTrackedTargetTest.java | 17 +++++++++++++++++ .../targeting/TargetCornerTest.java | 17 +++++++++++++++++ 5 files changed, 85 insertions(+) diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultsTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultsTest.java index fb7e051b08..6b72ffdf97 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultsTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultsTest.java @@ -1,3 +1,20 @@ +/* + * 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 . + */ + package org.photonvision.targeting; import static org.junit.jupiter.api.Assertions.assertEquals; diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultsTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultsTest.java index bc3e449c25..55e015f76a 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultsTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultsTest.java @@ -1,3 +1,20 @@ +/* + * 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 . + */ + package org.photonvision.targeting; import static org.junit.jupiter.api.Assertions.assertEquals; diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java index 92442cbff7..f103fc4e3c 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java @@ -1,3 +1,20 @@ +/* + * 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 . + */ + package org.photonvision.targeting; import static org.junit.jupiter.api.Assertions.assertEquals; diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java index 20e436db6b..cafdb36ae6 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java @@ -1,3 +1,20 @@ +/* + * 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 . + */ + package org.photonvision.targeting; import static org.junit.jupiter.api.Assertions.assertEquals; diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java index 9ade583956..085a2fc9b0 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java @@ -1,3 +1,20 @@ +/* + * 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 . + */ + package org.photonvision.targeting; import static org.junit.jupiter.api.Assertions.assertEquals; From 2043802ee261eb66b8b7ead2223f95c0e8421122 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 8 Nov 2023 00:45:14 -0500 Subject: [PATCH 22/74] prob wont fix but one can hope --- .../java/org/photonvision/targeting/PhotonTrackedTarget.java | 3 ++- .../src/main/java/org/photonvision/targeting/TargetCorner.java | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 96ece1c277..1a66a5aae4 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -334,7 +334,8 @@ public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) { public List unpack(RepeatedMessage msg) { ArrayList targets = new ArrayList<>(msg.length()); - for (ProtobufPhotonTrackedTarget target : msg) { + for (int i = 0; i < msg.length(); i++) { + var target = msg.next(); targets.add(unpack(target)); } return targets; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index 2cd1cfcc54..f9b3f9eef0 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -79,7 +79,8 @@ public TargetCorner unpack(ProtobufTargetCorner msg) { public List unpack(RepeatedMessage msg) { ArrayList corners = new ArrayList<>(msg.length()); - for (ProtobufTargetCorner corner : msg) { + for (int i = 0; i < msg.length(); i++) { + var corner = msg.next(); corners.add(unpack(corner)); } return corners; From 6b9f0c938d2c1cda7c19778588f110840abe1a70 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 8 Nov 2023 17:59:23 -0500 Subject: [PATCH 23/74] add equality tests --- .../targeting/MultiTargetPNPResultsTest.java | 38 ++ .../targeting/PNPResultsTest.java | 51 +++ .../targeting/PhotonPipelineResultTest.java | 371 ++++++++++++++++++ .../targeting/PhotonTrackedTargetTest.java | 95 +++++ .../targeting/TargetCornerTest.java | 17 + 5 files changed, 572 insertions(+) diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultsTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultsTest.java index 6b72ffdf97..6f01196719 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultsTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultsTest.java @@ -18,6 +18,7 @@ package org.photonvision.targeting; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -44,4 +45,41 @@ public void protobufTest() { unpackedResult = MultiTargetPNPResults.proto.unpack(serializedResult); assertEquals(result, unpackedResult); } + + @Test + public void equalityTest() { + var a = new MultiTargetPNPResults(); + var b = new MultiTargetPNPResults(); + assertEquals(a, b); + + a = + new MultiTargetPNPResults( + new PNPResults( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3)); + + b = + new MultiTargetPNPResults( + new PNPResults( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3)); + + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = + new MultiTargetPNPResults( + new PNPResults( + new Transform3d(new Translation3d(1, 8, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(3, 4, 7)); + var b = + new MultiTargetPNPResults( + new PNPResults( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3)); + + assertNotEquals(a, b); + } } diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultsTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultsTest.java index 55e015f76a..26cdd7044e 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultsTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultsTest.java @@ -18,6 +18,7 @@ package org.photonvision.targeting; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -38,4 +39,54 @@ public void protobufTest() { unpackedPNPRes = PNPResults.proto.unpack(serializedPNPRes); assertEquals(pnpRes, unpackedPNPRes); } + + @Test + public void equalityTest() { + var a = new PNPResults(); + var b = new PNPResults(); + assertEquals(a, b); + + a = new PNPResults(new Transform3d(0, 1, 2, new Rotation3d()), 0.0); + b = new PNPResults(new Transform3d(0, 1, 2, new Rotation3d()), 0.0); + assertEquals(a, b); + + a = + new PNPResults( + new Transform3d(0, 1, 2, new Rotation3d()), + new Transform3d(3, 4, 5, new Rotation3d()), + 0.5, + 0.1, + 0.1); + b = + new PNPResults( + new Transform3d(0, 1, 2, new Rotation3d()), + new Transform3d(3, 4, 5, new Rotation3d()), + 0.5, + 0.1, + 0.1); + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = new PNPResults(new Transform3d(0, 1, 2, new Rotation3d()), 0.0); + var b = new PNPResults(new Transform3d(3, 4, 5, new Rotation3d()), 0.1); + assertNotEquals(a, b); + + a = + new PNPResults( + new Transform3d(3, 4, 5, new Rotation3d()), + new Transform3d(0, 1, 2, new Rotation3d()), + 0.5, + 0.1, + 0.1); + b = + new PNPResults( + new Transform3d(3, 4, 5, new Rotation3d()), + new Transform3d(0, 1, 2, new Rotation3d()), + 0.5, + 0.1, + 0.2); + assertNotEquals(a, b); + } } diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java index f103fc4e3c..bca777bcf7 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java @@ -18,6 +18,7 @@ package org.photonvision.targeting; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -135,4 +136,374 @@ public void protobufTest() { unpackedResult = PhotonPipelineResult.proto.unpack(serializedResult); assertEquals(result, unpackedResult); } + + @Test + public void equalityTest() { + var a = new PhotonPipelineResult(); + var b = new PhotonPipelineResult(); + assertEquals(a, b); + + a = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + b = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + assertEquals(a, b); + + a = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResults( + new PNPResults( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3))); + b = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResults( + new PNPResults( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3))); + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 7.0, + 2.0, + 1.0, + -9.0, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + var b = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 7.0, + 2.0, + 1.0, + -9.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + assertNotEquals(a, b); + + a = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResults( + new PNPResults( + new Transform3d(new Translation3d(1, 8, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(3, 4, 7))); + b = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResults( + new PNPResults( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3))); + assertNotEquals(a, b); + } } diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java index cafdb36ae6..b4bd658f63 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java @@ -18,6 +18,7 @@ package org.photonvision.targeting; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -59,4 +60,98 @@ public void protobufTest() { unpackedTarget = PhotonTrackedTarget.proto.unpack(serializedTarget); assertEquals(target, unpackedTarget); } + + @Test + public void equalityTest() { + var a = new PhotonTrackedTarget(); + var b = new PhotonTrackedTarget(); + assertEquals(a, b); + + a = + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + b = + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + assertEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + var b = + new PhotonTrackedTarget( + 7.0, + 2.0, + 1.0, + -9.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + assertNotEquals(a, b); + } } diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java index 085a2fc9b0..c317ae6fbc 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java @@ -18,6 +18,7 @@ package org.photonvision.targeting; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; import org.junit.jupiter.api.Test; @@ -30,4 +31,20 @@ public void protobufTest() { var unpackedCorner = TargetCorner.proto.unpack(serializedCorner); assertEquals(corner, unpackedCorner); } + + @Test + public void equalityTest() { + var a = new TargetCorner(0, 1); + var b = new TargetCorner(0, 1); + + assertNotEquals(a, b); + } + + @Test + public void inequalityTest() { + var a = new TargetCorner(0, 1); + var b = new TargetCorner(2, 4); + + assertNotEquals(a, b); + } } From 4a5f1453d468401351f7b5a5aaa3af397e0e618f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 8 Nov 2023 18:15:43 -0500 Subject: [PATCH 24/74] fix assertion test --- .../test/java/org/photonvision/targeting/TargetCornerTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java index c317ae6fbc..6bae08bd1c 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java @@ -37,7 +37,7 @@ public void equalityTest() { var a = new TargetCorner(0, 1); var b = new TargetCorner(0, 1); - assertNotEquals(a, b); + assertEquals(a, b); } @Test From 237b94d4fd1e1c3865efa74e516893bd43a7b383 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 8 Nov 2023 18:18:50 -0500 Subject: [PATCH 25/74] revert to before CI crash --- .../java/org/photonvision/targeting/PhotonTrackedTarget.java | 3 +-- .../main/java/org/photonvision/targeting/TargetCorner.java | 3 +-- photon-targeting/src/main/proto/photon_types.proto | 4 ++-- 3 files changed, 4 insertions(+), 6 deletions(-) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 1a66a5aae4..96ece1c277 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -334,8 +334,7 @@ public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) { public List unpack(RepeatedMessage msg) { ArrayList targets = new ArrayList<>(msg.length()); - for (int i = 0; i < msg.length(); i++) { - var target = msg.next(); + for (ProtobufPhotonTrackedTarget target : msg) { targets.add(unpack(target)); } return targets; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index f9b3f9eef0..2cd1cfcc54 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -79,8 +79,7 @@ public TargetCorner unpack(ProtobufTargetCorner msg) { public List unpack(RepeatedMessage msg) { ArrayList corners = new ArrayList<>(msg.length()); - for (int i = 0; i < msg.length(); i++) { - var corner = msg.next(); + for (ProtobufTargetCorner corner : msg) { corners.add(unpack(corner)); } return corners; diff --git a/photon-targeting/src/main/proto/photon_types.proto b/photon-targeting/src/main/proto/photon_types.proto index 4eb8eb95e1..d6778e4ff8 100644 --- a/photon-targeting/src/main/proto/photon_types.proto +++ b/photon-targeting/src/main/proto/photon_types.proto @@ -41,6 +41,6 @@ message ProtobufPhotonTrackedTarget { message ProtobufPhotonPipelineResult { double latency_ms = 1; - repeated ProtobufPhotonTrackedTarget targets = 3; - ProtobufMultiTargetPNPResults multi_target_result = 4; + repeated ProtobufPhotonTrackedTarget targets = 2; + ProtobufMultiTargetPNPResults multi_target_result = 3; } From c9e04b8a8686e3990d3b9e3eee12ca937c90761f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 8 Nov 2023 18:44:28 -0500 Subject: [PATCH 26/74] Remove Packet stuff --- .../vision/calibration/JsonMat.java | 6 - .../java/org/photonvision/PhotonCamera.java | 3 - .../java/org/photonvision/PacketTest.java | 190 --------------- .../org/photonvision/PhotonCameraTest.java | 46 ---- .../common/dataflow/structures/Packet.java | 216 ------------------ .../targeting/MultiTargetPNPResults.java | 28 --- .../photonvision/targeting/PNPResults.java | 28 --- .../targeting/PhotonPipelineResult.java | 58 +---- .../targeting/PhotonTrackedTarget.java | 79 ------- .../org/photonvision/utils/PacketUtils.java | 49 ---- 10 files changed, 1 insertion(+), 702 deletions(-) delete mode 100644 photon-lib/src/test/java/org/photonvision/PacketTest.java delete mode 100644 photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java delete mode 100644 photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java delete mode 100644 photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java index 15a6d0b9e0..4dc788a0a7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java @@ -26,7 +26,6 @@ import org.opencv.core.CvType; import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.vision.opencv.Releasable; public class JsonMat implements Releasable { @@ -121,9 +120,4 @@ public Matrix getAsWpilibMat() { public void release() { getAsMat().release(); } - - public Packet populatePacket(Packet packet) { - packet.encode(this.data); - return packet; - } } diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index d2db3e6d9d..ff6ed070d3 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -49,7 +49,6 @@ import edu.wpi.first.wpilibj.Timer; 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; @@ -117,8 +116,6 @@ public static void setVersionCheckEnabled(boolean enabled) { VERSION_CHECK_ENABLED = enabled; } - Packet packet = new Packet(1); - private static AprilTagFieldLayout lastSetTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); diff --git a/photon-lib/src/test/java/org/photonvision/PacketTest.java b/photon-lib/src/test/java/org/photonvision/PacketTest.java deleted file mode 100644 index c29a629b5b..0000000000 --- a/photon-lib/src/test/java/org/photonvision/PacketTest.java +++ /dev/null @@ -1,190 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.photonvision; - -import edu.wpi.first.math.geometry.*; -import java.util.ArrayList; -import java.util.List; -import org.junit.jupiter.api.Assertions; -import org.junit.jupiter.api.Test; -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 - void testSimpleTrackedTarget() { - var target = - new PhotonTrackedTarget( - 3.0, - 4.0, - 9.0, - -5.0, - -1, - new Transform3d(new Translation3d(), new Rotation3d()), - new Transform3d(new Translation3d(), new Rotation3d()), - 0.25, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))); - var p = new Packet(PhotonTrackedTarget.PACK_SIZE_BYTES); - target.populatePacket(p); - - var b = new PhotonTrackedTarget(); - b.createFromPacket(p); - - Assertions.assertEquals(target, b); - } - - @Test - void testSimplePipelineResult() { - var result = new PhotonPipelineResult(1, new ArrayList<>()); - var p = new Packet(result.getPacketSize()); - result.populatePacket(p); - - var b = new PhotonPipelineResult(); - b.createFromPacket(p); - - Assertions.assertEquals(result, b); - - var result2 = - new PhotonPipelineResult( - 2, - List.of( - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.0, - 4.0, - 2, - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - 0.25, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.1, - 6.7, - 3, - new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), - new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), - 0.25, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))))); - var p2 = new Packet(result2.getPacketSize()); - result2.populatePacket(p2); - - var b2 = new PhotonPipelineResult(); - b2.createFromPacket(p2); - - Assertions.assertEquals(result2, b2); - } - - @Test - public void testMultiTargetSerde() { - var result = - new PhotonPipelineResult( - 2, - List.of( - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.0, - 4.0, - 2, - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - 0.25, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.1, - 6.7, - 3, - new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), - new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), - 0.25, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)))), - new MultiTargetPNPResults( - new PNPResults( - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), - List.of(1, 2, 3))); - - Packet packet = new Packet(result.getPacketSize()); - result.populatePacket(packet); - - var result_deserialized = new PhotonPipelineResult(); - result_deserialized.createFromPacket(packet); - - Assertions.assertEquals(result, result_deserialized); - } -} diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java deleted file mode 100644 index 8a3fb5fad8..0000000000 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ /dev/null @@ -1,46 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.photonvision; - -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 - public void testEmpty() { - Assertions.assertDoesNotThrow( - () -> { - var packet = new Packet(1); - var ret = new PhotonPipelineResult(); - packet.setData(new byte[0]); - if (packet.getSize() < 1) { - return; - } - ret.createFromPacket(packet); - }); - } -} diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java deleted file mode 100644 index e97e3f1658..0000000000 --- a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java +++ /dev/null @@ -1,216 +0,0 @@ -/* - * 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 . - */ - -package org.photonvision.common.dataflow.structures; - -/** 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. - * - * @param size The size of the packet buffer. - */ - public Packet(int size) { - this.size = size; - packetData = new byte[size]; - } - - /** - * Constructs a packet with the given data. - * - * @param data The packet data. - */ - 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]; - readPos = 0; - writePos = 0; - } - - public int getSize() { - return size; - } - - /** - * Returns the packet data. - * - * @return The packet data. - */ - public byte[] getData() { - return packetData; - } - - /** - * Sets the packet data. - * - * @param data The packet data. - */ - public void setData(byte[] data) { - packetData = data; - size = data.length; - } - - /** - * Encodes the byte into the packet. - * - * @param src The byte to encode. - */ - public void encode(byte src) { - packetData[writePos++] = src; - } - - /** - * Encodes the short into the packet. - * - * @param src The short to encode. - */ - public void encode(short src) { - packetData[writePos++] = (byte) (src >>> 8); - packetData[writePos++] = (byte) src; - } - - /** - * Encodes the integer into the packet. - * - * @param src The integer to encode. - */ - public void encode(int src) { - packetData[writePos++] = (byte) (src >>> 24); - packetData[writePos++] = (byte) (src >>> 16); - packetData[writePos++] = (byte) (src >>> 8); - packetData[writePos++] = (byte) src; - } - - /** - * Encodes the double into the packet. - * - * @param src The double to encode. - */ - public void encode(double src) { - long data = Double.doubleToRawLongBits(src); - packetData[writePos++] = (byte) ((data >> 56) & 0xff); - packetData[writePos++] = (byte) ((data >> 48) & 0xff); - packetData[writePos++] = (byte) ((data >> 40) & 0xff); - packetData[writePos++] = (byte) ((data >> 32) & 0xff); - packetData[writePos++] = (byte) ((data >> 24) & 0xff); - packetData[writePos++] = (byte) ((data >> 16) & 0xff); - packetData[writePos++] = (byte) ((data >> 8) & 0xff); - packetData[writePos++] = (byte) (data & 0xff); - } - - /** - * Encodes the boolean into the packet. - * - * @param src The boolean to encode. - */ - public void encode(boolean src) { - packetData[writePos++] = src ? (byte) 1 : (byte) 0; - } - - /** - * Returns a decoded byte from the packet. - * - * @return A decoded byte from the packet. - */ - public byte decodeByte() { - if (packetData.length < readPos) { - return '\0'; - } - return packetData[readPos++]; - } - - /** - * Returns a decoded int from the packet. - * - * @return A decoded int from the packet. - */ - public int decodeInt() { - if (packetData.length < readPos + 3) { - return 0; - } - return (0xff & packetData[readPos++]) << 24 - | (0xff & packetData[readPos++]) << 16 - | (0xff & packetData[readPos++]) << 8 - | (0xff & packetData[readPos++]); - } - - /** - * Returns a decoded double from the packet. - * - * @return A decoded double from the packet. - */ - public double decodeDouble() { - if (packetData.length < (readPos + 7)) { - return 0; - } - long data = - (long) (0xff & packetData[readPos++]) << 56 - | (long) (0xff & packetData[readPos++]) << 48 - | (long) (0xff & packetData[readPos++]) << 40 - | (long) (0xff & packetData[readPos++]) << 32 - | (long) (0xff & packetData[readPos++]) << 24 - | (long) (0xff & packetData[readPos++]) << 16 - | (long) (0xff & packetData[readPos++]) << 8 - | (long) (0xff & packetData[readPos++]); - return Double.longBitsToDouble(data); - } - - /** - * Returns a decoded boolean from the packet. - * - * @return A decoded boolean from the packet. - */ - public boolean decodeBoolean() { - if (packetData.length < readPos) { - return false; - } - return packetData[readPos++] == 1; - } - - public void encode(double[] data) { - for (double d : data) { - encode(d); - } - } - - public double[] decodeDoubleArray(int len) { - double[] ret = new double[len]; - for (int i = 0; i < len; i++) { - ret[i] = decodeDouble(); - } - return ret; - } - - public short decodeShort() { - if (packetData.length < readPos + 1) { - return 0; - } - return (short) ((0xff & packetData[readPos++]) << 8 | (0xff & packetData[readPos++])); - } -} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java index 2da2994924..5334264645 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java @@ -18,21 +18,14 @@ package org.photonvision.targeting; import edu.wpi.first.util.protobuf.Protobuf; -import java.util.ArrayList; import java.util.Arrays; import java.util.List; import java.util.stream.Collectors; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.proto.PhotonTypes.ProtobufMultiTargetPNPResults; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.RepeatedInt; public class MultiTargetPNPResults { - // Seeing 32 apriltags at once seems like a sane limit - private static final int MAX_IDS = 32; - // pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally) - public static final int PACK_SIZE_BYTES = PNPResults.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS); - public PNPResults estimatedPose = new PNPResults(); public List fiducialIDsUsed = List.of(); @@ -43,27 +36,6 @@ public MultiTargetPNPResults(PNPResults results, List ids) { fiducialIDsUsed = ids; } - public static MultiTargetPNPResults createFromPacket(Packet packet) { - var results = PNPResults.createFromPacket(packet); - var ids = new ArrayList(MAX_IDS); - for (int i = 0; i < MAX_IDS; i++) { - int targetId = (int) packet.decodeShort(); - if (targetId > -1) ids.add(targetId); - } - return new MultiTargetPNPResults(results, ids); - } - - public void populatePacket(Packet packet) { - estimatedPose.populatePacket(packet); - for (int i = 0; i < MAX_IDS; i++) { - if (i < fiducialIDsUsed.size()) { - packet.encode((short) fiducialIDsUsed.get(i).byteValue()); - } else { - packet.encode((short) -1); - } - } - } - @Override public int hashCode() { final int prime = 31; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java index 8598eb0312..23c26f0488 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java @@ -19,9 +19,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.util.protobuf.Protobuf; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.proto.PhotonTypes.ProtobufPNPResults; -import org.photonvision.utils.PacketUtils; import us.hebi.quickbuf.Descriptors.Descriptor; /** @@ -89,32 +87,6 @@ public PNPResults( this.altReprojErr = altReprojErr; } - public static final int PACK_SIZE_BYTES = 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3); - - public static PNPResults createFromPacket(Packet packet) { - var present = packet.decodeBoolean(); - var best = PacketUtils.decodeTransform(packet); - var alt = PacketUtils.decodeTransform(packet); - var bestEr = packet.decodeDouble(); - var altEr = packet.decodeDouble(); - var ambiguity = packet.decodeDouble(); - if (present) { - return new PNPResults(best, alt, ambiguity, bestEr, altEr); - } else { - return new PNPResults(); - } - } - - public Packet populatePacket(Packet packet) { - packet.encode(isPresent); - PacketUtils.encodeTransform(packet, best); - PacketUtils.encodeTransform(packet, alt); - packet.encode(bestReprojErr); - packet.encode(altReprojErr); - packet.encode(ambiguity); - return packet; - } - @Override public int hashCode() { final int prime = 31; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 31bc7f2ea3..43a750a490 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -20,7 +20,6 @@ import edu.wpi.first.util.protobuf.Protobuf; import java.util.ArrayList; import java.util.List; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.proto.PhotonTypes.ProtobufPhotonPipelineResult; import org.photonvision.proto.PhotonTypes.ProtobufPhotonTrackedTarget; import us.hebi.quickbuf.Descriptors.Descriptor; @@ -70,18 +69,6 @@ public PhotonPipelineResult( this.multiTagResult = result; } - /** - * Returns the size of the packet needed to store this pipeline result. - * - * @return The size of the packet needed to store this pipeline result. - */ - public int getPacketSize() { - return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES - + 8 // latency - + MultiTargetPNPResults.PACK_SIZE_BYTES - + 1; // target count - } - /** * Returns the best target in this pipeline result. If there are no targets, this method will * return null. The best target is determined by the target sort mode in the PhotonVision UI. @@ -135,7 +122,7 @@ public void setTimestampSeconds(double timestampSeconds) { * @return Whether the pipeline has targets. */ public boolean hasTargets() { - return targets.size() > 0; + return !targets.isEmpty(); } /** @@ -155,49 +142,6 @@ public MultiTargetPNPResults getMultiTagResult() { return multiTagResult; } - /** - * Populates the fields of the pipeline result from the packet. - * - * @param packet The incoming packet. - * @return The incoming packet. - */ - public Packet createFromPacket(Packet packet) { - // Decode latency, existence of targets, and number of targets. - latencyMillis = packet.decodeDouble(); - this.multiTagResult = MultiTargetPNPResults.createFromPacket(packet); - byte targetCount = packet.decodeByte(); - - targets.clear(); - - // Decode the information of each target. - for (int i = 0; i < (int) targetCount; ++i) { - var target = new PhotonTrackedTarget(); - target.createFromPacket(packet); - targets.add(target); - } - - return packet; - } - - /** - * Populates the outgoing packet with information from this pipeline result. - * - * @param packet The outgoing packet. - * @return The outgoing packet. - */ - public Packet populatePacket(Packet packet) { - // Encode latency, existence of targets, and number of targets. - packet.encode(latencyMillis); - multiTagResult.populatePacket(packet); - packet.encode((byte) targets.size()); - - // Encode the information of each target. - for (var target : targets) target.populatePacket(packet); - - // Return the packet. - return packet; - } - @Override public int hashCode() { final int prime = 31; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 96ece1c277..4c6ed5a76b 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -21,17 +21,13 @@ import edu.wpi.first.util.protobuf.Protobuf; import java.util.ArrayList; import java.util.List; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.proto.PhotonTypes.ProtobufPhotonTrackedTarget; import org.photonvision.proto.PhotonTypes.ProtobufTargetCorner; -import org.photonvision.utils.PacketUtils; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.RepeatedMessage; public class PhotonTrackedTarget { private static final int MAX_CORNERS = 8; - public static final int PACK_SIZE_BYTES = - Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS); private double yaw; private double pitch; @@ -201,81 +197,6 @@ public boolean equals(Object obj) { return true; } - private static void encodeList(Packet packet, List list) { - packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE)); - for (int i = 0; i < list.size(); i++) { - packet.encode(list.get(i).x); - packet.encode(list.get(i).y); - } - } - - private static List decodeList(Packet p) { - byte len = p.decodeByte(); - var ret = new ArrayList(); - for (int i = 0; i < len; i++) { - double cx = p.decodeDouble(); - double cy = p.decodeDouble(); - ret.add(new TargetCorner(cx, cy)); - } - return ret; - } - - /** - * Populates the fields of this class with information from the incoming packet. - * - * @param packet The incoming packet. - * @return The incoming packet. - */ - public Packet createFromPacket(Packet packet) { - this.yaw = packet.decodeDouble(); - this.pitch = packet.decodeDouble(); - this.area = packet.decodeDouble(); - this.skew = packet.decodeDouble(); - this.fiducialId = packet.decodeInt(); - - this.bestCameraToTarget = PacketUtils.decodeTransform(packet); - this.altCameraToTarget = PacketUtils.decodeTransform(packet); - - this.poseAmbiguity = packet.decodeDouble(); - - this.minAreaRectCorners = new ArrayList<>(4); - for (int i = 0; i < 4; i++) { - double cx = packet.decodeDouble(); - double cy = packet.decodeDouble(); - minAreaRectCorners.add(new TargetCorner(cx, cy)); - } - - detectedCorners = decodeList(packet); - - return packet; - } - - /** - * Populates the outgoing packet with information from the current target. - * - * @param packet The outgoing packet. - * @return The outgoing packet. - */ - public Packet populatePacket(Packet packet) { - packet.encode(yaw); - packet.encode(pitch); - packet.encode(area); - packet.encode(skew); - packet.encode(fiducialId); - PacketUtils.encodeTransform(packet, bestCameraToTarget); - PacketUtils.encodeTransform(packet, altCameraToTarget); - packet.encode(poseAmbiguity); - - for (int i = 0; i < 4; i++) { - packet.encode(minAreaRectCorners.get(i).x); - packet.encode(minAreaRectCorners.get(i).y); - } - - encodeList(packet, detectedCorners); - - return packet; - } - @Override public String toString() { return "PhotonTrackedTarget{" diff --git a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java deleted file mode 100644 index 43a0dd4cf6..0000000000 --- a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java +++ /dev/null @@ -1,49 +0,0 @@ -/* - * 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 . - */ - -package org.photonvision.utils; - -import edu.wpi.first.math.geometry.Quaternion; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import org.photonvision.common.dataflow.structures.Packet; - -public class PacketUtils { - public static Transform3d decodeTransform(Packet packet) { - double x = packet.decodeDouble(); - double y = packet.decodeDouble(); - double z = packet.decodeDouble(); - var translation = new Translation3d(x, y, z); - double w = packet.decodeDouble(); - x = packet.decodeDouble(); - y = packet.decodeDouble(); - z = packet.decodeDouble(); - var rotation = new Rotation3d(new Quaternion(w, x, y, z)); - return new Transform3d(translation, rotation); - } - - public static void encodeTransform(Packet packet, Transform3d transform) { - packet.encode(transform.getTranslation().getX()); - packet.encode(transform.getTranslation().getY()); - packet.encode(transform.getTranslation().getZ()); - packet.encode(transform.getRotation().getQuaternion().getW()); - packet.encode(transform.getRotation().getQuaternion().getX()); - packet.encode(transform.getRotation().getQuaternion().getY()); - packet.encode(transform.getRotation().getQuaternion().getZ()); - } -} From c441321e9565699f6872badbb7f2ead24ffd7d38 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 8 Nov 2023 19:15:35 -0500 Subject: [PATCH 27/74] general cleanup of photon-targetting --- .../main/java/org/photonvision/estimation/OpenCVHelp.java | 4 ++-- .../main/java/org/photonvision/estimation/TargetModel.java | 2 +- .../java/org/photonvision/estimation/VisionEstimation.java | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 84e5d8d7cc..5cece16f1a 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -170,8 +170,8 @@ public static Point[] cornersToPoints(TargetCorner... corners) { public static List pointsToCorners(Point... points) { var corners = new ArrayList(points.length); - for (int i = 0; i < points.length; i++) { - corners.add(new TargetCorner(points[i].x, points[i].y)); + for (Point point : points) { + corners.add(new TargetCorner(point.x, point.y)); } return corners; } diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java index f945d2c806..aa77a1e5d9 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java @@ -151,7 +151,7 @@ public TargetModel(List vertices) { */ public List getFieldVertices(Pose3d targetPose) { var basisChange = new RotTrlTransform3d(targetPose.getRotation(), targetPose.getTranslation()); - return vertices.stream().map(t -> basisChange.apply(t)).collect(Collectors.toList()); + return vertices.stream().map(basisChange::apply).collect(Collectors.toList()); } /** diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java index 9ad6e0fc4e..51f6bd2f61 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -74,8 +74,8 @@ public static PNPResults estimateCamPosePNP( TargetModel tagModel) { if (tagLayout == null || visTags == null - || tagLayout.getTags().size() == 0 - || visTags.size() == 0) { + || tagLayout.getTags().isEmpty() + || visTags.isEmpty()) { return new PNPResults(); } @@ -92,7 +92,7 @@ public static PNPResults estimateCamPosePNP( corners.addAll(tgt.getDetectedCorners()); }); } - if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { + if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) { return new PNPResults(); } Point[] points = OpenCVHelp.cornersToPoints(corners); From 7412c18dad5c83fb92cb6ef18a3de85f5b237679 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 12 Nov 2023 01:21:28 -0500 Subject: [PATCH 28/74] todo, squash me --- .../targeting/PhotonPipelineResult.java | 8 +------- .../targeting/PhotonTrackedTarget.java | 19 +++++++------------ .../photonvision/targeting/TargetCorner.java | 6 ++++++ 3 files changed, 14 insertions(+), 19 deletions(-) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 43a750a490..d14031878f 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -215,13 +215,7 @@ public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) { @Override public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) { - RepeatedMessage targets = - msg.getMutableTargets().reserve(value.targets.size()); - for (int i = 0; i < value.targets.size(); i++) { - ProtobufPhotonTrackedTarget corner = targets.next(); - PhotonTrackedTarget.proto.pack(corner, value.targets.get(i)); - } - + PhotonTrackedTarget.proto.pack(msg.getMutableTargets(), value.getTargets()); MultiTargetPNPResults.proto.pack(msg.getMutableMultiTargetResult(), value.multiTagResult); msg.setLatencyMs(value.getLatencyMillis()); diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 4c6ed5a76b..a39157f6eb 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -254,6 +254,7 @@ public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) { } public List unpack(RepeatedMessage msg) { + // TODO add test ArrayList targets = new ArrayList<>(msg.length()); for (ProtobufPhotonTrackedTarget target : msg) { targets.add(unpack(target)); @@ -273,19 +274,13 @@ public void pack(ProtobufPhotonTrackedTarget msg, PhotonTrackedTarget value) { Transform3d.proto.pack(msg.getMutableBestCameraToTarget(), value.bestCameraToTarget); Transform3d.proto.pack(msg.getMutableAltCameraToTarget(), value.altCameraToTarget); - RepeatedMessage minAreaRectCorners = - msg.getMutableMinAreaRectCorners().reserve(value.minAreaRectCorners.size()); - for (int i = 0; i < value.minAreaRectCorners.size(); i++) { - ProtobufTargetCorner corner = minAreaRectCorners.next(); - TargetCorner.proto.pack(corner, value.minAreaRectCorners.get(i)); - } + TargetCorner.proto.pack(msg.getMutableMinAreaRectCorners(), value.getMinAreaRectCorners()); + TargetCorner.proto.pack(msg.getMutableDetectedCorners(), value.getDetectedCorners()); + } - RepeatedMessage detectedCorners = - msg.getMutableDetectedCorners().reserve(value.detectedCorners.size()); - for (int i = 0; i < value.detectedCorners.size(); i++) { - ProtobufTargetCorner corner = detectedCorners.next(); - TargetCorner.proto.pack(corner, value.detectedCorners.get(i)); - } + public void pack(RepeatedMessage msg, List value) { + // TODO write + // TODO add test } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index 2cd1cfcc54..edcc3fc17e 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -78,6 +78,7 @@ public TargetCorner unpack(ProtobufTargetCorner msg) { } public List unpack(RepeatedMessage msg) { + // TODO add test ArrayList corners = new ArrayList<>(msg.length()); for (ProtobufTargetCorner corner : msg) { corners.add(unpack(corner)); @@ -89,6 +90,11 @@ public List unpack(RepeatedMessage msg) { public void pack(ProtobufTargetCorner msg, TargetCorner value) { msg.setX(value.x).setY(value.y); } + + public void pack(RepeatedMessage msg, List value) { + // TODO write + // TODO add test + } } public static final AProto proto = new AProto(); From 3eb4d4041c6e6cb6ad5a6e8069dde01427515d01 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 12 Nov 2023 11:21:24 -0500 Subject: [PATCH 29/74] add missing descriptor field --- .../org/photonvision/targeting/MultiTargetPNPResults.java | 5 +++++ .../main/java/org/photonvision/targeting/PNPResults.java | 5 +++++ .../org/photonvision/targeting/PhotonPipelineResult.java | 7 +++++-- .../org/photonvision/targeting/PhotonTrackedTarget.java | 5 +++++ 4 files changed, 20 insertions(+), 2 deletions(-) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java index 5334264645..5a423054a4 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java @@ -81,6 +81,11 @@ public Descriptor getDescriptor() { return ProtobufMultiTargetPNPResults.getDescriptor(); } + @Override + public Protobuf[] getNested() { + return new Protobuf[] {PNPResults.proto}; + } + @Override public ProtobufMultiTargetPNPResults createMessage() { return ProtobufMultiTargetPNPResults.newInstance(); diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java index 23c26f0488..800fe4ed65 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java @@ -154,6 +154,11 @@ public Descriptor getDescriptor() { return ProtobufPNPResults.getDescriptor(); } + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Transform3d.proto}; + } + @Override public ProtobufPNPResults createMessage() { return ProtobufPNPResults.newInstance(); diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index d14031878f..b6cfd09fd6 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -21,9 +21,7 @@ import java.util.ArrayList; import java.util.List; import org.photonvision.proto.PhotonTypes.ProtobufPhotonPipelineResult; -import org.photonvision.proto.PhotonTypes.ProtobufPhotonTrackedTarget; import us.hebi.quickbuf.Descriptors.Descriptor; -import us.hebi.quickbuf.RepeatedMessage; /** Represents a pipeline result from a PhotonCamera. */ public class PhotonPipelineResult { @@ -200,6 +198,11 @@ public Descriptor getDescriptor() { return ProtobufPhotonPipelineResult.getDescriptor(); } + @Override + public Protobuf[] getNested() { + return new Protobuf[] {PhotonTrackedTarget.proto, MultiTargetPNPResults.proto}; + } + @Override public ProtobufPhotonPipelineResult createMessage() { return ProtobufPhotonPipelineResult.newInstance(); diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index a39157f6eb..a0c3e1c563 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -229,6 +229,11 @@ public Descriptor getDescriptor() { return ProtobufPhotonTrackedTarget.getDescriptor(); } + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Transform3d.proto, TargetCorner.proto}; + } + @Override public ProtobufPhotonTrackedTarget createMessage() { return ProtobufPhotonTrackedTarget.newInstance(); From e5c15fa26aaac61ae549de1efb8d8c043c2a96eb Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 12 Nov 2023 11:22:42 -0500 Subject: [PATCH 30/74] add tests for Protobuf list pack and unpack --- .../targeting/PhotonTrackedTarget.java | 12 ++-- .../photonvision/targeting/TargetCorner.java | 8 ++- .../targeting/PhotonTrackedTargetTest.java | 58 +++++++++++++++++++ .../targeting/TargetCornerTest.java | 20 +++++++ 4 files changed, 90 insertions(+), 8 deletions(-) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index a0c3e1c563..c7dd8cc173 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -22,7 +22,6 @@ import java.util.ArrayList; import java.util.List; import org.photonvision.proto.PhotonTypes.ProtobufPhotonTrackedTarget; -import org.photonvision.proto.PhotonTypes.ProtobufTargetCorner; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.RepeatedMessage; @@ -259,7 +258,6 @@ public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) { } public List unpack(RepeatedMessage msg) { - // TODO add test ArrayList targets = new ArrayList<>(msg.length()); for (ProtobufPhotonTrackedTarget target : msg) { targets.add(unpack(target)); @@ -283,9 +281,13 @@ public void pack(ProtobufPhotonTrackedTarget msg, PhotonTrackedTarget value) { TargetCorner.proto.pack(msg.getMutableDetectedCorners(), value.getDetectedCorners()); } - public void pack(RepeatedMessage msg, List value) { - // TODO write - // TODO add test + public void pack( + RepeatedMessage msg, List value) { + var targets = msg.reserve(value.size()); + for (PhotonTrackedTarget trackedTarget : value) { + var target = targets.next(); + pack(target, trackedTarget); + } } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index edcc3fc17e..773031b84a 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -78,7 +78,6 @@ public TargetCorner unpack(ProtobufTargetCorner msg) { } public List unpack(RepeatedMessage msg) { - // TODO add test ArrayList corners = new ArrayList<>(msg.length()); for (ProtobufTargetCorner corner : msg) { corners.add(unpack(corner)); @@ -92,8 +91,11 @@ public void pack(ProtobufTargetCorner msg, TargetCorner value) { } public void pack(RepeatedMessage msg, List value) { - // TODO write - // TODO add test + var corners = msg.reserve(value.size()); + for (TargetCorner targetCorner : value) { + var corner = corners.next(); + pack(corner, targetCorner); + } } } diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java index b4bd658f63..fc90d113c5 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java @@ -25,6 +25,8 @@ import edu.wpi.first.math.geometry.Translation3d; import java.util.List; import org.junit.jupiter.api.Test; +import org.photonvision.proto.PhotonTypes; +import us.hebi.quickbuf.RepeatedMessage; public class PhotonTrackedTargetTest { @Test @@ -61,6 +63,62 @@ public void protobufTest() { assertEquals(target, unpackedTarget); } + @Test + public void protobufListTest() { + List targets = List.of(); + var serializedTargets = + RepeatedMessage.newEmptyInstance(PhotonTypes.ProtobufPhotonTrackedTarget.getFactory()); + PhotonTrackedTarget.proto.pack(serializedTargets, targets); + var unpackedTargets = PhotonTrackedTarget.proto.unpack(serializedTargets); + assertEquals(targets, unpackedTargets); + + targets = + List.of( + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 7.0, + 2.0, + 1.0, + -9.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))); + serializedTargets = + RepeatedMessage.newEmptyInstance(PhotonTypes.ProtobufPhotonTrackedTarget.getFactory()); + PhotonTrackedTarget.proto.pack(serializedTargets, targets); + unpackedTargets = PhotonTrackedTarget.proto.unpack(serializedTargets); + assertEquals(targets, unpackedTargets); + } + @Test public void equalityTest() { var a = new PhotonTrackedTarget(); diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java index 6bae08bd1c..4de9e3a741 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java @@ -20,7 +20,10 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; +import java.util.List; import org.junit.jupiter.api.Test; +import org.photonvision.proto.PhotonTypes; +import us.hebi.quickbuf.RepeatedMessage; public class TargetCornerTest { @Test @@ -32,6 +35,23 @@ public void protobufTest() { assertEquals(corner, unpackedCorner); } + @Test + public void protobufListTest() { + List corners = List.of(); + var serializedCorners = + RepeatedMessage.newEmptyInstance(PhotonTypes.ProtobufTargetCorner.getFactory()); + TargetCorner.proto.pack(serializedCorners, corners); + var unpackedCorners = TargetCorner.proto.unpack(serializedCorners); + assertEquals(corners, unpackedCorners); + + corners = List.of(new TargetCorner(0, 1), new TargetCorner(1, 2)); + serializedCorners = + RepeatedMessage.newEmptyInstance(PhotonTypes.ProtobufTargetCorner.getFactory()); + TargetCorner.proto.pack(serializedCorners, corners); + unpackedCorners = TargetCorner.proto.unpack(serializedCorners); + assertEquals(corners, unpackedCorners); + } + @Test public void equalityTest() { var a = new TargetCorner(0, 1); From 73eae67bf282a180aa596cb91af192fe390c1854 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 12 Nov 2023 11:35:49 -0500 Subject: [PATCH 31/74] Update PhotonPipelineResult.java issue is with packing targets --- .../java/org/photonvision/targeting/PhotonPipelineResult.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index b6cfd09fd6..91b442aca4 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -218,7 +218,7 @@ public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) { @Override public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) { - PhotonTrackedTarget.proto.pack(msg.getMutableTargets(), value.getTargets()); + PhotonTrackedTarget.proto.pack(msg.getMutableTargets(), value.targets); MultiTargetPNPResults.proto.pack(msg.getMutableMultiTargetResult(), value.multiTagResult); msg.setLatencyMs(value.getLatencyMillis()); From 58fbe69d666e9a36f45f572737e436717a9da5a5 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 12 Nov 2023 13:59:40 -0500 Subject: [PATCH 32/74] Make PhotonTrackedTarget final --- .../targeting/PhotonPipelineResult.java | 6 ++--- .../targeting/PhotonTrackedTarget.java | 27 +++++++------------ .../targeting/PhotonTrackedTargetTest.java | 20 ++++---------- 3 files changed, 17 insertions(+), 36 deletions(-) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 91b442aca4..025acefc22 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -144,7 +144,7 @@ public MultiTargetPNPResults getMultiTagResult() { public int hashCode() { final int prime = 31; int result = 1; - result = prime * result + ((targets == null) ? 0 : targets.hashCode()); + result = prime * result + targets.hashCode(); long temp; temp = Double.doubleToLongBits(latencyMillis); result = prime * result + (int) (temp ^ (temp >>> 32)); @@ -160,9 +160,7 @@ public boolean equals(Object obj) { if (obj == null) return false; if (getClass() != obj.getClass()) return false; PhotonPipelineResult other = (PhotonPipelineResult) obj; - if (targets == null) { - if (other.targets != null) return false; - } else if (!targets.equals(other.targets)) return false; + if (!targets.equals(other.targets)) return false; if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis)) return false; if (Double.doubleToLongBits(timestampSeconds) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index c7dd8cc173..71c0f6665c 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -28,22 +28,19 @@ public class PhotonTrackedTarget { private static final int MAX_CORNERS = 8; - private double yaw; - private double pitch; - private double area; - private double skew; - private int fiducialId; - private Transform3d bestCameraToTarget = new Transform3d(); - private Transform3d altCameraToTarget = new Transform3d(); - private double poseAmbiguity; + private final double yaw; + private final double pitch; + private final double area; + private final double skew; + private final int fiducialId; + private final Transform3d bestCameraToTarget; + private final Transform3d altCameraToTarget; + private final double poseAmbiguity; // Corners from the min-area rectangle bounding the target - private List minAreaRectCorners = List.of(); - + private final List minAreaRectCorners; // Corners from whatever corner detection method was used - private List detectedCorners = List.of(); - - public PhotonTrackedTarget() {} + private final List detectedCorners; /** Construct a tracked target, given exactly 4 corners */ public PhotonTrackedTarget( @@ -240,10 +237,6 @@ public ProtobufPhotonTrackedTarget createMessage() { @Override public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) { - if (msg.getMinAreaRectCorners().length() != 4) { - return new PhotonTrackedTarget(); - } - return new PhotonTrackedTarget( msg.getYaw(), msg.getPitch(), diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java index fc90d113c5..0742c2361e 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java @@ -31,13 +31,7 @@ public class PhotonTrackedTargetTest { @Test public void protobufTest() { - var target = new PhotonTrackedTarget(); - var serializedTarget = PhotonTrackedTarget.proto.createMessage(); - PhotonTrackedTarget.proto.pack(serializedTarget, target); - var unpackedTarget = PhotonTrackedTarget.proto.unpack(serializedTarget); - assertEquals(target, unpackedTarget); - - target = + var target = new PhotonTrackedTarget( 3.0, 4.0, @@ -57,9 +51,9 @@ public void protobufTest() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))); - serializedTarget = PhotonTrackedTarget.proto.createMessage(); + var serializedTarget = PhotonTrackedTarget.proto.createMessage(); PhotonTrackedTarget.proto.pack(serializedTarget, target); - unpackedTarget = PhotonTrackedTarget.proto.unpack(serializedTarget); + var unpackedTarget = PhotonTrackedTarget.proto.unpack(serializedTarget); assertEquals(target, unpackedTarget); } @@ -121,11 +115,7 @@ public void protobufListTest() { @Test public void equalityTest() { - var a = new PhotonTrackedTarget(); - var b = new PhotonTrackedTarget(); - assertEquals(a, b); - - a = + var a = new PhotonTrackedTarget( 3.0, 4.0, @@ -145,7 +135,7 @@ public void equalityTest() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))); - b = + var b = new PhotonTrackedTarget( 3.0, 4.0, From a50e70e55c70915df109b83a482fb25a2a066a2a Mon Sep 17 00:00:00 2001 From: Matt Date: Mon, 13 Nov 2023 17:48:11 -0500 Subject: [PATCH 33/74] Very broken gradle code lol --- photon-lib/build.gradle | 7 +++++++ photon-targeting/build.gradle | 22 +++++++++++++++++++++- 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 02cb0fd064..841afc4bca 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -64,6 +64,12 @@ nativeUtils { } } + +// Gradle complains about using outputs without an explicit dependency w/o this +tasks.named("compileJava") { + dependsOn ":photon-targeting:generateNativePhotonProtos" +} + model { components { Photon(NativeLibrarySpec) { @@ -71,6 +77,7 @@ model { cpp { source { srcDirs "src/main/native/cpp" + srcDirs project(":photon-targeting").tasks.named("generateNativePhotonProtos").get().outputs.files include "**/*.cpp" } exportedHeaders { diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 5c85d4bcb5..33e8b8c77a 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -23,7 +23,7 @@ dependencies { sourceSets { main { java { - // TODO this is a hack! I think! Why are we generating wpilib quickbufs anyways? + // So quickbuffers generates messages for all included protobufs, even wpilib ones. For now, exclude srcDirs 'build/generated/source/proto/main/quickbuf/org' exclude 'build/generated/source/proto/main/quickbuf/edu/wpi/**' } @@ -71,4 +71,24 @@ protobuf { } } + +tasks.register("generateNativePhotonProtos") { + dependsOn tasks.named("generateProto") + + // This is also valid, but i'd need to append "cpp" to it + // tasks.named("generateProto").get().outputs.files.each { println it } + + // instead, hard-code + outputs.dir "build/generated/source/proto/main/cpp" + + // outputs.files.each { println it } + + // reflectino go brr + // tasks.named("generateProto").get().metaClass.properties.each { println it.name } + // it.outputs.metaClass.methods.each { println it.name } +} + + + + apply from: "publish.gradle" From c2a3c1e95533c414d5d1f6eb1da7885a0993076f Mon Sep 17 00:00:00 2001 From: Matt Date: Mon, 13 Nov 2023 17:59:58 -0500 Subject: [PATCH 34/74] Create unit test Can run explicitly with ./gradlew checkCppTestWindowsx86-64ReleaseGoogleTestExe --- photon-lib/build.gradle | 4 +++- photon-lib/src/test/native/cpp/ProtoTest.cpp | 1 + photon-targeting/build.gradle | 2 -- 3 files changed, 4 insertions(+), 3 deletions(-) create mode 100644 photon-lib/src/test/native/cpp/ProtoTest.cpp diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 841afc4bca..0c38acc425 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -67,7 +67,7 @@ nativeUtils { // Gradle complains about using outputs without an explicit dependency w/o this tasks.named("compileJava") { - dependsOn ":photon-targeting:generateNativePhotonProtos" + mustRunAfter ":photon-targeting:generateNativePhotonProtos" } model { @@ -83,6 +83,8 @@ model { exportedHeaders { srcDirs "src/main/native/include" srcDirs "src/generate/native/include" + srcDirs project(":photon-targeting").tasks.named("generateNativePhotonProtos").get().outputs.files + include "**/*.h" } } } diff --git a/photon-lib/src/test/native/cpp/ProtoTest.cpp b/photon-lib/src/test/native/cpp/ProtoTest.cpp new file mode 100644 index 0000000000..59de8f7dd3 --- /dev/null +++ b/photon-lib/src/test/native/cpp/ProtoTest.cpp @@ -0,0 +1 @@ +#include "photon_types.pb.h" diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 33e8b8c77a..8962b38d2e 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -89,6 +89,4 @@ tasks.register("generateNativePhotonProtos") { } - - apply from: "publish.gradle" From 25e6b4b0ce367c5bdeaf0b785f5943b8bb552967 Mon Sep 17 00:00:00 2001 From: Matt Date: Mon, 13 Nov 2023 18:03:28 -0500 Subject: [PATCH 35/74] Run lint --- photon-lib/src/test/native/cpp/ProtoTest.cpp | 24 ++++++++++++++++++++ photon-targeting/build.gradle | 4 ++-- 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/photon-lib/src/test/native/cpp/ProtoTest.cpp b/photon-lib/src/test/native/cpp/ProtoTest.cpp index 59de8f7dd3..8835e93fcb 100644 --- a/photon-lib/src/test/native/cpp/ProtoTest.cpp +++ b/photon-lib/src/test/native/cpp/ProtoTest.cpp @@ -1 +1,25 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + #include "photon_types.pb.h" diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 8962b38d2e..36b9a012ba 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -80,8 +80,8 @@ tasks.register("generateNativePhotonProtos") { // instead, hard-code outputs.dir "build/generated/source/proto/main/cpp" - - // outputs.files.each { println it } + + // outputs.files.each { println it } // reflectino go brr // tasks.named("generateProto").get().metaClass.properties.each { println it.name } From 07f7d49f2ab7096ac08fb5d2c3d73eeed7903b7c Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 13 Nov 2023 20:22:07 -0500 Subject: [PATCH 36/74] bump wpilib version to fix gradle issue --- build.gradle | 2 +- photon-server/src/main/resources/web/index.html | 16 +++++++++++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/build.gradle b/build.gradle index fdf2fa70cb..b55f7dd674 100644 --- a/build.gradle +++ b/build.gradle @@ -20,7 +20,7 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2024.1.1-beta-3" + wpilibVersion = "2024.1.1-beta-3-21-gd105f9e" openCVversion = "4.8.0-2" joglVersion = "2.4.0-rc-20200307" javalinVersion = "5.6.2" diff --git a/photon-server/src/main/resources/web/index.html b/photon-server/src/main/resources/web/index.html index 988f55e6a3..cae7a4db99 100644 --- a/photon-server/src/main/resources/web/index.html +++ b/photon-server/src/main/resources/web/index.html @@ -1 +1,15 @@ -

UI has not been copied!

+ + + + + + + Photon Client + + + + +
+ + + From 48d1da5c9b2346c2f9264a7da9a325414e8bbbab Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 13 Nov 2023 22:52:32 -0500 Subject: [PATCH 37/74] add verifyVersion() call back --- .../src/main/java/org/photonvision/PhotonCamera.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index ff6ed070d3..82834a9ac7 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -110,7 +110,7 @@ public void close() { private long prevHeartbeatValue = -1; private double prevHeartbeatChangeTime = 0; - private static final double HEARBEAT_DEBOUNCE_SEC = 0.5; + private static final double HEARTBEAT_DEBOUNCE_SEC = 0.5; public static void setVersionCheckEnabled(boolean enabled) { VERSION_CHECK_ENABLED = enabled; @@ -183,6 +183,8 @@ public PhotonCamera(String cameraName) { * @return The latest pipeline result. */ public PhotonPipelineResult getLatestResult() { + verifyVersion(); + var ret = pipelineResultsSubscriber.get(); // Set the timestamp of the result. @@ -318,7 +320,7 @@ public boolean isConnected() { prevHeartbeatValue = curHeartbeat; } - return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC; + return (now - prevHeartbeatChangeTime) < HEARTBEAT_DEBOUNCE_SEC; } // TODO: Implement ATFL subscribing in backend @@ -417,7 +419,7 @@ else if (!isConnected()) { // Check for version. Warn if the versions aren't aligned. String versionString = versionEntry.get(""); - if (!versionString.equals("") && !PhotonVersion.versionMatches(versionString)) { + if (!versionString.isEmpty() && !PhotonVersion.versionMatches(versionString)) { // Error on a verified version mismatch // But stay silent otherwise DriverStation.reportWarning( From 8bdb5b26f5c9804074547b8d37aee2216311b138 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 13 Nov 2023 22:54:35 -0500 Subject: [PATCH 38/74] Revert "bump wpilib version to fix gradle issue" This reverts commit 07f7d49f2ab7096ac08fb5d2c3d73eeed7903b7c. --- photon-server/src/main/resources/web/index.html | 16 +--------------- 1 file changed, 1 insertion(+), 15 deletions(-) diff --git a/photon-server/src/main/resources/web/index.html b/photon-server/src/main/resources/web/index.html index cae7a4db99..988f55e6a3 100644 --- a/photon-server/src/main/resources/web/index.html +++ b/photon-server/src/main/resources/web/index.html @@ -1,15 +1 @@ - - - - - - - Photon Client - - - - -
- - - +

UI has not been copied!

From c5c51e3f2ff469b05fd8ec9b416f4591225211ef Mon Sep 17 00:00:00 2001 From: Matt Date: Tue, 14 Nov 2023 07:28:08 -0500 Subject: [PATCH 39/74] stupid shim --- build.gradle | 2 +- .../src/main/native/cpp/photonlib/PhotonPipelineResult.cpp | 2 ++ .../native/cpp/photonlib/geometry3d.pb.h} | 6 +++++- 3 files changed, 8 insertions(+), 2 deletions(-) rename photon-lib/src/{test/native/cpp/ProtoTest.cpp => main/native/cpp/photonlib/geometry3d.pb.h} (83%) diff --git a/build.gradle b/build.gradle index b55f7dd674..9669d4f0e1 100644 --- a/build.gradle +++ b/build.gradle @@ -20,7 +20,7 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2024.1.1-beta-3-21-gd105f9e" + wpilibVersion = "2024.1.1-beta-3-22-g0f81296" openCVversion = "4.8.0-2" joglVersion = "2.4.0-rc-20200307" javalinVersion = "5.6.2" diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp index 462dd25a90..ab2f309352 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp @@ -24,6 +24,8 @@ #include "photonlib/PhotonPipelineResult.h" +#include "photon_types.pb.h" + namespace photonlib { PhotonPipelineResult::PhotonPipelineResult( units::second_t latency, std::span targets) diff --git a/photon-lib/src/test/native/cpp/ProtoTest.cpp b/photon-lib/src/main/native/cpp/photonlib/geometry3d.pb.h similarity index 83% rename from photon-lib/src/test/native/cpp/ProtoTest.cpp rename to photon-lib/src/main/native/cpp/photonlib/geometry3d.pb.h index 8835e93fcb..2bfe3ddb1d 100644 --- a/photon-lib/src/test/native/cpp/ProtoTest.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/geometry3d.pb.h @@ -22,4 +22,8 @@ * SOFTWARE. */ -#include "photon_types.pb.h" +#pragma once +// So wpilib publishes protbufs here at wpimath/protobuf. but generated code +// assumes that the protobuf includes are on your include path. So we need this +// stupid shim +#include "wpimath/protobuf/geometry3d.pb.h" From 067df21b479d425b7dd4f530bfc045eddc5d2961 Mon Sep 17 00:00:00 2001 From: Matt Date: Tue, 14 Nov 2023 20:08:17 -0500 Subject: [PATCH 40/74] Start on C++ things --- .../native/cpp/photonlib/PhotonCamera.cpp | 21 +++---- .../cpp/photonlib/PhotonPipelineResult.cpp | 45 ++++++++++++++ .../cpp/photonlib/PhotonTrackedTarget.cpp | 58 +++++++++++++++++++ .../native/include/photonlib/PhotonCamera.h | 4 +- .../include/photonlib/PhotonPipelineResult.h | 10 ++++ .../include/photonlib/PhotonTrackedTarget.h | 14 ++++- .../include/photonlib/SimPhotonCamera.h | 9 ++- .../src/test/native/cpp/PhotonUtilsTest.cpp | 28 --------- 8 files changed, 139 insertions(+), 50 deletions(-) delete mode 100644 photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp index d0901b10bd..32fa49b096 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp @@ -40,9 +40,12 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, const std::string_view cameraName) : mainTable(instance.GetTable("photonvision")), rootTable(mainTable->GetSubTable(cameraName)), - rawBytesEntry( - rootTable->GetRawTopic("rawBytes") - .Subscribe("rawBytes", {}, {.periodic = 0.01, .sendAll = true})), + // rawBytesEntry( + // rootTable->GetRawTopic("rawBytes") + // .Subscribe("rawBytes", {}, {.periodic = 0.01, .sendAll = true})), + pipelineResultsSubscriber( + rootTable->GetProtobufTopic("result_proto").Subscribe(PhotonPipelineResult(), {.periodic = 0.01, .sendAll = true}) + ), inputSaveImgEntry( rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()), inputSaveImgSubscriber( @@ -85,17 +88,9 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { packet.Clear(); // Create the new result; - PhotonPipelineResult result; + PhotonPipelineResult result = pipelineResultsSubscriber.Get(); - // Fill the packet with latest data and populate result. - const auto value = rawBytesEntry.Get(); - if (!value.size()) return result; - - photonlib::Packet packet{value}; - - packet >> result; - - result.SetTimestamp(units::microsecond_t(rawBytesEntry.GetLastChange()) - + result.SetTimestamp(units::microsecond_t(pipelineResultsSubscriber.GetLastChange()) - result.GetLatency()); return result; diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp index ab2f309352..6efc660058 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp @@ -71,3 +71,48 @@ Packet& operator>>(Packet& packet, PhotonPipelineResult& result) { } } // namespace photonlib + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufPhotonPipelineResult>(arena); +} + +photonlib::PhotonPipelineResult +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + using namespace photonlib; + using photonvision::proto::ProtobufPhotonPipelineResult; + + auto m = static_cast(&msg); + + std::vector targets; + targets.reserve(m->targets_size()); + for (const auto& t : m->targets()) { + targets.emplace_back(wpi::UnpackProtobuf(t)); + } + + // TODO -- multi-target + + return photonlib::PhotonPipelineResult{units::millisecond_t{m->latency_ms()}, + targets}; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, + const photonlib::PhotonPipelineResult& value) { + using namespace photonlib; + using photonvision::proto::ProtobufPhotonPipelineResult; + using photonvision::proto::ProtobufPhotonTrackedTarget; + + auto m = static_cast(msg); + + m->set_latency_ms(units::millisecond_t(value.GetLatency()).value()); + + m->clear_targets(); + for (const auto& t : value.GetTargets()) { + wpi::PackProtobuf(m->add_targets(), t); + } + + // TODO -- multi-target +} diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp index 84a5846fcd..68f70aa9a5 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp @@ -30,6 +30,8 @@ #include #include +#include "photon_types.pb.h" + static constexpr const uint8_t MAX_CORNERS = 8; namespace photonlib { @@ -145,3 +147,59 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) { } } // namespace photonlib + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufPhotonTrackedTarget>(arena); +} + +photonlib::PhotonTrackedTarget +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + using namespace photonlib; + using photonvision::proto::ProtobufPhotonTrackedTarget; + + auto m = static_cast(&msg); + + return photonlib::PhotonTrackedTarget{}; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, + const photonlib::PhotonTrackedTarget& value) { + using namespace photonlib; + using photonvision::proto::ProtobufPhotonTrackedTarget; + + auto m = static_cast(msg); + +#define SET(proto, getter) m->set_##proto(value.Get##getter()) + SET(yaw, Yaw); + SET(pitch, Yaw); + SET(area, Area); + SET(skew, Skew); + SET(fiducialid, FiducialId); + SET(poseambiguity, PoseAmbiguity); +#undef SET + + m->clear_minarearectcorners(); + for (const auto& t : value.GetMinAreaRectCorners()) { + auto* corner = m->add_minarearectcorners(); + corner->set_x(t.first); + corner->set_y(t.second); + } + + m->clear_detectedcorners(); + for (const std::pair& t : value.GetDetectedCorners()) { + auto* corner = m->add_detectedcorners(); + corner->set_x(t.first); + corner->set_y(t.second); + } + + wpi::PackProtobuf(m->mutable_bestcameratotarget(), + value.GetBestCameraToTarget()); + wpi::PackProtobuf(m->mutable_altcameratotarget(), + value.GetAlternateCameraToTarget()); + + // TODO -- multi-target +} diff --git a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h index 2f2af08a26..0a4cb98e8b 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -34,7 +35,6 @@ #include #include #include -#include #include #include #include @@ -175,7 +175,7 @@ class PhotonCamera { protected: std::shared_ptr mainTable; std::shared_ptr rootTable; - nt::RawSubscriber rawBytesEntry; + nt::ProtobufSubscriber pipelineResultsSubscriber; nt::IntegerPublisher inputSaveImgEntry; nt::IntegerSubscriber inputSaveImgSubscriber; nt::IntegerPublisher outputSaveImgEntry; diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h index 8370702b3b..9a76fce034 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h @@ -30,6 +30,7 @@ #include #include #include +#include #include "photonlib/MultiTargetPNPResult.h" #include "photonlib/Packet.h" @@ -131,3 +132,12 @@ class PhotonPipelineResult { inline static bool HAS_WARNED = false; }; } // namespace photonlib + +template <> +struct wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static photonlib::PhotonPipelineResult Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const photonlib::PhotonPipelineResult& value); +}; diff --git a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h index 2674d680a7..154cbb9924 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h @@ -97,7 +97,8 @@ class PhotonTrackedTarget { * down), in no particular order, of the minimum area bounding rectangle of * this target */ - wpi::SmallVector, 4> GetMinAreaRectCorners() const { + const wpi::SmallVector, 4>& GetMinAreaRectCorners() + const { return minAreaRectCorners; } @@ -112,7 +113,7 @@ class PhotonTrackedTarget { * V + Y | | * 0 ----- 1 */ - std::vector> GetDetectedCorners() { + const std::vector>& GetDetectedCorners() const { return detectedCorners; } @@ -159,3 +160,12 @@ class PhotonTrackedTarget { std::vector> detectedCorners; }; } // namespace photonlib + +template <> +struct wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static photonlib::PhotonTrackedTarget Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const photonlib::PhotonTrackedTarget& value); +}; diff --git a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h index 44ab0917a2..22d7c5c9a7 100644 --- a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h @@ -48,9 +48,8 @@ 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("rawBytes").Publish("rawBytes"); versionEntry = instance.GetTable("photonvision")->GetEntry("version"); - // versionEntry.SetString(PhotonVersion.versionString); } explicit SimPhotonCamera(const std::string& cameraName) @@ -89,8 +88,8 @@ class SimPhotonCamera : public PhotonCamera { Packet packet{}; packet << newResult; - rawBytesPublisher.Set( - std::span{packet.GetData().data(), packet.GetDataSize()}); + // rawBytesPublisher.Set( + // std::span{packet.GetData().data(), packet.GetDataSize()}); bool hasTargets = newResult.HasTargets(); hasTargetEntry.SetBoolean(hasTargets); @@ -127,6 +126,6 @@ class SimPhotonCamera : public PhotonCamera { nt::NetworkTableEntry targetSkewEntry; nt::NetworkTableEntry targetPoseEntry; nt::NetworkTableEntry versionEntry; - nt::RawPublisher rawBytesPublisher; + nt::ProtobufPublisher pipelineResultsPublisher; }; } // namespace photonlib diff --git a/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp b/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp deleted file mode 100644 index c1be0ca91b..0000000000 --- a/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp +++ /dev/null @@ -1,28 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "gtest/gtest.h" -#include "photonlib/PhotonUtils.h" - -TEST(PhotonUtilsTest, Include) {} From b4b71acaaa44dc33b8dcf32d9a84fdc669d2f444 Mon Sep 17 00:00:00 2001 From: Matt Date: Tue, 14 Nov 2023 20:14:01 -0500 Subject: [PATCH 41/74] Update PhotonCamera.cpp --- photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp index 32fa49b096..41bf88dbd0 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp @@ -40,9 +40,6 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, const std::string_view cameraName) : mainTable(instance.GetTable("photonvision")), rootTable(mainTable->GetSubTable(cameraName)), - // rawBytesEntry( - // rootTable->GetRawTopic("rawBytes") - // .Subscribe("rawBytes", {}, {.periodic = 0.01, .sendAll = true})), pipelineResultsSubscriber( rootTable->GetProtobufTopic("result_proto").Subscribe(PhotonPipelineResult(), {.periodic = 0.01, .sendAll = true}) ), From 82d25332f225ab7749e95ca9c1032b459f94bb1f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 15 Nov 2023 19:00:06 -0500 Subject: [PATCH 42/74] bump wpilib --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index 9669d4f0e1..46d9d09108 100644 --- a/build.gradle +++ b/build.gradle @@ -20,7 +20,7 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2024.1.1-beta-3-22-g0f81296" + wpilibVersion = "2024.1.1-beta-3-29-g3985c03" openCVversion = "4.8.0-2" joglVersion = "2.4.0-rc-20200307" javalinVersion = "5.6.2" From 465fe3bef39f7a6bf51a7cd8e94b87b211b3fa03 Mon Sep 17 00:00:00 2001 From: Matt Date: Wed, 15 Nov 2023 22:03:16 -0500 Subject: [PATCH 43/74] Move geometry3d --- photon-lib/build.gradle | 2 +- .../src/main/native/{cpp/photonlib => include}/geometry3d.pb.h | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename photon-lib/src/main/native/{cpp/photonlib => include}/geometry3d.pb.h (100%) diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 0c38acc425..3e875d4479 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -78,7 +78,7 @@ model { source { srcDirs "src/main/native/cpp" srcDirs project(":photon-targeting").tasks.named("generateNativePhotonProtos").get().outputs.files - include "**/*.cpp" + include "**/*.cpp", "**/*.cc" } exportedHeaders { srcDirs "src/main/native/include" diff --git a/photon-lib/src/main/native/cpp/photonlib/geometry3d.pb.h b/photon-lib/src/main/native/include/geometry3d.pb.h similarity index 100% rename from photon-lib/src/main/native/cpp/photonlib/geometry3d.pb.h rename to photon-lib/src/main/native/include/geometry3d.pb.h From aa8f0f5d02fbc3cab66cc6811ae54fd4c93d9ed7 Mon Sep 17 00:00:00 2001 From: Matt M Date: Wed, 15 Nov 2023 22:33:06 -0500 Subject: [PATCH 44/74] wot --- .../src/main/native/cpp/photonlib/PhotonCamera.cpp | 10 ++++++---- .../main/native/cpp/photonlib/PhotonTrackedTarget.cpp | 10 +++++++++- .../src/main/native/include/photonlib/PhotonCamera.h | 2 +- .../main/native/include/photonlib/SimPhotonCamera.h | 3 ++- 4 files changed, 18 insertions(+), 7 deletions(-) diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp index 41bf88dbd0..4d45b6d47e 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp @@ -41,8 +41,9 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, : mainTable(instance.GetTable("photonvision")), rootTable(mainTable->GetSubTable(cameraName)), pipelineResultsSubscriber( - rootTable->GetProtobufTopic("result_proto").Subscribe(PhotonPipelineResult(), {.periodic = 0.01, .sendAll = true}) - ), + rootTable->GetProtobufTopic("result_proto") + .Subscribe(PhotonPipelineResult(), + {.periodic = 0.01, .sendAll = true})), inputSaveImgEntry( rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()), inputSaveImgSubscriber( @@ -87,8 +88,9 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { // Create the new result; PhotonPipelineResult result = pipelineResultsSubscriber.Get(); - result.SetTimestamp(units::microsecond_t(pipelineResultsSubscriber.GetLastChange()) - - result.GetLatency()); + result.SetTimestamp( + units::microsecond_t(pipelineResultsSubscriber.GetLastChange()) - + result.GetLatency()); return result; } diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp index 68f70aa9a5..25b1ea2dfd 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp @@ -162,7 +162,15 @@ wpi::Protobuf::Unpack( auto m = static_cast(&msg); - return photonlib::PhotonTrackedTarget{}; + return photonlib::PhotonTrackedTarget( + m->yaw(), m->pitch(), m->area(), m->skew(), m->fiducialid(), + wpi::UnpackProtobuf(m->bestcameratotarget()), + wpi::UnpackProtobuf(m->altcameratotarget()), + m->poseambiguity(), + // corners + {}, + // detected corners + {}); } void wpi::Protobuf::Pack( diff --git a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h index 0a4cb98e8b..8acef300cf 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h @@ -27,7 +27,6 @@ #include #include -#include #include #include #include @@ -35,6 +34,7 @@ #include #include #include +#include #include #include #include diff --git a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h index 22d7c5c9a7..ff96bbd5c9 100644 --- a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h @@ -48,7 +48,8 @@ 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("rawBytes").Publish("rawBytes"); versionEntry = instance.GetTable("photonvision")->GetEntry("version"); } From fb192f58b61f78b7851beb36446f99513ead5c2d Mon Sep 17 00:00:00 2001 From: Matt M Date: Wed, 15 Nov 2023 22:45:58 -0500 Subject: [PATCH 45/74] Nuke packet --- .../cpp/photonlib/MultiTargetPNPResult.cpp | 113 --------------- .../native/cpp/photonlib/PhotonCamera.cpp | 4 - .../cpp/photonlib/PhotonPipelineResult.cpp | 30 ---- .../cpp/photonlib/PhotonTrackedTarget.cpp | 100 ++------------ .../include/photonlib/MultiTargetPNPResult.h | 8 -- .../main/native/include/photonlib/Packet.h | 130 ------------------ .../native/include/photonlib/PhotonCamera.h | 2 - .../include/photonlib/PhotonPipelineResult.h | 4 - .../include/photonlib/PhotonTrackedTarget.h | 5 - .../include/photonlib/SimPhotonCamera.h | 10 +- photon-lib/src/test/native/cpp/PacketTest.cpp | 108 --------------- 11 files changed, 16 insertions(+), 498 deletions(-) delete mode 100644 photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp delete mode 100644 photon-lib/src/main/native/include/photonlib/Packet.h delete mode 100644 photon-lib/src/test/native/cpp/PacketTest.cpp diff --git a/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp b/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp deleted file mode 100644 index 91d6ffd6c3..0000000000 --- a/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "photonlib/MultiTargetPNPResult.h" - -namespace photonlib { - -Packet& operator<<(Packet& packet, const MultiTargetPnpResult& target) { - packet << target.result; - - size_t i; - for (i = 0; i < target.fiducialIdsUsed.capacity(); i++) { - if (i < target.fiducialIdsUsed.size()) { - packet << static_cast(target.fiducialIdsUsed[i]); - } else { - packet << static_cast(-1); - } - } - - return packet; -} - -Packet& operator>>(Packet& packet, MultiTargetPnpResult& target) { - packet >> target.result; - - target.fiducialIdsUsed.clear(); - for (size_t i = 0; i < target.fiducialIdsUsed.capacity(); i++) { - int16_t id = 0; - packet >> id; - - if (id > -1) { - target.fiducialIdsUsed.push_back(id); - } - } - - return packet; -} - -// Encode a transform3d -Packet& operator<<(Packet& packet, const frc::Transform3d& transform) { - packet << transform.Translation().X().value() - << transform.Translation().Y().value() - << transform.Translation().Z().value() - << transform.Rotation().GetQuaternion().W() - << transform.Rotation().GetQuaternion().X() - << transform.Rotation().GetQuaternion().Y() - << transform.Rotation().GetQuaternion().Z(); - - return packet; -} - -// Decode a transform3d -Packet& operator>>(Packet& packet, frc::Transform3d& transform) { - frc::Transform3d ret; - - // We use these for best and alt transforms below - double x = 0; - double y = 0; - double z = 0; - double w = 0; - - // decode and unitify translation - packet >> x >> y >> z; - const auto translation = frc::Translation3d( - units::meter_t(x), units::meter_t(y), units::meter_t(z)); - - // decode and add units to rotation - packet >> w >> x >> y >> z; - const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z)); - - transform = frc::Transform3d(translation, rotation); - - return packet; -} - -Packet& operator<<(Packet& packet, PNPResults const& result) { - packet << result.isValid << result.best << result.alt - << result.bestReprojectionErr << result.altReprojectionErr - << result.ambiguity; - - return packet; -} - -Packet& operator>>(Packet& packet, PNPResults& result) { - packet >> result.isValid >> result.best >> result.alt >> - result.bestReprojectionErr >> result.altReprojectionErr >> - result.ambiguity; - - return packet; -} - -} // namespace photonlib diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp index 4d45b6d47e..e1eb754d66 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp @@ -29,7 +29,6 @@ #include #include "PhotonVersion.h" -#include "photonlib/Packet.h" namespace photonlib { @@ -82,9 +81,6 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { // Prints warning if not connected VerifyVersion(); - // Clear the current packet. - packet.Clear(); - // Create the new result; PhotonPipelineResult result = pipelineResultsSubscriber.Get(); diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp index 6efc660058..3fbe8fd52d 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp @@ -40,36 +40,6 @@ bool PhotonPipelineResult::operator!=(const PhotonPipelineResult& other) const { return !operator==(other); } -Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { - // Encode latency and number of targets. - packet << result.latency.value() * 1000 << result.m_pnpResults - << static_cast(result.targets.size()); - - // Encode the information of each target. - for (auto& target : result.targets) packet << target; - - // Return the packet - return packet; -} - -Packet& operator>>(Packet& packet, PhotonPipelineResult& result) { - // Decode latency, existence of targets, and number of targets. - double latencyMillis = 0; - int8_t targetCount = 0; - packet >> latencyMillis >> result.m_pnpResults >> targetCount; - result.latency = units::second_t(latencyMillis / 1000.0); - - result.targets.clear(); - - // Decode the information of each target. - for (int i = 0; i < targetCount; ++i) { - PhotonTrackedTarget target; - packet >> target; - result.targets.push_back(target); - } - return packet; -} - } // namespace photonlib google::protobuf::Message* wpi::Protobuf::New( diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp index 25b1ea2dfd..720b8965b0 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp @@ -63,89 +63,6 @@ bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const { return !operator==(other); } -Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) { - packet << target.yaw << target.pitch << target.area << target.skew - << target.fiducialId - << target.bestCameraToTarget.Translation().X().value() - << target.bestCameraToTarget.Translation().Y().value() - << target.bestCameraToTarget.Translation().Z().value() - << target.bestCameraToTarget.Rotation().GetQuaternion().W() - << target.bestCameraToTarget.Rotation().GetQuaternion().X() - << target.bestCameraToTarget.Rotation().GetQuaternion().Y() - << target.bestCameraToTarget.Rotation().GetQuaternion().Z() - << target.altCameraToTarget.Translation().X().value() - << target.altCameraToTarget.Translation().Y().value() - << target.altCameraToTarget.Translation().Z().value() - << target.altCameraToTarget.Rotation().GetQuaternion().W() - << target.altCameraToTarget.Rotation().GetQuaternion().X() - << target.altCameraToTarget.Rotation().GetQuaternion().Y() - << target.altCameraToTarget.Rotation().GetQuaternion().Z() - << target.poseAmbiguity; - - for (int i = 0; i < 4; i++) { - packet << target.minAreaRectCorners[i].first - << target.minAreaRectCorners[i].second; - } - - uint8_t num_corners = - std::min(target.detectedCorners.size(), MAX_CORNERS); - packet << num_corners; - for (size_t i = 0; i < target.detectedCorners.size(); i++) { - packet << target.detectedCorners[i].first - << target.detectedCorners[i].second; - } - - return packet; -} - -Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) { - packet >> target.yaw >> target.pitch >> target.area >> target.skew >> - target.fiducialId; - - // We use these for best and alt transforms below - double x = 0; - double y = 0; - double z = 0; - double w = 0; - - // First transform is the "best" pose - packet >> x >> y >> z; - const auto bestTranslation = frc::Translation3d( - units::meter_t(x), units::meter_t(y), units::meter_t(z)); - packet >> w >> x >> y >> z; - const auto bestRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z)); - target.bestCameraToTarget = frc::Transform3d(bestTranslation, bestRotation); - - // Second transform is the "alternate" pose - packet >> x >> y >> z; - const auto altTranslation = frc::Translation3d( - units::meter_t(x), units::meter_t(y), units::meter_t(z)); - packet >> w >> x >> y >> z; - const auto altRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z)); - target.altCameraToTarget = frc::Transform3d(altTranslation, altRotation); - - packet >> target.poseAmbiguity; - - target.minAreaRectCorners.clear(); - double first = 0; - double second = 0; - for (int i = 0; i < 4; i++) { - packet >> first >> second; - target.minAreaRectCorners.emplace_back(first, second); - } - - uint8_t numCorners = 0; - packet >> numCorners; - target.detectedCorners.clear(); - target.detectedCorners.reserve(numCorners); - for (size_t i = 0; i < numCorners; i++) { - packet >> first >> second; - target.detectedCorners.emplace_back(first, second); - } - - return packet; -} - } // namespace photonlib google::protobuf::Message* wpi::Protobuf::New( @@ -162,15 +79,22 @@ wpi::Protobuf::Unpack( auto m = static_cast(&msg); + wpi::SmallVector, 4> minAreaRectCorners; + for (const auto& t : m->minarearectcorners()) { + minAreaRectCorners.emplace_back(t.x(), t.y()); + } + + std::vector> detectedCorners; + detectedCorners.reserve(m->detectedcorners_size()); + for (const auto& t : m->detectedcorners()) { + detectedCorners.emplace_back(t.x(), t.y()); + } + return photonlib::PhotonTrackedTarget( m->yaw(), m->pitch(), m->area(), m->skew(), m->fiducialid(), wpi::UnpackProtobuf(m->bestcameratotarget()), wpi::UnpackProtobuf(m->altcameratotarget()), - m->poseambiguity(), - // corners - {}, - // detected corners - {}); + m->poseambiguity(), minAreaRectCorners, detectedCorners); } void wpi::Protobuf::Pack( diff --git a/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h b/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h index 96681a0307..74da796203 100644 --- a/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h +++ b/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h @@ -27,8 +27,6 @@ #include #include -#include "photonlib/Packet.h" - namespace photonlib { class PNPResults { @@ -44,18 +42,12 @@ class PNPResults { double altReprojectionErr; double ambiguity; - - friend Packet& operator<<(Packet& packet, const PNPResults& result); - friend Packet& operator>>(Packet& packet, PNPResults& result); }; class MultiTargetPnpResult { public: PNPResults result; wpi::SmallVector fiducialIdsUsed; - - friend Packet& operator<<(Packet& packet, const MultiTargetPnpResult& result); - friend Packet& operator>>(Packet& packet, MultiTargetPnpResult& result); }; } // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/Packet.h b/photon-lib/src/main/native/include/photonlib/Packet.h deleted file mode 100644 index 59ae40b462..0000000000 --- a/photon-lib/src/main/native/include/photonlib/Packet.h +++ /dev/null @@ -1,130 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#pragma once - -#include -#include -#include - -#include - -namespace photonlib { - -/** - * A packet that holds byte-packed data to be sent over NetworkTables. - */ -class Packet { - public: - /** - * Constructs an empty packet. - */ - Packet() = default; - - /** - * Constructs a packet with the given data. - * @param data The packet data. - */ - explicit Packet(std::vector data) : packetData(data) {} - - /** - * Clears the packet and resets the read and write positions. - */ - void Clear() { - packetData.clear(); - readPos = 0; - writePos = 0; - } - - /** - * Returns the packet data. - * @return The packet data. - */ - const std::vector& GetData() { return packetData; } - - /** - * Returns the number of bytes in the data. - * @return The number of bytes in the data. - */ - size_t GetDataSize() const { return packetData.size(); } - - /** - * Adds a value to the data buffer. This should only be used with PODs. - * @tparam T The data type. - * @param src The data source. - * @return A reference to the current object. - */ - template - Packet& operator<<(T src) { - packetData.resize(packetData.size() + sizeof(T)); - std::memcpy(packetData.data() + writePos, &src, sizeof(T)); - - if constexpr (wpi::support::endian::system_endianness() == - wpi::support::endianness::little) { - // Reverse to big endian for network conventions. - std::reverse(packetData.data() + writePos, - packetData.data() + writePos + sizeof(T)); - } - - writePos += sizeof(T); - return *this; - } - - /** - * Extracts a value to the provided destination. - * @tparam T The type of value to extract. - * @param value The value to extract. - * @return A reference to the current object. - */ - template - Packet& operator>>(T& value) { - if (!packetData.empty()) { - std::memcpy(&value, packetData.data() + readPos, sizeof(T)); - - if constexpr (wpi::support::endian::system_endianness() == - wpi::support::endianness::little) { - // Reverse to little endian for host. - uint8_t& raw = reinterpret_cast(value); - std::reverse(&raw, &raw + sizeof(T)); - } - } - - readPos += sizeof(T); - return *this; - } - - bool operator==(const Packet& right) const { - return packetData == right.packetData; - } - bool operator!=(const Packet& right) const { return !operator==(right); } - - private: - // Data stored in the packet - std::vector packetData; - - size_t readPos = 0; - size_t writePos = 0; -}; - -} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h index 8acef300cf..59027ed140 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h @@ -198,8 +198,6 @@ class PhotonCamera { std::string path; std::string m_cameraName; - mutable Packet packet; - private: units::second_t lastVersionCheckTime = 0_s; inline static bool VERSION_CHECK_ENABLED = true; diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h index 9a76fce034..08825eca8e 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h @@ -33,7 +33,6 @@ #include #include "photonlib/MultiTargetPNPResult.h" -#include "photonlib/Packet.h" #include "photonlib/PhotonTrackedTarget.h" namespace photonlib { @@ -121,9 +120,6 @@ class PhotonPipelineResult { bool operator==(const PhotonPipelineResult& other) const; bool operator!=(const PhotonPipelineResult& other) const; - friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result); - friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result); - private: units::second_t latency = 0_s; units::second_t timestamp = -1_s; diff --git a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h index 154cbb9924..933b0da2aa 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h @@ -32,8 +32,6 @@ #include #include -#include "photonlib/Packet.h" - namespace photonlib { /** * Represents a tracked target within a pipeline. @@ -144,9 +142,6 @@ class PhotonTrackedTarget { bool operator==(const PhotonTrackedTarget& other) const; bool operator!=(const PhotonTrackedTarget& other) const; - friend Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target); - friend Packet& operator>>(Packet& packet, PhotonTrackedTarget& target); - private: double yaw = 0; double pitch = 0; diff --git a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h index ff96bbd5c9..9972b06e67 100644 --- a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h @@ -48,8 +48,9 @@ class SimPhotonCamera : public PhotonCamera { targetAreaEntry = rootTable->GetEntry("targetAreaEntry"); targetSkewEntry = rootTable->GetEntry("targetSkewEntry"); targetPoseEntry = rootTable->GetEntry("targetPoseEntry"); - // rawBytesPublisher = - // rootTable->GetRawTopic("rawBytes").Publish("rawBytes"); + pipelineResultsPublisher = + rootTable->GetProtobufTopic("result_proto") + .Publish({.periodic = 0.01, .sendAll = true}); versionEntry = instance.GetTable("photonvision")->GetEntry("version"); } @@ -86,11 +87,8 @@ class SimPhotonCamera : public PhotonCamera { std::sort(targetList.begin(), targetList.end(), [&](auto lhs, auto rhs) { return sortMode(lhs, rhs); }); PhotonPipelineResult newResult{latency, targetList}; - Packet packet{}; - packet << newResult; - // rawBytesPublisher.Set( - // std::span{packet.GetData().data(), packet.GetDataSize()}); + pipelineResultsPublisher.Set(newResult); bool hasTargets = newResult.HasTargets(); hasTargetEntry.SetBoolean(hasTargets); diff --git a/photon-lib/src/test/native/cpp/PacketTest.cpp b/photon-lib/src/test/native/cpp/PacketTest.cpp deleted file mode 100644 index fe53cd85c1..0000000000 --- a/photon-lib/src/test/native/cpp/PacketTest.cpp +++ /dev/null @@ -1,108 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include - -#include - -#include "gtest/gtest.h" -#include "photonlib/PhotonPipelineResult.h" -#include "photonlib/PhotonTrackedTarget.h" - -TEST(PacketTest, PhotonTrackedTarget) { - photonlib::PhotonTrackedTarget target{ - 3.0, - 4.0, - 9.0, - -5.0, - -1, - 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)), - -1, - {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, - {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}; - - photonlib::Packet p; - p << target; - - photonlib::PhotonTrackedTarget b; - p >> b; - - for (auto& c : p.GetData()) { - std::cout << static_cast(c) << ","; - } - - EXPECT_EQ(target, b); -} - -TEST(PacketTest, PhotonPipelineResult) { - photonlib::PhotonPipelineResult result{1_s, {}}; - photonlib::Packet p; - p << result; - - photonlib::PhotonPipelineResult b; - p >> b; - - EXPECT_EQ(result, b); - - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ - 3.0, - -4.0, - 9.0, - 4.0, - 1, - 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)), - -1, - {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, - {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}, - photonlib::PhotonTrackedTarget{ - 3.0, - -4.0, - 9.1, - 6.7, - -1, - 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)), - -1, - {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, - {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, - std::pair{7, 8}}}}; - - photonlib::PhotonPipelineResult result2{2_s, targets}; - photonlib::Packet p2; - p2 << result2; - - photonlib::PhotonPipelineResult b2; - p2 >> b2; - - EXPECT_EQ(result2, b2); -} From 22632b29750e9feb7b29c8b9d6c2bf15e47edc83 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Thu, 16 Nov 2023 07:44:41 -0500 Subject: [PATCH 46/74] conform proto file to styleguide --- .../src/main/proto/photon_types.proto | 39 +++++++++++++------ 1 file changed, 28 insertions(+), 11 deletions(-) diff --git a/photon-targeting/src/main/proto/photon_types.proto b/photon-targeting/src/main/proto/photon_types.proto index 3ffa9ea810..9f3a56a2ac 100644 --- a/photon-targeting/src/main/proto/photon_types.proto +++ b/photon-targeting/src/main/proto/photon_types.proto @@ -1,27 +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 . + */ + syntax = "proto3"; package photonvision.proto; -option java_package = "org.photonvision.proto"; - import "geometry3d.proto"; +option java_package = "org.photonvision.proto"; + message ProtobufTargetCorner { double x = 1; double y = 2; } message ProtobufPNPResult { - bool isPresent = 1; + bool is_present = 1; wpi.proto.ProtobufTransform3d best = 2; - double bestReprojErr = 3; + double best_reproj_err = 3; optional wpi.proto.ProtobufTransform3d alt = 4; - optional double altReprojErr = 5; + optional double alt_reproj_err = 5; double ambiguity = 6; } message ProtobufMultiTargetPNPResult { - ProtobufPNPResult estimatedPose = 1; + ProtobufPNPResult estimated_pose = 1; repeated int32 fiducial_ids_used = 2; } @@ -30,12 +47,12 @@ message ProtobufPhotonTrackedTarget { double pitch = 2; double area = 3; double skew = 4; - int32 fiducialId = 5; - wpi.proto.ProtobufTransform3d bestCameraToTarget = 6; - wpi.proto.ProtobufTransform3d altCameraToTarget = 7; + int32 fiducial_id = 5; + wpi.proto.ProtobufTransform3d best_camera_to_target = 6; + wpi.proto.ProtobufTransform3d alt_camera_to_target = 7; double poseAmbiguity = 8; - repeated ProtobufTargetCorner minAreaRectCorners = 9; - repeated ProtobufTargetCorner detectedCorners = 10; + repeated ProtobufTargetCorner min_area_rect_corners = 9; + repeated ProtobufTargetCorner detected_corners = 10; } message ProtobufPhotonPipelineResult { From 932ee16721dd2faadf3f9f6143be707561ecbe36 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Thu, 16 Nov 2023 09:22:55 -0500 Subject: [PATCH 47/74] move sim stuff --- .../native/include/photonlib/{ => simulation}/SimPhotonCamera.h | 0 .../native/include/photonlib/{ => simulation}/SimVisionSystem.h | 0 .../native/include/photonlib/{ => simulation}/SimVisionTarget.h | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename photon-lib/src/main/native/include/photonlib/{ => simulation}/SimPhotonCamera.h (100%) rename photon-lib/src/main/native/include/photonlib/{ => simulation}/SimVisionSystem.h (100%) rename photon-lib/src/main/native/include/photonlib/{ => simulation}/SimVisionTarget.h (100%) diff --git a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h b/photon-lib/src/main/native/include/photonlib/simulation/SimPhotonCamera.h similarity index 100% rename from photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h rename to photon-lib/src/main/native/include/photonlib/simulation/SimPhotonCamera.h diff --git a/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h b/photon-lib/src/main/native/include/photonlib/simulation/SimVisionSystem.h similarity index 100% rename from photon-lib/src/main/native/include/photonlib/SimVisionSystem.h rename to photon-lib/src/main/native/include/photonlib/simulation/SimVisionSystem.h diff --git a/photon-lib/src/main/native/include/photonlib/SimVisionTarget.h b/photon-lib/src/main/native/include/photonlib/simulation/SimVisionTarget.h similarity index 100% rename from photon-lib/src/main/native/include/photonlib/SimVisionTarget.h rename to photon-lib/src/main/native/include/photonlib/simulation/SimVisionTarget.h From f48ea8a9c345dd38b4de77eacd9eb36795657fa6 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Fri, 17 Nov 2023 00:23:33 -0500 Subject: [PATCH 48/74] photon_types -> photon --- .../org/photonvision/targeting/MultiTargetPNPResult.java | 2 +- .../src/main/java/org/photonvision/targeting/PNPResult.java | 2 +- .../org/photonvision/targeting/PhotonPipelineResult.java | 2 +- .../org/photonvision/targeting/PhotonTrackedTarget.java | 2 +- .../main/java/org/photonvision/targeting/TargetCorner.java | 2 +- .../src/main/proto/{photon_types.proto => photon.proto} | 0 .../org/photonvision/targeting/PhotonTrackedTargetTest.java | 6 +++--- .../java/org/photonvision/targeting/TargetCornerTest.java | 6 +++--- 8 files changed, 11 insertions(+), 11 deletions(-) rename photon-targeting/src/main/proto/{photon_types.proto => photon.proto} (100%) diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java index 1e935a6bfc..144a514fd4 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java @@ -21,7 +21,7 @@ import java.util.Arrays; import java.util.List; import java.util.stream.Collectors; -import org.photonvision.proto.PhotonTypes.ProtobufMultiTargetPNPResult; +import org.photonvision.proto.Photon.ProtobufMultiTargetPNPResult; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.RepeatedInt; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java index 0a63ccb865..09be52941f 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java @@ -19,7 +19,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.util.protobuf.Protobuf; -import org.photonvision.proto.PhotonTypes.ProtobufPNPResult; +import org.photonvision.proto.Photon.ProtobufPNPResult; import us.hebi.quickbuf.Descriptors.Descriptor; /** diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 05cadd632d..6759237a73 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -20,7 +20,7 @@ import edu.wpi.first.util.protobuf.Protobuf; import java.util.ArrayList; import java.util.List; -import org.photonvision.proto.PhotonTypes.ProtobufPhotonPipelineResult; +import org.photonvision.proto.Photon.ProtobufPhotonPipelineResult; import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a pipeline result from a PhotonCamera. */ diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 71c0f6665c..c9bd1d792f 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -21,7 +21,7 @@ import edu.wpi.first.util.protobuf.Protobuf; import java.util.ArrayList; import java.util.List; -import org.photonvision.proto.PhotonTypes.ProtobufPhotonTrackedTarget; +import org.photonvision.proto.Photon.ProtobufPhotonTrackedTarget; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.RepeatedMessage; diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index 773031b84a..ccfa41713a 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -21,7 +21,7 @@ import java.util.ArrayList; import java.util.List; import java.util.Objects; -import org.photonvision.proto.PhotonTypes.ProtobufTargetCorner; +import org.photonvision.proto.Photon.ProtobufTargetCorner; import us.hebi.quickbuf.Descriptors.Descriptor; import us.hebi.quickbuf.RepeatedMessage; diff --git a/photon-targeting/src/main/proto/photon_types.proto b/photon-targeting/src/main/proto/photon.proto similarity index 100% rename from photon-targeting/src/main/proto/photon_types.proto rename to photon-targeting/src/main/proto/photon.proto diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java index 0742c2361e..73ea70e581 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java @@ -25,7 +25,7 @@ import edu.wpi.first.math.geometry.Translation3d; import java.util.List; import org.junit.jupiter.api.Test; -import org.photonvision.proto.PhotonTypes; +import org.photonvision.proto.Photon.ProtobufPhotonTrackedTarget; import us.hebi.quickbuf.RepeatedMessage; public class PhotonTrackedTargetTest { @@ -61,7 +61,7 @@ public void protobufTest() { public void protobufListTest() { List targets = List.of(); var serializedTargets = - RepeatedMessage.newEmptyInstance(PhotonTypes.ProtobufPhotonTrackedTarget.getFactory()); + RepeatedMessage.newEmptyInstance(ProtobufPhotonTrackedTarget.getFactory()); PhotonTrackedTarget.proto.pack(serializedTargets, targets); var unpackedTargets = PhotonTrackedTarget.proto.unpack(serializedTargets); assertEquals(targets, unpackedTargets); @@ -107,7 +107,7 @@ public void protobufListTest() { new TargetCorner(5, 6), new TargetCorner(7, 8)))); serializedTargets = - RepeatedMessage.newEmptyInstance(PhotonTypes.ProtobufPhotonTrackedTarget.getFactory()); + RepeatedMessage.newEmptyInstance(ProtobufPhotonTrackedTarget.getFactory()); PhotonTrackedTarget.proto.pack(serializedTargets, targets); unpackedTargets = PhotonTrackedTarget.proto.unpack(serializedTargets); assertEquals(targets, unpackedTargets); diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java index 4de9e3a741..c1c78694f7 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java @@ -22,7 +22,7 @@ import java.util.List; import org.junit.jupiter.api.Test; -import org.photonvision.proto.PhotonTypes; +import org.photonvision.proto.Photon.ProtobufTargetCorner; import us.hebi.quickbuf.RepeatedMessage; public class TargetCornerTest { @@ -39,14 +39,14 @@ public void protobufTest() { public void protobufListTest() { List corners = List.of(); var serializedCorners = - RepeatedMessage.newEmptyInstance(PhotonTypes.ProtobufTargetCorner.getFactory()); + RepeatedMessage.newEmptyInstance(ProtobufTargetCorner.getFactory()); TargetCorner.proto.pack(serializedCorners, corners); var unpackedCorners = TargetCorner.proto.unpack(serializedCorners); assertEquals(corners, unpackedCorners); corners = List.of(new TargetCorner(0, 1), new TargetCorner(1, 2)); serializedCorners = - RepeatedMessage.newEmptyInstance(PhotonTypes.ProtobufTargetCorner.getFactory()); + RepeatedMessage.newEmptyInstance(ProtobufTargetCorner.getFactory()); TargetCorner.proto.pack(serializedCorners, corners); unpackedCorners = TargetCorner.proto.unpack(serializedCorners); assertEquals(corners, unpackedCorners); From e7be5d0f353a0413f80904221bae8d2a6b89f1f3 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 19 Nov 2023 16:37:18 -0500 Subject: [PATCH 49/74] stuff i missed --- photon-targeting/build.gradle | 2 +- .../cpp/photon/targeting/PhotonTrackedTarget.cpp | 2 -- .../include/photon/targeting/MultiTargetPNPResult.h | 5 +++-- .../main/native/include/photon/targeting/PNPResult.h | 2 +- .../include/photon/targeting/PhotonPipelineResult.h | 10 +++++----- .../include/photon/targeting/PhotonTrackedTarget.h | 9 ++++----- 6 files changed, 14 insertions(+), 16 deletions(-) diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index f1ac55e75b..6f91d82dc6 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -47,4 +47,4 @@ dependencies { implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); -} \ No newline at end of file +} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp index a852f42fea..bff421cfce 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp @@ -23,8 +23,6 @@ #include #include -#include "photon_types.pb.h" - static constexpr const uint8_t MAX_CORNERS = 8; namespace photon { diff --git a/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h index 011c4093c6..442724c00e 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h @@ -36,7 +36,8 @@ class MultiTargetPNPResult { template <> struct wpi::Protobuf { static google::protobuf::Message* New(google::protobuf::Arena* arena); - static photon::MultiTargetPNPResult Unpack(const google::protobuf::Message& msg); + static photon::MultiTargetPNPResult Unpack( + const google::protobuf::Message& msg); static void Pack(google::protobuf::Message* msg, - const photon::MultiTargetPNPResult& value); + const photon::MultiTargetPNPResult& value); }; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h index 8f2174fefa..d7f3c7af1c 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h @@ -45,5 +45,5 @@ struct wpi::Protobuf { static google::protobuf::Message* New(google::protobuf::Arena* arena); static photon::PNPResult Unpack(const google::protobuf::Message& msg); static void Pack(google::protobuf::Message* msg, - const photon::PNPResult& value); + const photon::PNPResult& value); }; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h index 8c366b8d0e..68753b27a5 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -122,19 +122,19 @@ class PhotonPipelineResult { bool operator==(const PhotonPipelineResult& other) const; - private: units::second_t latency = 0_s; units::second_t timestamp = -1_s; wpi::SmallVector targets; MultiTargetPNPResult multitagResult; inline static bool HAS_WARNED = false; }; -} // namespace photonlib +} // namespace photon template <> struct wpi::Protobuf { static google::protobuf::Message* New(google::protobuf::Arena* arena); - static photon::PhotonPipelineResult Unpack(const google::protobuf::Message& msg); + static photon::PhotonPipelineResult Unpack( + const google::protobuf::Message& msg); static void Pack(google::protobuf::Message* msg, - const photon::PhotonPipelineResult& value); -}; \ No newline at end of file + const photon::PhotonPipelineResult& value); +}; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h index 00658166fd..8c42b18f1f 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h @@ -26,7 +26,6 @@ #include #include - namespace photon { /** * Represents a tracked target within a pipeline. @@ -137,7 +136,6 @@ class PhotonTrackedTarget { bool operator==(const PhotonTrackedTarget& other) const; - private: double yaw = 0; double pitch = 0; double area = 0; @@ -149,12 +147,13 @@ class PhotonTrackedTarget { wpi::SmallVector, 4> minAreaRectCorners; std::vector> detectedCorners; }; -} // namespace photonlib +} // namespace photon template <> struct wpi::Protobuf { static google::protobuf::Message* New(google::protobuf::Arena* arena); - static photon::PhotonTrackedTarget Unpack(const google::protobuf::Message& msg); + static photon::PhotonTrackedTarget Unpack( + const google::protobuf::Message& msg); static void Pack(google::protobuf::Message* msg, - const photon::PhotonTrackedTarget& value); + const photon::PhotonTrackedTarget& value); }; From 15a0eb678c2e77b2f86c2e964bda3a3d63d2e75b Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 19 Nov 2023 19:06:47 -0500 Subject: [PATCH 50/74] move ctors to headers --- .../photon/targeting/PhotonPipelineResult.cpp | 13 +------ .../photon/targeting/PhotonTrackedTarget.cpp | 18 --------- .../photon/targeting/PhotonPipelineResult.h | 25 +++++++----- .../photon/targeting/PhotonTrackedTarget.h | 38 ++++++++++++------- 4 files changed, 40 insertions(+), 54 deletions(-) diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp index 1950e0f80d..7664fcf1d6 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp @@ -17,19 +17,8 @@ #include "photon/targeting/PhotonPipelineResult.h" -namespace photon { -PhotonPipelineResult::PhotonPipelineResult( - units::second_t latency, std::span targets) - : latency(latency), - targets(targets.data(), targets.data() + targets.size()) {} - -PhotonPipelineResult::PhotonPipelineResult( - units::second_t latency, std::span targets, - MultiTargetPNPResult multitagResult) - : latency(latency), - targets(targets.data(), targets.data() + targets.size()), - multitagResult(multitagResult) {} +namespace photon { bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const { return latency == other.latency && targets == other.targets && multitagResult == other.multitagResult; diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp index bff421cfce..8d5a089d8c 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp @@ -26,24 +26,6 @@ static constexpr const uint8_t MAX_CORNERS = 8; namespace photon { - -PhotonTrackedTarget::PhotonTrackedTarget( - double yaw, double pitch, double area, double skew, int id, - const frc::Transform3d& pose, const frc::Transform3d& alternatePose, - double ambiguity, - const wpi::SmallVector, 4> minAreaRectCorners, - const std::vector> detectedCorners) - : yaw(yaw), - pitch(pitch), - area(area), - skew(skew), - fiducialId(id), - bestCameraToTarget(pose), - altCameraToTarget(alternatePose), - poseAmbiguity(ambiguity), - minAreaRectCorners(minAreaRectCorners), - detectedCorners(detectedCorners) {} - bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const { return other.yaw == yaw && other.pitch == pitch && other.area == area && other.skew == skew && other.bestCameraToTarget == bestCameraToTarget && diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h index 68753b27a5..8c1abeb965 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -34,6 +34,12 @@ namespace photon { */ class PhotonPipelineResult { public: + units::millisecond_t latency = 0_s; + units::second_t timestamp = -1_s; + wpi::SmallVector targets; + MultiTargetPNPResult multitagResult; + inline static bool HAS_WARNED = false; + /** * Constructs an empty pipeline result */ @@ -44,8 +50,10 @@ class PhotonPipelineResult { * @param latency The latency in the pipeline. * @param targets The list of targets identified by the pipeline. */ - PhotonPipelineResult(units::second_t latency, - std::span targets); + PhotonPipelineResult(units::millisecond_t latency, + std::span targets) + : latency(latency), + targets(targets.data(), targets.data() + targets.size()) {} /** * Constructs a pipeline result. @@ -53,9 +61,12 @@ class PhotonPipelineResult { * @param targets The list of targets identified by the pipeline. * @param multitagResult The multitarget result */ - PhotonPipelineResult(units::second_t latency, + PhotonPipelineResult(units::millisecond_t latency, std::span targets, - MultiTargetPNPResult multitagResult); + MultiTargetPNPResult multitagResult) + : latency(latency), + targets(targets.data(), targets.data() + targets.size()), + multitagResult(multitagResult) {} /** * Returns the best target in this pipeline result. If there are no targets, @@ -121,12 +132,6 @@ class PhotonPipelineResult { } bool operator==(const PhotonPipelineResult& other) const; - - units::second_t latency = 0_s; - units::second_t timestamp = -1_s; - wpi::SmallVector targets; - MultiTargetPNPResult multitagResult; - inline static bool HAS_WARNED = false; }; } // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h index 8c42b18f1f..8233347b7a 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h @@ -32,6 +32,17 @@ namespace photon { */ class PhotonTrackedTarget { public: + double yaw = 0; + double pitch = 0; + double area = 0; + double skew = 0; + int fiducialId; + frc::Transform3d bestCameraToTarget; + frc::Transform3d altCameraToTarget; + double poseAmbiguity; + wpi::SmallVector, 4> minAreaRectCorners; + std::vector> detectedCorners; + /** * Constructs an empty target. */ @@ -49,11 +60,21 @@ class PhotonTrackedTarget { * @param detectedCorners All detected corners */ PhotonTrackedTarget( - double yaw, double pitch, double area, double skew, int fiducialID, - const frc::Transform3d& pose, const frc::Transform3d& alternatePose, + double yaw, double pitch, double area, double skew, int fiducialId, + const frc::Transform3d pose, const frc::Transform3d alternatePose, double ambiguity, const wpi::SmallVector, 4> minAreaRectCorners, - const std::vector> detectedCorners); + const std::vector> detectedCorners) + : yaw(yaw), + pitch(pitch), + area(area), + skew(skew), + fiducialId(fiducialId), + bestCameraToTarget(pose), + altCameraToTarget(alternatePose), + poseAmbiguity(ambiguity), + minAreaRectCorners(minAreaRectCorners), + detectedCorners(detectedCorners) {} /** * Returns the target yaw (positive-left). @@ -135,17 +156,6 @@ class PhotonTrackedTarget { } bool operator==(const PhotonTrackedTarget& other) const; - - double yaw = 0; - double pitch = 0; - double area = 0; - double skew = 0; - int fiducialId; - frc::Transform3d bestCameraToTarget; - frc::Transform3d altCameraToTarget; - double poseAmbiguity; - wpi::SmallVector, 4> minAreaRectCorners; - std::vector> detectedCorners; }; } // namespace photon From 6ec60a7eae4d8ad55f6f2db3b709e4a701f68a9f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 19 Nov 2023 19:11:51 -0500 Subject: [PATCH 51/74] implement Protobuf into photon-targetting c++ --- photon-targeting/build.gradle | 62 +++++++++++++++++ .../photon/targeting/MultiTargetPNPResult.cpp | 37 +++++++++++ .../native/cpp/photon/targeting/PNPResult.cpp | 33 ++++++++++ .../photon/targeting/PhotonPipelineResult.cpp | 40 +++++++++++ .../photon/targeting/PhotonTrackedTarget.cpp | 66 +++++++++++++++++++ .../src/main/native/include/geometry3d.pb.h | 22 +++++++ .../photon/targeting/MultiTargetPNPResult.h | 6 ++ .../include/photon/targeting/PNPResult.h | 12 ++++ photon-targeting/src/main/proto/photon.proto | 2 +- 9 files changed, 279 insertions(+), 1 deletion(-) create mode 100644 photon-targeting/src/main/native/include/geometry3d.pb.h diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 6f91d82dc6..71f6798a48 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -1,5 +1,7 @@ plugins { + id 'java' id 'edu.wpi.first.WpilibTools' version '1.3.0' + id 'com.google.protobuf' version '0.9.3' } ext { @@ -48,3 +50,63 @@ dependencies { implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); } + +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" + } + } + } + } +} + +model { + components { + all { + it.sources.each { + it.source { + srcDirs 'src/main/native/cpp' + srcDirs 'build/generated/source/proto/main/cpp' + include '**/*.cpp', '**/*.cc' + } + + it.exportedHeaders { + srcDirs 'src/main/native/include' + srcDirs 'build/generated/source/proto/main/cpp' + include "**/*.h" + } + } + it.binaries.all { + if (it instanceof SharedLibraryBinarySpec) { + it.buildable = false + return + } + it.tasks.withType(CppCompile) { + it.dependsOn generateProto + } + } + } + } + binaries { + withType(GoogleTestTestSuiteBinarySpec) { + it.tasks.withType(CppCompile) { + it.dependsOn generateProto + } + } + } +} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp index 2845789611..dee7cc2b9d 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp @@ -17,9 +17,46 @@ #include "photon/targeting/MultiTargetPNPResult.h" +#include "photon.pb.h" + namespace photon { bool MultiTargetPNPResult::operator==(const MultiTargetPNPResult& other) const { return other.result == result && other.fiducialIdsUsed == fiducialIdsUsed; } } // namespace photon + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufMultiTargetPNPResult>(arena); +} + +photon::MultiTargetPNPResult +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast( + &msg); + + wpi::SmallVector fiducialIdsUsed; + for (int i = 0; i < m->fiducial_ids_used_size(); i++) { + fiducialIdsUsed.push_back(m->fiducial_ids_used(i)); + } + + return photon::MultiTargetPNPResult{ + wpi::UnpackProtobuf(m->estimated_pose()), + fiducialIdsUsed}; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const photon::MultiTargetPNPResult& value) { + auto m = static_cast(msg); + + wpi::PackProtobuf(m->mutable_estimated_pose(), value.result); + + m->clear_fiducial_ids_used(); + for (const auto& t : value.fiducialIdsUsed) { + m->add_fiducial_ids_used(t); + } +} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp index 5b85ea8581..15887ab35e 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp @@ -17,6 +17,8 @@ #include "photon/targeting/PNPResult.h" +#include "photon.pb.h" + namespace photon { bool PNPResult::operator==(const PNPResult& other) const { return other.isPresent == isPresent && other.best == best && @@ -24,3 +26,34 @@ bool PNPResult::operator==(const PNPResult& other) const { other.altReprojErr == altReprojErr && other.ambiguity == ambiguity; } } // namespace photon + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufPNPResult>(arena); +} + +photon::PNPResult wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + + if (!m->is_present()) { + return photon::PNPResult(); + } + + return photon::PNPResult{wpi::UnpackProtobuf(m->best()), + m->best_reproj_err(), + wpi::UnpackProtobuf(m->alt()), + m->alt_reproj_err(), m->ambiguity()}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const photon::PNPResult& value) { + auto m = static_cast(msg); + + wpi::PackProtobuf(m->mutable_best(), value.best); + m->set_best_reproj_err(value.bestReprojErr); + wpi::PackProtobuf(m->mutable_alt(), value.alt); + m->set_alt_reproj_err(value.altReprojErr); + m->set_ambiguity(value.ambiguity); +} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp index 7664fcf1d6..64378dd3e7 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp @@ -17,6 +17,7 @@ #include "photon/targeting/PhotonPipelineResult.h" +#include "photon.pb.h" namespace photon { bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const { @@ -24,3 +25,42 @@ bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const { multitagResult == other.multitagResult; } } // namespace photon + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufPhotonPipelineResult>(arena); +} + +photon::PhotonPipelineResult +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast( + &msg); + + std::vector targets; + targets.reserve(m->targets_size()); + for (const auto& t : m->targets()) { + targets.emplace_back(wpi::UnpackProtobuf(t)); + } + + return photon::PhotonPipelineResult{ + units::millisecond_t{m->latency_ms()}, targets, + wpi::UnpackProtobuf( + m->multi_target_result())}; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const photon::PhotonPipelineResult& value) { + auto m = static_cast(msg); + + m->set_latency_ms(value.latency.value()); + + m->clear_targets(); + for (const auto& t : value.GetTargets()) { + wpi::PackProtobuf(m->add_targets(), t); + } + + wpi::PackProtobuf(m->mutable_multi_target_result(), value.multitagResult); +} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp index 8d5a089d8c..7b4909ca9c 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp @@ -23,6 +23,8 @@ #include #include +#include "photon.pb.h" + static constexpr const uint8_t MAX_CORNERS = 8; namespace photon { @@ -32,3 +34,67 @@ bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const { other.minAreaRectCorners == minAreaRectCorners; } } // namespace photon + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufPhotonTrackedTarget>(arena); +} + +photon::PhotonTrackedTarget wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast( + &msg); + + wpi::SmallVector, 4> minAreaRectCorners; + for (const auto& t : m->min_area_rect_corners()) { + minAreaRectCorners.emplace_back(t.x(), t.y()); + } + + std::vector> detectedCorners; + detectedCorners.reserve(m->detected_corners_size()); + for (const auto& t : m->detected_corners()) { + detectedCorners.emplace_back(t.x(), t.y()); + } + + return photon::PhotonTrackedTarget{ + m->yaw(), + m->pitch(), + m->area(), + m->skew(), + m->fiducial_id(), + wpi::UnpackProtobuf(m->best_camera_to_target()), + wpi::UnpackProtobuf(m->alt_camera_to_target()), + m->pose_ambiguity(), + minAreaRectCorners, + detectedCorners}; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const photon::PhotonTrackedTarget& value) { + auto m = static_cast(msg); + + m->set_yaw(value.yaw); + m->set_pitch(value.pitch); + m->set_area(value.area); + m->set_skew(value.skew); + m->set_fiducial_id(value.fiducialId); + wpi::PackProtobuf(m->mutable_best_camera_to_target(), + value.bestCameraToTarget); + wpi::PackProtobuf(m->mutable_alt_camera_to_target(), value.altCameraToTarget); + m->set_pose_ambiguity(value.poseAmbiguity); + + m->clear_min_area_rect_corners(); + for (const auto& t : value.GetMinAreaRectCorners()) { + auto* corner = m->add_min_area_rect_corners(); + corner->set_x(t.first); + corner->set_y(t.second); + } + + m->clear_detected_corners(); + for (const auto& t : value.GetDetectedCorners()) { + auto* corner = m->add_detected_corners(); + corner->set_x(t.first); + corner->set_y(t.second); + } +} diff --git a/photon-targeting/src/main/native/include/geometry3d.pb.h b/photon-targeting/src/main/native/include/geometry3d.pb.h new file mode 100644 index 0000000000..3cff26d7e0 --- /dev/null +++ b/photon-targeting/src/main/native/include/geometry3d.pb.h @@ -0,0 +1,22 @@ +/* + * 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 . + */ + +#pragma once +// So wpilib publishes protbufs here at wpimath/protobuf. but generated code +// assumes that the protobuf includes are on your include path. So we need this +// stupid shim +#include "wpimath/protobuf/geometry3d.pb.h" diff --git a/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h index 442724c00e..cf5f58741a 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h @@ -29,6 +29,12 @@ class MultiTargetPNPResult { PNPResult result; wpi::SmallVector fiducialIdsUsed; + MultiTargetPNPResult() = default; + + MultiTargetPNPResult(PNPResult result, + wpi::SmallVector fiducialIdsUsed) + : result(result), fiducialIdsUsed(fiducialIdsUsed) {} + bool operator==(const MultiTargetPNPResult& other) const; }; } // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h index d7f3c7af1c..156be1b3b8 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h @@ -36,6 +36,18 @@ class PNPResult { double ambiguity; + PNPResult() = default; + + PNPResult(frc::Transform3d best, double bestReprojErr, frc::Transform3d alt, + double altReprojErr, double ambiguity) + : best(best), + bestReprojErr(bestReprojErr), + alt(alt), + altReprojErr(altReprojErr), + ambiguity(ambiguity) { + this->isPresent = true; + } + bool operator==(const PNPResult& other) const; }; } // namespace photon diff --git a/photon-targeting/src/main/proto/photon.proto b/photon-targeting/src/main/proto/photon.proto index 9f3a56a2ac..d5b75802fc 100644 --- a/photon-targeting/src/main/proto/photon.proto +++ b/photon-targeting/src/main/proto/photon.proto @@ -50,7 +50,7 @@ message ProtobufPhotonTrackedTarget { int32 fiducial_id = 5; wpi.proto.ProtobufTransform3d best_camera_to_target = 6; wpi.proto.ProtobufTransform3d alt_camera_to_target = 7; - double poseAmbiguity = 8; + double pose_ambiguity = 8; repeated ProtobufTargetCorner min_area_rect_corners = 9; repeated ProtobufTargetCorner detected_corners = 10; } From ce3966c92f201858638d3253a7d7c8690f14ad76 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 19 Nov 2023 19:21:29 -0500 Subject: [PATCH 52/74] Formatting fixes --- .../org/photonvision/targeting/PhotonTrackedTargetTest.java | 3 +-- .../java/org/photonvision/targeting/TargetCornerTest.java | 6 ++---- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java index 73ea70e581..c458bc4c17 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java @@ -106,8 +106,7 @@ public void protobufListTest() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8)))); - serializedTargets = - RepeatedMessage.newEmptyInstance(ProtobufPhotonTrackedTarget.getFactory()); + serializedTargets = RepeatedMessage.newEmptyInstance(ProtobufPhotonTrackedTarget.getFactory()); PhotonTrackedTarget.proto.pack(serializedTargets, targets); unpackedTargets = PhotonTrackedTarget.proto.unpack(serializedTargets); assertEquals(targets, unpackedTargets); diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java index c1c78694f7..53beaed614 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java @@ -38,15 +38,13 @@ public void protobufTest() { @Test public void protobufListTest() { List corners = List.of(); - var serializedCorners = - RepeatedMessage.newEmptyInstance(ProtobufTargetCorner.getFactory()); + var serializedCorners = RepeatedMessage.newEmptyInstance(ProtobufTargetCorner.getFactory()); TargetCorner.proto.pack(serializedCorners, corners); var unpackedCorners = TargetCorner.proto.unpack(serializedCorners); assertEquals(corners, unpackedCorners); corners = List.of(new TargetCorner(0, 1), new TargetCorner(1, 2)); - serializedCorners = - RepeatedMessage.newEmptyInstance(ProtobufTargetCorner.getFactory()); + serializedCorners = RepeatedMessage.newEmptyInstance(ProtobufTargetCorner.getFactory()); TargetCorner.proto.pack(serializedCorners, corners); unpackedCorners = TargetCorner.proto.unpack(serializedCorners); assertEquals(corners, unpackedCorners); From 69bafc1554b155fd4330c2d14437f87ff7c37521 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 19 Nov 2023 20:48:26 -0500 Subject: [PATCH 53/74] update build for dependencies --- build.gradle | 1 + photon-targeting/build.gradle | 61 ----------------------------------- shared/javacommon.gradle | 25 ++++++++++++++ shared/setupBuild.gradle | 35 +++++++++++++------- 4 files changed, 50 insertions(+), 72 deletions(-) diff --git a/build.gradle b/build.gradle index 46d9d09108..e7a3ef7ba0 100644 --- a/build.gradle +++ b/build.gradle @@ -4,6 +4,7 @@ plugins { id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2" id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" id 'edu.wpi.first.WpilibTools' version '1.3.0' + id 'com.google.protobuf' version '0.9.3' apply false } allprojects { diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 71f6798a48..47eff41568 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -1,7 +1,6 @@ plugins { id 'java' id 'edu.wpi.first.WpilibTools' version '1.3.0' - id 'com.google.protobuf' version '0.9.3' } ext { @@ -50,63 +49,3 @@ dependencies { implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get() implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); } - -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" - } - } - } - } -} - -model { - components { - all { - it.sources.each { - it.source { - srcDirs 'src/main/native/cpp' - srcDirs 'build/generated/source/proto/main/cpp' - include '**/*.cpp', '**/*.cc' - } - - it.exportedHeaders { - srcDirs 'src/main/native/include' - srcDirs 'build/generated/source/proto/main/cpp' - include "**/*.h" - } - } - it.binaries.all { - if (it instanceof SharedLibraryBinarySpec) { - it.buildable = false - return - } - it.tasks.withType(CppCompile) { - it.dependsOn generateProto - } - } - } - } - binaries { - withType(GoogleTestTestSuiteBinarySpec) { - it.tasks.withType(CppCompile) { - it.dependsOn generateProto - } - } - } -} diff --git a/shared/javacommon.gradle b/shared/javacommon.gradle index 6792f61c73..7acffeb447 100644 --- a/shared/javacommon.gradle +++ b/shared/javacommon.gradle @@ -1,6 +1,7 @@ apply plugin: 'maven-publish' apply plugin: 'java-library' apply plugin: 'jacoco' +apply plugin: 'com.google.protobuf' java { sourceCompatibility = JavaVersion.VERSION_11 @@ -113,3 +114,27 @@ jacocoTestReport { html.required = true } } + +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" + } + } + } + } +} diff --git a/shared/setupBuild.gradle b/shared/setupBuild.gradle index b90f87e154..51926d32ee 100644 --- a/shared/setupBuild.gradle +++ b/shared/setupBuild.gradle @@ -18,18 +18,26 @@ model { sources { cpp { source { + srcDirs 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp" srcDirs 'src/main/native/cpp' - include '**/*.cpp' + include '**/*.cpp', '**/*.cc' } exportedHeaders { + srcDirs 'src/main/native/include', "$buildDir/generated/source/proto/main/cpp" srcDirs 'src/main/native/include' - include "**/*.h" + include '**/*.h' } } } - - if(project.hasProperty('includePhotonTargeting')) { - binaries.all { + binaries.all { + if (it instanceof SharedLibraryBinarySpec) { + it.buildable = false + return + } + it.tasks.withType(CppCompile) { + it.dependsOn generateProto + } + if(project.hasProperty('includePhotonTargeting')) { lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared' } } @@ -59,18 +67,23 @@ model { } } - if(project.hasProperty('includePhotonTargeting')) { - binaries.all { - lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared' - } - } - nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") nativeUtils.useRequiredLibrary(it, "googletest_static") nativeUtils.useRequiredLibrary(it, "apriltag_shared") nativeUtils.useRequiredLibrary(it, "opencv_shared") } } + binaries { + withType(GoogleTestTestSuiteBinarySpec) { + it.tasks.withType(CppCompile) { + it.dependsOn generateProto + } + + if(project.hasProperty('includePhotonTargeting')) { + lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared' + } + } + } } apply from: "${rootDir}/shared/publish.gradle" From 31d204964c2580bc2790ad0f57c2802cfce79121 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 19 Nov 2023 21:16:43 -0500 Subject: [PATCH 54/74] Update build.gradle --- photon-lib/build.gradle | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index e5d880fb4a..5becd0a264 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -57,12 +57,6 @@ dependencies { implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get(); } -cppHeadersZip { - from('src/generate/native/include') { - into '/' - } -} - def photonlibFileInput = file("src/generate/photonlib.json.in") ext.photonlibFileOutput = file("$buildDir/generated/vendordeps/photonlib.json") @@ -103,6 +97,14 @@ task writeCurrentVersion { build.mustRunAfter writeCurrentVersion +cppHeadersZip { + from('src/generate/native/include') { + into '/' + } +} + +cppHeadersZip.dependsOn writeCurrentVersion + model { components { all { From 2a30b099bc793a0b7a78afe52adf0d9d85fbd130 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 19 Nov 2023 23:53:37 -0500 Subject: [PATCH 55/74] fix deprecation warning --- photon-lib/src/main/java/org/photonvision/PhotonCamera.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index df186c6088..f7bf613fb9 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -312,14 +312,14 @@ public boolean isConnected() { public Optional> getCameraMatrix() { var cameraMatrix = cameraIntrinsicsSubscriber.get(); if (cameraMatrix != null && cameraMatrix.length == 9) { - return Optional.of(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(cameraMatrix)); + return Optional.of(MatBuilder.fill(Nat.N3(), Nat.N3(), cameraMatrix)); } else return Optional.empty(); } public Optional> getDistCoeffs() { var distCoeffs = cameraDistortionSubscriber.get(); if (distCoeffs != null && distCoeffs.length == 5) { - return Optional.of(new MatBuilder<>(Nat.N5(), Nat.N1()).fill(distCoeffs)); + return Optional.of(MatBuilder.fill(Nat.N5(), Nat.N1(), distCoeffs)); } else return Optional.empty(); } From e5cf987159507c8f8a82aa4ab1edf46893bd0b27 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 20 Nov 2023 00:27:13 -0500 Subject: [PATCH 56/74] Update OpenCVHelp.java --- .../main/java/org/photonvision/estimation/OpenCVHelp.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index ed53a17b80..e2413a4d22 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -18,6 +18,7 @@ package org.photonvision.estimation; import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.Num; @@ -63,8 +64,8 @@ public static void forceLoadOpenCV() { } static { - NWU_TO_EDN = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0)); - EDN_TO_NWU = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0)); + NWU_TO_EDN = new Rotation3d(MatBuilder.fill(Nat.N3(), Nat.N3(), 0, -1, 0, 0, 0, -1, 1, 0, 0)); + EDN_TO_NWU = new Rotation3d(MatBuilder.fill(Nat.N3(), Nat.N3(), 0, 0, 1, -1, 0, 0, 0, -1, 0)); } public static Mat matrixToMat(SimpleMatrix matrix) { From 68f9925c1f800db167745257e6ebce8d88376c11 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 20 Nov 2023 09:32:05 -0500 Subject: [PATCH 57/74] fix deprecations --- .../simulation/SimCameraProperties.java | 127 ++++++++++-------- 1 file changed, 69 insertions(+), 58 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java index 6d157fb252..1e2183fac8 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -25,6 +25,7 @@ package org.photonvision.simulation; import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -146,8 +147,8 @@ public SimCameraProperties(Path path, int width, int height) throws IOException setCalibration( jsonWidth, jsonHeight, - Matrix.mat(Nat.N3(), Nat.N3()).fill(jsonIntrinsics), - Matrix.mat(Nat.N5(), Nat.N1()).fill(jsonDistortion)); + MatBuilder.fill(Nat.N3(), Nat.N3(), jsonIntrinsics), + MatBuilder.fill(Nat.N5(), Nat.N1(), jsonDistortion)); setCalibError(jsonAvgError, jsonErrorStdDev); success = true; } @@ -184,7 +185,7 @@ public void setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) { double fy = cy / Math.tan(fovHeight.getRadians() / 2.0); // create camera intrinsics matrix - var camIntrinsics = Matrix.mat(Nat.N3(), Nat.N3()).fill(fx, 0, cx, 0, fy, cy, 0, 0, 1); + var camIntrinsics = MatBuilder.fill(Nat.N3(), Nat.N3(), fx, 0, cx, 0, fy, cy, 0, 0, 1); setCalibration(resWidth, resHeight, camIntrinsics, distCoeff); } @@ -597,17 +598,19 @@ public static SimCameraProperties PI4_LIFECAM_320_240() { prop.setCalibration( 320, 240, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 328.2733242048587, - 0.0, - 164.8190261141906, - 0.0, - 318.0609794305216, - 123.8633838438093, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 328.2733242048587, + 0.0, + 164.8190261141906, + 0.0, + 318.0609794305216, + 123.8633838438093, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.09957946553445934, -0.9166265114485799, @@ -626,17 +629,19 @@ public static SimCameraProperties PI4_LIFECAM_640_480() { prop.setCalibration( 640, 480, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 669.1428078983059, - 0.0, - 322.53377249329213, - 0.0, - 646.9843137061716, - 241.26567383784163, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 669.1428078983059, + 0.0, + 322.53377249329213, + 0.0, + 646.9843137061716, + 241.26567383784163, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.12788470750464645, -1.2350335805796528, @@ -655,17 +660,19 @@ public static SimCameraProperties LL2_640_480() { prop.setCalibration( 640, 480, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 511.22843367007755, - 0.0, - 323.62049380211096, - 0.0, - 514.5452336723849, - 261.8827920543568, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 511.22843367007755, + 0.0, + 323.62049380211096, + 0.0, + 514.5452336723849, + 261.8827920543568, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.1917469998873756, -0.5142936883324216, @@ -684,17 +691,19 @@ public static SimCameraProperties LL2_960_720() { prop.setCalibration( 960, 720, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 769.6873145148892, - 0.0, - 486.1096609458122, - 0.0, - 773.8164483705323, - 384.66071662358354, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 769.6873145148892, + 0.0, + 486.1096609458122, + 0.0, + 773.8164483705323, + 384.66071662358354, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.189462064814501, -0.49903003669627627, @@ -713,17 +722,19 @@ public static SimCameraProperties LL2_1280_720() { prop.setCalibration( 1280, 720, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 1011.3749416937393, - 0.0, - 645.4955139388737, - 0.0, - 1008.5391755084075, - 508.32877656020196, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 1011.3749416937393, + 0.0, + 645.4955139388737, + 0.0, + 1008.5391755084075, + 508.32877656020196, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.13730101577061535, -0.2904345656989261, From defe690c7e28d33318fa4161d71c2dfca0b28938 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 20 Nov 2023 09:42:19 -0500 Subject: [PATCH 58/74] Update setupBuild.gradle --- shared/setupBuild.gradle | 2 -- 1 file changed, 2 deletions(-) diff --git a/shared/setupBuild.gradle b/shared/setupBuild.gradle index 51926d32ee..caf6ee43d0 100644 --- a/shared/setupBuild.gradle +++ b/shared/setupBuild.gradle @@ -19,12 +19,10 @@ model { cpp { source { srcDirs 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp" - srcDirs 'src/main/native/cpp' include '**/*.cpp', '**/*.cc' } exportedHeaders { srcDirs 'src/main/native/include', "$buildDir/generated/source/proto/main/cpp" - srcDirs 'src/main/native/include' include '**/*.h' } } From 57219e560dd19afb16fd3de444374cc1bb7accc8 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 20 Nov 2023 16:52:36 -0500 Subject: [PATCH 59/74] update build --- shared/setupBuild.gradle | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/shared/setupBuild.gradle b/shared/setupBuild.gradle index caf6ee43d0..cd5bb03383 100644 --- a/shared/setupBuild.gradle +++ b/shared/setupBuild.gradle @@ -23,10 +23,11 @@ model { } exportedHeaders { srcDirs 'src/main/native/include', "$buildDir/generated/source/proto/main/cpp" - include '**/*.h' + include "**/*.h" } } } + binaries.all { if (it instanceof SharedLibraryBinarySpec) { it.buildable = false @@ -60,28 +61,26 @@ model { include '**/*.cpp' } exportedHeaders { - srcDirs 'src/test/native/include', 'src/main/native/cpp' + srcDirs 'src/test/native/include', "$buildDir/generated/source/proto/main/cpp" } } } + binaries.all { + it.tasks.withType(CppCompile) { + it.dependsOn generateProto + } + if(project.hasProperty('includePhotonTargeting')) { + lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared' + } + } + nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") nativeUtils.useRequiredLibrary(it, "googletest_static") nativeUtils.useRequiredLibrary(it, "apriltag_shared") nativeUtils.useRequiredLibrary(it, "opencv_shared") } } - binaries { - withType(GoogleTestTestSuiteBinarySpec) { - it.tasks.withType(CppCompile) { - it.dependsOn generateProto - } - - if(project.hasProperty('includePhotonTargeting')) { - lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared' - } - } - } } apply from: "${rootDir}/shared/publish.gradle" From 3e7ed35b0ecc28ed45cf170ccd68d7a05100f781 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 20 Nov 2023 21:57:48 -0500 Subject: [PATCH 60/74] gib push --- build.gradle | 2 +- photon-lib/build.gradle | 14 ++++++++++++++ shared/setupBuild.gradle | 4 ---- 3 files changed, 15 insertions(+), 5 deletions(-) diff --git a/build.gradle b/build.gradle index d95d9ceaa7..899ad1f515 100644 --- a/build.gradle +++ b/build.gradle @@ -4,7 +4,7 @@ plugins { id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2" id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" id 'edu.wpi.first.WpilibTools' version '1.3.0' - id 'com.google.protobuf' version '0.9.3' apply false + id 'com.google.protobuf' version '0.9.4' apply false } allprojects { diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 5becd0a264..2592e2d006 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -114,6 +114,20 @@ model { srcDirs "src/generate/native/include" } } + it.binaries.all { + it.tasks.withType(CppCompile) { + it.dependsOn ":photon-targeting:generateProto" + } + } + } + } + testSuites { + all { + it.binaries.all { + it.tasks.withType(CppCompile) { + it.dependsOn ":photon-targeting:generateProto" + } + } } } diff --git a/shared/setupBuild.gradle b/shared/setupBuild.gradle index cd5bb03383..b3206b56a3 100644 --- a/shared/setupBuild.gradle +++ b/shared/setupBuild.gradle @@ -29,10 +29,6 @@ model { } binaries.all { - if (it instanceof SharedLibraryBinarySpec) { - it.buildable = false - return - } it.tasks.withType(CppCompile) { it.dependsOn generateProto } From 198d1f7e77c59cebccd81b73537664cd9bcefed6 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Mon, 20 Nov 2023 22:00:17 -0500 Subject: [PATCH 61/74] add comment --- photon-lib/build.gradle | 1 + 1 file changed, 1 insertion(+) diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 2592e2d006..9858e157d3 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -105,6 +105,7 @@ cppHeadersZip { cppHeadersZip.dependsOn writeCurrentVersion +// Building photon-lib requires photon-targeting to generate its proto files. This technically shouldn't be required but is needed for it to build. model { components { all { From 479c6bf31bb9da193dfce54c09744a5cbda0b751 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 21 Nov 2023 11:06:16 -0500 Subject: [PATCH 62/74] json wpilib bump --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index 899ad1f515..15eccaba75 100644 --- a/build.gradle +++ b/build.gradle @@ -21,7 +21,7 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2024.1.1-beta-3-29-g3985c03" + wpilibVersion = "2024.1.1-beta-3-32-gcfbff32" openCVversion = "4.8.0-2" joglVersion = "2.4.0-rc-20200307" javalinVersion = "5.6.2" From b9113942d5ecda5e665629106a8e6a23571f66ec Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 21 Nov 2023 15:01:09 -0500 Subject: [PATCH 63/74] fix PNPResult encoding bug --- .../src/main/native/cpp/photon/targeting/PNPResult.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp index 15887ab35e..96d3b3d23b 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp @@ -51,6 +51,7 @@ void wpi::Protobuf::Pack(google::protobuf::Message* msg, const photon::PNPResult& value) { auto m = static_cast(msg); + m->set_is_present(value.isPresent); wpi::PackProtobuf(m->mutable_best(), value.best); m->set_best_reproj_err(value.bestReprojErr); wpi::PackProtobuf(m->mutable_alt(), value.alt); From a6b669e23d80d59857b6a95b2f574d9e76299807 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 21 Nov 2023 18:10:32 -0500 Subject: [PATCH 64/74] bump wpilib again --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index 15eccaba75..a8991a1e95 100644 --- a/build.gradle +++ b/build.gradle @@ -21,7 +21,7 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2024.1.1-beta-3-32-gcfbff32" + wpilibVersion = "2024.1.1-beta-3-43-g25b7dca" openCVversion = "4.8.0-2" joglVersion = "2.4.0-rc-20200307" javalinVersion = "5.6.2" From 1a219274cb4d90d6c82f0bb0ccdbf5b1d1d3da9f Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 21 Nov 2023 18:21:24 -0500 Subject: [PATCH 65/74] fix gradle --- photon-core/build.gradle | 1 - photon-lib/build.gradle | 1 + photon-server/build.gradle | 8 -------- photon-targeting/build.gradle | 1 + shared/common.gradle | 1 + 5 files changed, 3 insertions(+), 9 deletions(-) diff --git a/photon-core/build.gradle b/photon-core/build.gradle index 6fbf407797..dafd62f141 100644 --- a/photon-core/build.gradle +++ b/photon-core/build.gradle @@ -24,7 +24,6 @@ dependencies { // Zip implementation 'org.zeroturnaround:zt-zip:1.14' - implementation wpilibTools.deps.wpilibJava("apriltag") implementation "org.xerial:sqlite-jdbc:3.41.0.0" } diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 9858e157d3..ebd30ad239 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -47,6 +47,7 @@ dependencies { implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") + implementation wpilibTools.deps.wpilibJava("wpiunits") implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() diff --git a/photon-server/build.gradle b/photon-server/build.gradle index 2fdfcff4ec..8aaef99ec3 100644 --- a/photon-server/build.gradle +++ b/photon-server/build.gradle @@ -22,14 +22,6 @@ dependencies { implementation "io.javalin:javalin:$javalinVersion" implementation "org.slf4j:slf4j-simple:2.0.7" - implementation wpilibTools.deps.wpilibJava("wpiutil") - implementation wpilibTools.deps.wpilibJava("wpimath") - implementation wpilibTools.deps.wpilibJava("wpinet") - implementation wpilibTools.deps.wpilibJava("hal") - implementation wpilibTools.deps.wpilibJava("ntcore") - implementation wpilibTools.deps.wpilibJava("wpilibj") - implementation wpilibTools.deps.wpilibOpenCv("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) - implementation "org.msgpack:msgpack-core:0.9.0" implementation "org.msgpack:jackson-dataformat-msgpack:0.9.0" } diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 47eff41568..6df75d8222 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -40,6 +40,7 @@ dependencies { implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") + implementation wpilibTools.deps.wpilibJava("wpiunits") implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() diff --git a/shared/common.gradle b/shared/common.gradle index dc3331ff16..02aebd1c00 100644 --- a/shared/common.gradle +++ b/shared/common.gradle @@ -38,6 +38,7 @@ dependencies { implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") + implementation wpilibTools.deps.wpilibJava("wpiunits") implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) // Jackson From 5d931ffad7ddb2dea15602585922ec2f92595571 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Tue, 21 Nov 2023 20:25:49 -0500 Subject: [PATCH 66/74] look --- .../targeting/MultiTargetPNPResultTest.cpp | 55 +++++++- .../native/cpp/targeting/PNPResultTest.cpp | 55 +++++++- .../targeting/PhotonPipelineResultTest.cpp | 131 +++++++++++++++++- .../cpp/targeting/PhotonTrackedTargetTest.cpp | 61 +++++++- 4 files changed, 288 insertions(+), 14 deletions(-) diff --git a/photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp b/photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp index 1add81e9f6..ffda261ca4 100644 --- a/photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp +++ b/photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp @@ -16,10 +16,57 @@ */ #include "gtest/gtest.h" +#include "photon.pb.h" #include "photon/targeting/MultiTargetPNPResult.h" -// TODO -TEST(MultiTargetPNPResultTest, Equality) {} +TEST(MultiTargetPNPResultTest, Equality) { + photon::MultiTargetPNPResult a; + photon::MultiTargetPNPResult b; -// TODO -TEST(MultiTargetPNPResultTest, Inequality) {} + EXPECT_EQ(a, b); + + photon::PNPResult pnpRes{ + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, 0}; + + photon::MultiTargetPNPResult a1{pnpRes, {1, 2, 3, 4}}; + photon::MultiTargetPNPResult b1{pnpRes, {1, 2, 3, 4}}; + + EXPECT_EQ(a1, b1); +} + +TEST(MultiTargetPNPResultTest, Roundtrip) { + photon::MultiTargetPNPResult result; + + google::protobuf::Arena arena; + google::protobuf::Message* proto = + wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result); + + photon::MultiTargetPNPResult unpacked_data = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result, unpacked_data); + + photon::PNPResult pnpRes{ + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, 0}; + + photon::MultiTargetPNPResult result1{pnpRes, {1, 2, 3, 4}}; + + proto = wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result1); + + photon::MultiTargetPNPResult unpacked_data1 = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result1, unpacked_data1); +} diff --git a/photon-targeting/src/test/native/cpp/targeting/PNPResultTest.cpp b/photon-targeting/src/test/native/cpp/targeting/PNPResultTest.cpp index ac2c2adf11..c4313396ae 100644 --- a/photon-targeting/src/test/native/cpp/targeting/PNPResultTest.cpp +++ b/photon-targeting/src/test/native/cpp/targeting/PNPResultTest.cpp @@ -16,10 +16,57 @@ */ #include "gtest/gtest.h" +#include "photon.pb.h" #include "photon/targeting/PNPResult.h" -// TODO -TEST(PNPResultTest, Equality) {} +TEST(PNPResultTest, Equality) { + photon::PNPResult a; + photon::PNPResult b; -// TODO -TEST(PNPResultTest, Inequality) {} + EXPECT_EQ(a, b); + + photon::PNPResult a1{frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, 0}; + photon::PNPResult b1{frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, 0}; + + EXPECT_EQ(a1, b1); +} + +TEST(PNPResultTest, Roundtrip) { + photon::PNPResult result; + + google::protobuf::Arena arena; + google::protobuf::Message* proto = + wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result); + + photon::PNPResult unpacked_data = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result, unpacked_data); + + photon::PNPResult result1{ + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, 0}; + + proto = wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result1); + + photon::PNPResult unpacked_data2 = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result1, unpacked_data2); +} diff --git a/photon-targeting/src/test/native/cpp/targeting/PhotonPipelineResultTest.cpp b/photon-targeting/src/test/native/cpp/targeting/PhotonPipelineResultTest.cpp index 716334dcb3..73a4efcfdb 100644 --- a/photon-targeting/src/test/native/cpp/targeting/PhotonPipelineResultTest.cpp +++ b/photon-targeting/src/test/native/cpp/targeting/PhotonPipelineResultTest.cpp @@ -16,10 +16,137 @@ */ #include "gtest/gtest.h" +#include "photon.pb.h" #include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" // TODO -TEST(PhotonPipelineResultTest, Equality) {} +TEST(PhotonPipelineResultTest, Equality) { + photon::PhotonPipelineResult a{12_ms, {}}; + photon::PhotonPipelineResult b{12_ms, {}}; + + EXPECT_EQ(a, b); + + wpi::SmallVector targets{ + photon::PhotonTrackedTarget{ + 3.0, + -4.0, + 9.0, + 4.0, + 1, + 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)), + -1, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}, + photon::PhotonTrackedTarget{ + 3.0, + -4.0, + 9.1, + 6.7, + -1, + 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)), + -1, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, + std::pair{7, 8}}}}; + + photon::PhotonPipelineResult a1{12_ms, targets}; + photon::PhotonPipelineResult b1{12_ms, targets}; + + EXPECT_EQ(a1, b1); + + photon::PNPResult pnpRes{ + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, 0}; + + photon::MultiTargetPNPResult multitagRes{pnpRes, {1, 2, 3, 4}}; + + photon::PhotonPipelineResult a2{12_ms, targets, multitagRes}; + photon::PhotonPipelineResult b2{12_ms, targets, multitagRes}; + + EXPECT_EQ(a2, b2); +} // TODO -TEST(PhotonPipelineResultTest, Inequality) {} +TEST(PhotonPipelineResultTest, Roundtrip) { + photon::PhotonPipelineResult result{12_ms, {}}; + + google::protobuf::Arena arena; + google::protobuf::Message* proto = + wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result); + + photon::PhotonPipelineResult unpacked_data = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result, unpacked_data); + + wpi::SmallVector targets{ + photon::PhotonTrackedTarget{ + 3.0, + -4.0, + 9.0, + 4.0, + 1, + 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)), + -1, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}, + photon::PhotonTrackedTarget{ + 3.0, + -4.0, + 9.1, + 6.7, + -1, + 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)), + -1, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, + std::pair{7, 8}}}}; + + photon::PhotonPipelineResult result2{12_ms, targets}; + + proto = wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result2); + + photon::PhotonPipelineResult unpacked_data2 = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result2, unpacked_data2); + + photon::PNPResult pnpRes{ + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, 0}; + + photon::MultiTargetPNPResult multitagRes{pnpRes, {1, 2, 3, 4}}; + + photon::PhotonPipelineResult result3{12_ms, targets, multitagRes}; + + proto = wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result3); + + photon::PhotonPipelineResult unpacked_data3 = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result3, unpacked_data3); +} diff --git a/photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp b/photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp index 243c56ad6f..ea8d0d5de7 100644 --- a/photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp +++ b/photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp @@ -16,10 +16,63 @@ */ #include "gtest/gtest.h" +#include "photon.pb.h" #include "photon/targeting/PhotonTrackedTarget.h" -// TODO -TEST(PhotonTrackedTargetTest, Equality) {} +TEST(PhotonTrackedTargetTest, Equality) { + photon::PhotonTrackedTarget a{ + 3.0, + 4.0, + 9.0, + -5.0, + -1, + 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)), + -1, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}; -// TODO -TEST(PhotonTrackedTargetTest, Inequality) {} + photon::PhotonTrackedTarget b{ + 3.0, + 4.0, + 9.0, + -5.0, + -1, + 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)), + -1, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}; + + EXPECT_EQ(a, b); +} + +TEST(PhotonTrackedTargetTest, Roundtrip) { + photon::PhotonTrackedTarget target{ + 3.0, + 4.0, + 9.0, + -5.0, + -1, + 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)), + -1, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}; + + google::protobuf::Arena arena; + google::protobuf::Message* proto = + wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, target); + + photon::PhotonTrackedTarget unpacked_data = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(target, unpacked_data); +} From 0ab58a706cae69abc40da707d6b6302a11b11e58 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Wed, 22 Nov 2023 11:06:04 -0500 Subject: [PATCH 67/74] remove modified files --- .../apriltagExample/simgui-window.json | 14 +++++++------- .../aimandrange/networktables.json | 1 - 2 files changed, 7 insertions(+), 8 deletions(-) delete mode 100644 photonlib-java-examples/aimandrange/networktables.json diff --git a/photonlib-cpp-examples/apriltagExample/simgui-window.json b/photonlib-cpp-examples/apriltagExample/simgui-window.json index 0f521f36e0..79d676b16b 100644 --- a/photonlib-cpp-examples/apriltagExample/simgui-window.json +++ b/photonlib-cpp-examples/apriltagExample/simgui-window.json @@ -10,8 +10,8 @@ "style": "0", "userScale": "2", "width": "1652", - "xpos": "568", - "ypos": "336" + "xpos": "268", + "ypos": "82" } }, "Table": { @@ -30,12 +30,12 @@ "###FMS": { "Collapsed": "0", "Pos": "36,663", - "Size": "336,164" + "Size": "283,146" }, "###Joysticks": { "Collapsed": "0", "Pos": "373,500", - "Size": "976,239" + "Size": "796,206" }, "###Keyboard 0 Settings": { "Collapsed": "0", @@ -60,12 +60,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "5,350", - "Size": "232,254" + "Size": "192,218" }, "###Timing": { "Collapsed": "0", "Pos": "5,150", - "Size": "162,142" + "Size": "135,127" }, "Debug##Default": { "Collapsed": "0", @@ -75,7 +75,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "109,114" + "Size": "92,99" } } } diff --git a/photonlib-java-examples/aimandrange/networktables.json b/photonlib-java-examples/aimandrange/networktables.json deleted file mode 100644 index fe51488c70..0000000000 --- a/photonlib-java-examples/aimandrange/networktables.json +++ /dev/null @@ -1 +0,0 @@ -[] From 8ece13b225a5ccc714498ce60ff725cfcc5709ad Mon Sep 17 00:00:00 2001 From: Matt Date: Wed, 22 Nov 2023 19:59:32 -0500 Subject: [PATCH 68/74] Manually initialize pnpresult --- .../main/native/include/photon/targeting/PNPResult.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h index 156be1b3b8..f200a3de04 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h @@ -36,7 +36,15 @@ class PNPResult { double ambiguity; - PNPResult() = default; + // Apparently this can't be default-constructed? Things seem to have garbadge + // with the defaulted empty constructor anyhow + PNPResult() + : isPresent{false}, + best{frc::Transform3d{}}, + bestReprojErr{0}, + alt{frc::Transform3d()}, + altReprojErr{0}, + ambiguity{0} {} PNPResult(frc::Transform3d best, double bestReprojErr, frc::Transform3d alt, double altReprojErr, double ambiguity) From d12eca521b719e5dc523900fb2f750e680683dce Mon Sep 17 00:00:00 2001 From: Matt Date: Wed, 22 Nov 2023 20:15:25 -0500 Subject: [PATCH 69/74] Bump wpilib version in examples --- shared/examples_common.gradle | 3 +++ 1 file changed, 3 insertions(+) diff --git a/shared/examples_common.gradle b/shared/examples_common.gradle index 5212307f0f..711f6c9fa8 100644 --- a/shared/examples_common.gradle +++ b/shared/examples_common.gradle @@ -18,3 +18,6 @@ task copyPhotonlib() { } outputs.upToDateWhen { false } } + +wpi.maven.useDevelopment = true +wpi.versions.wpilibVersion = "2024.1.1-beta-3-43-g25b7dca" From 9340e4f8fc4829093b0a33536114bf185377a9a8 Mon Sep 17 00:00:00 2001 From: Matt Date: Wed, 22 Nov 2023 20:21:24 -0500 Subject: [PATCH 70/74] Update javacommon.gradle --- shared/javacommon.gradle | 1 + 1 file changed, 1 insertion(+) diff --git a/shared/javacommon.gradle b/shared/javacommon.gradle index ba1be6aa5c..d1c17f5016 100644 --- a/shared/javacommon.gradle +++ b/shared/javacommon.gradle @@ -114,6 +114,7 @@ dependencies { implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") + implementation wpilibTools.deps.wpilibJava("wpiunits") implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() From 1504f59cf6699da3a5b56e236793b73814ba5ebf Mon Sep 17 00:00:00 2001 From: Matt Date: Wed, 22 Nov 2023 20:21:57 -0500 Subject: [PATCH 71/74] Update build.gradle --- photon-lib/build.gradle | 1 + 1 file changed, 1 insertion(+) diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index e167944fc9..14cdc7ca30 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -88,6 +88,7 @@ model { } } } +} def vendorJson = artifacts.add('archives', file("$photonlibFileOutput")) model { From f98ed57b3cdc383571fe9cf49431e20e4cea90a3 Mon Sep 17 00:00:00 2001 From: Matt Date: Wed, 22 Nov 2023 20:22:39 -0500 Subject: [PATCH 72/74] Update build.gradle --- photon-lib/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 14cdc7ca30..ec468c1f21 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -22,7 +22,6 @@ cppHeadersZip { into '/' } } -cppHeadersZip.dependsOn writeCurrentVersion def photonlibFileInput = file("src/generate/photonlib.json.in") ext.photonlibFileOutput = file("$buildDir/generated/vendordeps/photonlib.json") @@ -61,6 +60,7 @@ task writeCurrentVersion { } build.mustRunAfter writeCurrentVersion +cppHeadersZip.dependsOn writeCurrentVersion // Building photon-lib requires photon-targeting to generate its proto files. This technically shouldn't be required but is needed for it to build. model { From 98e0117033eae18668b15f4c68c97edd16f70206 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Thu, 23 Nov 2023 18:49:42 -0500 Subject: [PATCH 73/74] bump wpilib --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index a8991a1e95..e106400cd6 100644 --- a/build.gradle +++ b/build.gradle @@ -21,7 +21,7 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2024.1.1-beta-3-43-g25b7dca" + wpilibVersion = "2024.1.1-beta-3-50-gc80b2d2" openCVversion = "4.8.0-2" joglVersion = "2.4.0-rc-20200307" javalinVersion = "5.6.2" From cadbb095964a3677afdb5f88e68f24a27a7eae18 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 24 Nov 2023 10:20:56 -0500 Subject: [PATCH 74/74] Update build.gradle --- build.gradle | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/build.gradle b/build.gradle index 4e409bb763..b4e3f50576 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "com.diffplug.spotless" version "6.22.0" - id "edu.wpi.first.NativeUtils" version "2024.3.2" apply false + id "edu.wpi.first.NativeUtils" version "2024.4.0" apply false id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2" id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" id 'edu.wpi.first.WpilibTools' version '1.3.0' @@ -21,7 +21,7 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2024.1.1-beta-3-50-gc80b2d2" + wpilibVersion = "2024.1.1-beta-3-52-g9280054" wpimathVersion = wpilibVersion openCVversion = "4.8.0-2" joglVersion = "2.4.0-rc-20200307"