Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add message UUID and type names to hash and message defintion (#1409)

Browse Source
This commit is contained in:
Matt
2024-09-22 22:27:13 -04:00
committed by GitHub
parent 360298cc24
commit f33218c49c
43 changed files with 605 additions and 190 deletions
Download Patch File Download Diff File Expand all files Collapse all files

93
photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
View File

@@ -160,8 +160,8 @@ public class Packet {
*/
public void encode(short src) {
ensureCapacity(2);
packetData[writePos++] = (byte) (src >>> 8);
packetData[writePos++] = (byte) src;
packetData[writePos++] = (byte) (src >>> 8);
}
/**
@@ -171,10 +171,10 @@ public class Packet {
*/
public void encode(int src) {
ensureCapacity(4);
packetData[writePos++] = (byte) (src >>> 24);
packetData[writePos++] = (byte) (src >>> 16);
packetData[writePos++] = (byte) (src >>> 8);
packetData[writePos++] = (byte) src;
packetData[writePos++] = (byte) (src >>> 8);
packetData[writePos++] = (byte) (src >>> 16);
packetData[writePos++] = (byte) (src >>> 24);
}
/**
@@ -185,10 +185,10 @@ public class Packet {
public void encode(float src) {
ensureCapacity(4);
int data = Float.floatToIntBits(src);
packetData[writePos++] = (byte) ((data >> 24) & 0xff);
packetData[writePos++] = (byte) ((data >> 16) & 0xff);
packetData[writePos++] = (byte) ((data >> 8) & 0xff);
packetData[writePos++] = (byte) (data & 0xff);
packetData[writePos++] = (byte) ((data >> 8) & 0xff);
packetData[writePos++] = (byte) ((data >> 16) & 0xff);
packetData[writePos++] = (byte) ((data >> 24) & 0xff);
}
/**
@@ -198,14 +198,14 @@ public class Packet {
*/
public void encode(long data) {
ensureCapacity(8);
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);
packetData[writePos++] = (byte) ((data >> 8) & 0xff);
packetData[writePos++] = (byte) ((data >> 16) & 0xff);
packetData[writePos++] = (byte) ((data >> 24) & 0xff);
packetData[writePos++] = (byte) ((data >> 32) & 0xff);
packetData[writePos++] = (byte) ((data >> 40) & 0xff);
packetData[writePos++] = (byte) ((data >> 48) & 0xff);
packetData[writePos++] = (byte) ((data >> 56) & 0xff);
}
/**
@@ -216,14 +216,14 @@ public class Packet {
public void encode(double src) {
ensureCapacity(8);
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);
packetData[writePos++] = (byte) ((data >> 8) & 0xff);
packetData[writePos++] = (byte) ((data >> 16) & 0xff);
packetData[writePos++] = (byte) ((data >> 24) & 0xff);
packetData[writePos++] = (byte) ((data >> 32) & 0xff);
packetData[writePos++] = (byte) ((data >> 40) & 0xff);
packetData[writePos++] = (byte) ((data >> 48) & 0xff);
packetData[writePos++] = (byte) ((data >> 56) & 0xff);
}
/**
@@ -303,10 +303,10 @@ public class Packet {
if (packetData.length < readPos + 3) {
return 0;
}
return (0xff & packetData[readPos++]) << 24
| (0xff & packetData[readPos++]) << 16
return (0xff & packetData[readPos++])
| (0xff & packetData[readPos++]) << 8
| (0xff & packetData[readPos++]);
| (0xff & packetData[readPos++]) << 16
| (0xff & packetData[readPos++]) << 24;
}
public long decodeLong() {
@@ -314,14 +314,15 @@ public class Packet {
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++]);
(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;
}
@@ -335,14 +336,15 @@ public class Packet {
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++]);
(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);
}
@@ -355,11 +357,12 @@ public class Packet {
if (packetData.length < (readPos + 3)) {
return 0;
}
int data =
(int) (0xff & packetData[readPos++]) << 24
| (int) (0xff & packetData[readPos++]) << 16
| (int) (0xff & packetData[readPos++]) << 8
| (int) (0xff & packetData[readPos++]);
((0xff & packetData[readPos++]
| (0xff & packetData[readPos++]) << 8
| (0xff & packetData[readPos++]) << 16
| (0xff & packetData[readPos++]) << 24));
return Float.intBitsToFloat(data);
}
@@ -393,7 +396,7 @@ public class Packet {
if (packetData.length < readPos + 1) {
return 0;
}
return (short) ((0xff & packetData[readPos++]) << 8 | (0xff & packetData[readPos++]));
return (short) ((0xff & packetData[readPos++]) | (0xff & packetData[readPos++]) << 8);
}
/**

29
photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java
View File

@@ -17,6 +17,8 @@
package org.photonvision.common.dataflow.structures;
import edu.wpi.first.util.struct.Struct;
public interface PacketSerde<T> {
int getMaxByteSize();
@@ -24,7 +26,32 @@ public interface PacketSerde<T> {
T unpack(Packet packet);
String getTypeString();
/** The name of this struct (eg "PhotonTrackedTarget") */
String getTypeName();
/**
* Gets the type string (e.g. for NetworkTables). This should be globally unique and start with
* "photonstruct:".
*
* @return type string
*/
default String getTypeString() {
return "photonstruct:" + getTypeName() + ":" + getInterfaceUUID();
}
/** Gets the list of photonstruct types referenced by this struct. */
default PacketSerde<?>[] getNestedPhotonMessages() {
return new PacketSerde<?>[] {};
}
/** Gets the list of WPILib struct types referenced by this struct. */
default Struct<?>[] getNestedWpilibMessages() {
return new Struct<?>[] {};
}
/** The schema definition, as defined in photon-serde/README.md */
String getSchema();
/** The hash of the schema string */
String getInterfaceUUID();
}

11
photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java
View File

@@ -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.IntegerTopic;
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<Transform3d> targetPoseEntry;
public DoublePublisher targetSkewEntry;
// The raw position of the best target, in pixels.
@@ -75,7 +77,10 @@ public class NTTopicSet {
var rawBytesEntry =
subTable
.getRawTopic("rawBytes")
.publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
.publish(
PhotonPipelineResult.photonStruct.getTypeString(),
PubSubOption.periodic(0.01),
PubSubOption.sendAll(true));
resultPublisher =
new PacketPublisher<PhotonPipelineResult>(rawBytesEntry, PhotonPipelineResult.photonStruct);
@@ -99,7 +104,7 @@ public class NTTopicSet {
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();

43
photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java
View File

@@ -20,6 +20,8 @@ package org.photonvision.common.networktables;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.networktables.RawPublisher;
import java.util.HashSet;
import java.util.Set;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
@@ -33,9 +35,6 @@ public class PacketPublisher<T> implements AutoCloseable {
var mapper = new ObjectMapper();
try {
this.publisher
.getTopic()
.setProperty("message_format", mapper.writeValueAsString(photonStruct.getTypeString()));
this.publisher
.getTopic()
.setProperty("message_uuid", mapper.writeValueAsString(photonStruct.getInterfaceUUID()));
@@ -44,12 +43,12 @@ public class PacketPublisher<T> implements AutoCloseable {
e.printStackTrace();
throw new RuntimeException(e);
}
addSchemaImpl(photonStruct, new HashSet<>());
}
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());
}
@@ -61,4 +60,40 @@ public class PacketPublisher<T> implements AutoCloseable {
public void close() {
publisher.close();
}
/**
* Publish the schema for our type (and all nested types under it) to NT.
*
* <p>Copyright (c) FIRST and other WPILib contributors. Open Source Software; you can modify
* and/or share it under the terms of the WPILib BSD license file in the root directory of this
* project.
*
* @param struct The struct to publish
* @param seen The set of types we've already published
*/
private void addSchemaImpl(PacketSerde<?> struct, Set<String> seen) {
var instance = this.publisher.getTopic().getInstance();
String typeString = struct.getTypeString();
if (instance.hasSchema(typeString)) {
return;
}
if (!seen.add(typeString)) {
throw new UnsupportedOperationException(typeString + ": circular reference with " + seen);
}
instance.addSchema(typeString, "photonstructschema", struct.getSchema());
for (var inner : struct.getNestedPhotonMessages()) {
System.out.println(inner.getTypeString());
addSchemaImpl(inner, seen);
}
for (var inner : struct.getNestedWpilibMessages()) {
System.out.println(inner.getTypeString());
instance.addSchema(inner);
}
seen.remove(typeString);
}
}

24
photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
View File

@@ -182,6 +182,8 @@ public class PhotonTrackedTarget
temp = Double.doubleToLongBits(skew);
result = prime * result + (int) (temp ^ (temp >>> 32));
result = prime * result + fiducialId;
result = prime * result + objDetectId;
result = prime * result + Float.floatToIntBits(objDetectConf);
result = prime * result + ((bestCameraToTarget == null) ? 0 : bestCameraToTarget.hashCode());
result = prime * result + ((altCameraToTarget == null) ? 0 : altCameraToTarget.hashCode());
temp = Double.doubleToLongBits(poseAmbiguity);
@@ -202,6 +204,9 @@ public class PhotonTrackedTarget
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 (objDetectId != other.objDetectId) return false;
if (Float.floatToIntBits(objDetectConf) != Float.floatToIntBits(other.objDetectConf))
return false;
if (bestCameraToTarget == null) {
if (other.bestCameraToTarget != null) return false;
} else if (!bestCameraToTarget.equals(other.bestCameraToTarget)) return false;
@@ -221,8 +226,7 @@ public class PhotonTrackedTarget
@Override
public String toString() {
return "PhotonTrackedTarget{"
+ "yaw="
return "PhotonTrackedTarget [yaw="
+ yaw
+ ", pitch="
+ pitch
@@ -232,11 +236,21 @@ public class PhotonTrackedTarget
+ skew
+ ", fiducialId="
+ fiducialId
+ ", cameraToTarget="
+ ", objDetectId="
+ objDetectId
+ ", objDetectConf="
+ objDetectConf
+ ", bestCameraToTarget="
+ bestCameraToTarget
+ ", targetCorners="
+ ", altCameraToTarget="
+ altCameraToTarget
+ ", poseAmbiguity="
+ poseAmbiguity
+ ", minAreaRectCorners="
+ minAreaRectCorners
+ '}';
+ ", detectedCorners="
+ detectedCorners
+ "]";
}
public static final PhotonTrackedTargetProto proto = new PhotonTrackedTargetProto();

4
photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java
View File

@@ -81,7 +81,9 @@ public class PhotonTrackedTargetProto
.setSkew(value.getSkew())
.setArea(value.getArea())
.setFiducialId(value.getFiducialId())
.setPoseAmbiguity(value.getPoseAmbiguity());
.setPoseAmbiguity(value.getPoseAmbiguity())
.setObjDetectionConf(value.getDetectedObjectConfidence())
.setObjDetectionId(value.getDetectedObjectClassID());
Transform3d.proto.pack(msg.getMutableBestCameraToTarget(), value.getBestCameraToTarget());
Transform3d.proto.pack(msg.getMutableAltCameraToTarget(), value.getAlternateCameraToTarget());