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 11aa38852b..be51316126 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 @@ -17,6 +17,7 @@ package org.photonvision.common.dataflow.networktables; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.util.WPIUtilJNI; @@ -159,16 +160,7 @@ public void accept(CVPipelineResult result) { ts.targetSkewEntry.set(bestTarget.getSkew()); var pose = bestTarget.getBestCameraToTarget3d(); - ts.targetPoseEntry.set( - new double[] { - pose.getTranslation().getX(), - pose.getTranslation().getY(), - pose.getTranslation().getZ(), - pose.getRotation().getQuaternion().getW(), - pose.getRotation().getQuaternion().getX(), - pose.getRotation().getQuaternion().getY(), - pose.getRotation().getQuaternion().getZ() - }); + ts.targetPoseEntry.set(pose); var targetOffsetPoint = bestTarget.getTargetOffsetPoint(); ts.bestTargetPosX.set(targetOffsetPoint.x); @@ -178,7 +170,7 @@ public void accept(CVPipelineResult result) { ts.targetYawEntry.set(0); ts.targetAreaEntry.set(0); ts.targetSkewEntry.set(0); - ts.targetPoseEntry.set(new double[] {0, 0, 0}); + ts.targetPoseEntry.set(new Transform3d()); ts.bestTargetPosX.set(0); ts.bestTargetPosY.set(0); } 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 f5ab57e5c1..7579218595 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -32,6 +32,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.util.PixelFormat; import edu.wpi.first.util.WPIUtilJNI; import edu.wpi.first.wpilibj.RobotController; @@ -588,7 +589,7 @@ public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimest ts.targetPitchEntry.set(0.0, receiveTimestamp); ts.targetYawEntry.set(0.0, receiveTimestamp); ts.targetAreaEntry.set(0.0, receiveTimestamp); - ts.targetPoseEntry.set(new double[] {0.0, 0.0, 0.0}, receiveTimestamp); + ts.targetPoseEntry.set(new Transform3d(), receiveTimestamp); ts.targetSkewEntry.set(0.0, receiveTimestamp); } else { var bestTarget = result.getBestTarget(); @@ -599,10 +600,7 @@ public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimest ts.targetSkewEntry.set(bestTarget.getSkew(), receiveTimestamp); var transform = bestTarget.getBestCameraToTarget(); - double[] poseData = { - transform.getX(), transform.getY(), transform.getRotation().toRotation2d().getDegrees() - }; - ts.targetPoseEntry.set(poseData, receiveTimestamp); + ts.targetPoseEntry.set(transform, receiveTimestamp); } ts.cameraIntrinsicsPublisher.set(prop.getIntrinsics().getData(), receiveTimestamp); 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 index bd560c1e94..24ce38c4f5 100644 --- 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 @@ -303,12 +303,11 @@ public int decodeInt() { if (packetData.length < readPos + 3) { return 0; } - return ( - (0xff & packetData[readPos++] - | (0xff & packetData[readPos++]) << 8 - | (0xff & packetData[readPos++]) << 16 - | 0xff & packetData[readPos++]) << 24 - ); + return ((0xff & packetData[readPos++] + | (0xff & packetData[readPos++]) << 8 + | (0xff & packetData[readPos++]) << 16 + | 0xff & packetData[readPos++]) + << 24); } public long decodeLong() { @@ -316,14 +315,15 @@ public long decodeLong() { return 0; } long data = - (long) (0xff & packetData[readPos++] - | (long) (0xff & packetData[readPos++]) << 8 - | (long) (0xff & packetData[readPos++]) << 16 - | (long) (0xff & packetData[readPos++]) << 24 - | (long) (0xff & packetData[readPos++]) << 32 - | (long) (0xff & packetData[readPos++]) << 40 - | (long) (0xff & packetData[readPos++]) << 48 - | (long) (0xff & packetData[readPos++]) << 56); + (long) + (0xff & packetData[readPos++] + | (long) (0xff & packetData[readPos++]) << 8 + | (long) (0xff & packetData[readPos++]) << 16 + | (long) (0xff & packetData[readPos++]) << 24 + | (long) (0xff & packetData[readPos++]) << 32 + | (long) (0xff & packetData[readPos++]) << 40 + | (long) (0xff & packetData[readPos++]) << 48 + | (long) (0xff & packetData[readPos++]) << 56); return data; } @@ -337,14 +337,15 @@ public double decodeDouble() { return 0; } long data = - (long) (0xff & packetData[readPos++] - | (long) (0xff & packetData[readPos++]) << 8 - | (long) (0xff & packetData[readPos++]) << 16 - | (long) (0xff & packetData[readPos++]) << 24 - | (long) (0xff & packetData[readPos++]) << 32 - | (long) (0xff & packetData[readPos++]) << 40 - | (long) (0xff & packetData[readPos++]) << 48 - | (long) (0xff & packetData[readPos++]) << 56); + (long) + (0xff & packetData[readPos++] + | (long) (0xff & packetData[readPos++]) << 8 + | (long) (0xff & packetData[readPos++]) << 16 + | (long) (0xff & packetData[readPos++]) << 24 + | (long) (0xff & packetData[readPos++]) << 32 + | (long) (0xff & packetData[readPos++]) << 40 + | (long) (0xff & packetData[readPos++]) << 48 + | (long) (0xff & packetData[readPos++]) << 56); return Double.longBitsToDouble(data); } @@ -358,12 +359,12 @@ public float decodeFloat() { return 0; } - int data = ( - (0xff & packetData[readPos++] - | (0xff & packetData[readPos++]) << 8 - | (0xff & packetData[readPos++]) << 16 - | 0xff & packetData[readPos++]) << 24 - ); + int data = + ((0xff & packetData[readPos++] + | (0xff & packetData[readPos++]) << 8 + | (0xff & packetData[readPos++]) << 16 + | 0xff & packetData[readPos++]) + << 24); return Float.intBitsToFloat(data); } 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 d5f53f70d2..1b1fd2698f 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,7 @@ package org.photonvision.common.networktables; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.BooleanPublisher; import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.networktables.BooleanTopic; @@ -28,6 +29,7 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.ProtobufPublisher; import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.networktables.StructPublisher; import org.photonvision.targeting.PhotonPipelineResult; /** @@ -56,7 +58,7 @@ public class NTTopicSet { public DoublePublisher targetPitchEntry; public DoublePublisher targetYawEntry; public DoublePublisher targetAreaEntry; - public DoubleArrayPublisher targetPoseEntry; + public StructPublisher targetPoseEntry; public DoublePublisher targetSkewEntry; // The raw position of the best target, in pixels. @@ -102,7 +104,7 @@ public void updateEntries() { targetPitchEntry = subTable.getDoubleTopic("targetPitch").publish(); targetAreaEntry = subTable.getDoubleTopic("targetArea").publish(); targetYawEntry = subTable.getDoubleTopic("targetYaw").publish(); - targetPoseEntry = subTable.getDoubleArrayTopic("targetPose").publish(); + targetPoseEntry = subTable.getStructTopic("targetPose", Transform3d.struct).publish(); targetSkewEntry = subTable.getDoubleTopic("targetSkew").publish(); bestTargetPosX = subTable.getDoubleTopic("targetPixelsX").publish(); diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java index 8b689914dc..751e6a3d6a 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java @@ -19,12 +19,8 @@ import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.ObjectMapper; - import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.networktables.RawPublisher; - -import java.util.Arrays; import java.util.HashSet; import java.util.Set; import org.photonvision.common.dataflow.structures.Packet; @@ -50,15 +46,14 @@ public PacketPublisher(RawPublisher publisher, PacketSerde photonStruct) { } addSchemaImpl(photonStruct, new HashSet<>()); + // TODO: don't hard-code this this.publisher.getTopic().getInstance().addSchema(Transform3d.struct); } public void set(T value, int byteSize) { var packet = new Packet(byteSize); photonStruct.pack(packet, value); - // todo: trim to only the bytes we need to send publisher.set(packet.getWrittenDataCopy()); - System.out.println(Arrays.toString(packet.getWrittenDataCopy())); } public void set(T value) {