diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index e2265dcead..cd64965990 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 { @@ -141,12 +140,9 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { path = cameraTable.getPath(); rawBytesEntry = cameraTable - .getRawTopic("result_proto") + .getRawTopic("rawBytes") .subscribe( - "proto:" + PhotonPipelineResult.getDescriptor().getFullName(), - new byte[] {}, - PubSubOption.periodic(0.01), - PubSubOption.sendAll(true)); + "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish(); driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false); inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0); @@ -187,26 +183,23 @@ public PhotonCamera(String cameraName) { * @return The latest pipeline result. */ public PhotonPipelineResult getLatestResult() { - // verifyVersion(); // Protobufs _should_ deal with this for us + verifyVersion(); - var ret = PhotonPipelineResult.newInstance(); + // Clear the packet. + packet.clear(); - var bytes = rawBytesEntry.get(new byte[] {}); - if (bytes.length < 1) { - return PhotonPipelineResult.newInstance(); - } + // Create latest result. + var ret = new PhotonPipelineResult(); - try { - ret = PhotonPipelineResult.parseFrom(bytes); - } catch (InvalidProtocolBufferException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - return ret; - } + // Populate packet and create result. + packet.setData(rawBytesEntry.get(new byte[] {})); + + if (packet.getSize() < 1) return ret; + ret.createFromPacket(packet); // Set the timestamp of the result. // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds. - ret.setTimestampSec((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMs() / 1e3); + ret.setTimestampSeconds((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3); // Return result. return ret; diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java 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..f725837e13 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -56,6 +56,8 @@ import org.photonvision.estimation.VisionEstimation; import org.photonvision.targeting.MultiTargetPNPResults; import org.photonvision.targeting.PNPResults; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; /** * A handle for simulating {@link PhotonCamera} values. Processing simulated targets through this diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java 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;