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

.github/workflows/lint-format.yml

@@ -37,7 +37,7 @@ jobs:
         with:
           python-version: 3.11
       - name: Install wpiformat
-        run: pip3 install wpiformat==2024.37
+        run: pip3 install wpiformat==2024.41
       - name: Run
         run: wpiformat
       - name: Check output

README.md

@@ -37,7 +37,7 @@ Note that these are case sensitive!
     * arm64
     * x86-64
     * x86
-- `-PtgtIp`: Specifies where `./gradlew deploy` should try to copy the fat JAR to
+- `-PtgtIP`: Specifies where `./gradlew deploy` should try to copy the fat JAR to
 - `-Pprofile`: enables JVM profiling
 
 ## Out-of-Source Dependencies

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);
         }

photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py

@@ -25,8 +25,8 @@
 
 class MultiTargetPNPResultSerde:
     # Message definition md5sum. See photon_packet.adoc for details
-    MESSAGE_VERSION = "ffc1cb847deb6e796a583a5b1885496b"
-    MESSAGE_FORMAT = "PnpResult estimatedPose;int16[?] fiducialIDsUsed;"
+    MESSAGE_VERSION = "541096947e9f3ca2d3f425ff7b04aa7b"
+    MESSAGE_FORMAT = "PnpResult:ae4d655c0a3104d88df4f5db144c1e86 estimatedPose;int16 fiducialIDsUsed[?];"
 
     @staticmethod
     def unpack(packet: "Packet") -> "MultiTargetPNPResult":

photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py

@@ -25,7 +25,7 @@
 
 class PhotonPipelineMetadataSerde:
     # Message definition md5sum. See photon_packet.adoc for details
-    MESSAGE_VERSION = "2a7039527bda14d13028a1b9282d40a2"
+    MESSAGE_VERSION = "626e70461cbdb274fb43ead09c255f4e"
     MESSAGE_FORMAT = (
         "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;"
     )

photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py

@@ -25,8 +25,8 @@
 
 class PhotonPipelineResultSerde:
     # Message definition md5sum. See photon_packet.adoc for details
-    MESSAGE_VERSION = "cb3e1605048ba49325888eb797399fe2"
-    MESSAGE_FORMAT = "PhotonPipelineMetadata metadata;PhotonTrackedTarget[?] targets;MultiTargetPNPResult? multiTagResult;"
+    MESSAGE_VERSION = "5eeaa293d0c69aea90eaddea786a2b3b"
+    MESSAGE_FORMAT = "PhotonPipelineMetadata:626e70461cbdb274fb43ead09c255f4e metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"
 
     @staticmethod
     def unpack(packet: "Packet") -> "PhotonPipelineResult":
@@ -38,8 +38,8 @@ def unpack(packet: "Packet") -> "PhotonPipelineResult":
         # targets is a custom VLA!
         ret.targets = packet.decodeList(PhotonTrackedTarget.photonStruct)
 
-        # multiTagResult is optional! it better not be a VLA too
-        ret.multiTagResult = packet.decodeOptional(MultiTargetPNPResult.photonStruct)
+        # multitagResult is optional! it better not be a VLA too
+        ret.multitagResult = packet.decodeOptional(MultiTargetPNPResult.photonStruct)
 
         return ret
 

photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py

@@ -25,8 +25,8 @@
 
 class PhotonTrackedTargetSerde:
     # Message definition md5sum. See photon_packet.adoc for details
-    MESSAGE_VERSION = "8fdada56b9162f2e32bd24f0055d7b60"
-    MESSAGE_FORMAT = "float64 yaw;float64 pitch;float64 area;float64 skew;int32 fiducialId;int32 objDetectId;float32 objDetectConf;Transform3d bestCameraToTarget;Transform3d altCameraToTarget;float64 poseAmbiguity;TargetCorner[?] minAreaRectCorners;TargetCorner[?] detectedCorners;"
+    MESSAGE_VERSION = "cc6dbb5c5c1e0fa808108019b20863f1"
+    MESSAGE_FORMAT = "float64 yaw;float64 pitch;float64 area;float64 skew;int32 fiducialId;int32 objDetectId;float32 objDetectConf;Transform3d bestCameraToTarget;Transform3d altCameraToTarget;float64 poseAmbiguity;TargetCorner:16f6ac0dedc8eaccb951f4895d9e18b6 minAreaRectCorners[?];TargetCorner:16f6ac0dedc8eaccb951f4895d9e18b6 detectedCorners[?];"
 
     @staticmethod
     def unpack(packet: "Packet") -> "PhotonTrackedTarget":
@@ -53,10 +53,8 @@ def unpack(packet: "Packet") -> "PhotonTrackedTarget":
         # objDetectConf is of intrinsic type float32
         ret.objDetectConf = packet.decodeFloat()
 
-        # field is shimmed!
         ret.bestCameraToTarget = packet.decodeTransform()
 
-        # field is shimmed!
         ret.altCameraToTarget = packet.decodeTransform()
 
         # poseAmbiguity is of intrinsic type float64

photon-lib/py/photonlibpy/generated/PnpResultSerde.py

@@ -25,17 +25,15 @@
 
 class PnpResultSerde:
     # Message definition md5sum. See photon_packet.adoc for details
-    MESSAGE_VERSION = "0d1f2546b00f24718e30f38d206d4491"
+    MESSAGE_VERSION = "ae4d655c0a3104d88df4f5db144c1e86"
     MESSAGE_FORMAT = "Transform3d best;Transform3d alt;float64 bestReprojErr;float64 altReprojErr;float64 ambiguity;"
 
     @staticmethod
     def unpack(packet: "Packet") -> "PnpResult":
         ret = PnpResult()
 
-        # field is shimmed!
         ret.best = packet.decodeTransform()
 
-        # field is shimmed!
         ret.alt = packet.decodeTransform()
 
         # bestReprojErr is of intrinsic type float64

photon-lib/py/photonlibpy/generated/TargetCornerSerde.py

@@ -25,7 +25,7 @@
 
 class TargetCornerSerde:
     # Message definition md5sum. See photon_packet.adoc for details
-    MESSAGE_VERSION = "22b1ff7551d10215af6fb3672fe4eda8"
+    MESSAGE_VERSION = "16f6ac0dedc8eaccb951f4895d9e18b6"
     MESSAGE_FORMAT = "float64 x;float64 y;"
 
     @staticmethod

photon-lib/py/photonlibpy/packet.py

@@ -85,7 +85,9 @@ def _decodeGeneric(self, unpackFormat, numBytes):
         for _ in range(numBytes):
             intList.append(self._getNextByteAsInt())
 
-        # Interpret the bytes as a floating point number
+        # Interpret the bytes as the requested type.
+        # Note due to NT's byte order assumptions,
+        # we have to flip the order of intList
         value = struct.unpack(unpackFormat, bytes(intList))[0]
 
         return value
@@ -96,47 +98,47 @@ def decode8(self) -> int:
         *
         * @return A decoded byte from the packet.
         """
-        return self._decodeGeneric(">b", 1)
+        return self._decodeGeneric("<b", 1)
 
     def decode16(self) -> int:
         """
         * Returns a single decoded short from the packet.
         *
         * @return A decoded short from the packet.
         """
-        return self._decodeGeneric(">h", 2)
+        return self._decodeGeneric("<h", 2)
 
     def decodeInt(self) -> int:
         """
         * Returns a decoded int (32 bytes) from the packet.
         *
         * @return A decoded int from the packet.
         """
-        return self._decodeGeneric(">l", 4)
+        return self._decodeGeneric("<l", 4)
 
     def decodeFloat(self) -> float:
         """
         * Returns a decoded float from the packet.
         *
         * @return A decoded float from the packet.
         """
-        return self._decodeGeneric(">f", 4)
+        return self._decodeGeneric("<f", 4)
 
     def decodeLong(self) -> int:
         """
         * Returns a decoded int64 from the packet.
         *
         * @return A decoded int64 from the packet.
         """
-        return self._decodeGeneric(">q", 8)
+        return self._decodeGeneric("<q", 8)
 
     def decodeDouble(self) -> float:
         """
         * Returns a decoded double from the packet.
         *
         * @return A decoded double from the packet.
         """
-        return self._decodeGeneric(">d", 8)
+        return self._decodeGeneric("<d", 8)
 
     def decodeBoolean(self) -> bool:
         """

photon-lib/py/photonlibpy/photonCamera.py

@@ -225,7 +225,15 @@ def _versionCheck(self) -> None:
             )
 
         versionString = self.versionEntry.get(defaultValue="")
-        if len(versionString) > 0 and versionString != PHOTONVISION_VERSION:
+        localUUID = PhotonPipelineResult.photonStruct.MESSAGE_VERSION
+        remoteUUID = self._rawBytesEntry.getTopic().getProperty("message_uuid")
+
+        if remoteUUID is None or len(remoteUUID) == 0:
+            wpilib.reportWarning(
+                f"PhotonVision coprocessor at path {self._path} has not reported a message interface UUID - is your coprocessor's camera started?",
+                True,
+            )
+        elif localUUID != remoteUUID:
             # Verified version mismatch
 
             bfw = """
@@ -250,6 +258,6 @@ def _versionCheck(self) -> None:
 
             wpilib.reportWarning(bfw)
 
-            errText = f"Photon version {PHOTONLIB_VERSION} does not match coprocessor version {versionString}. Please install photonlibpy version {versionString}, or update your coprocessor to {PHOTONLIB_VERSION}."
+            errText = f"Photonlibpy version {PHOTONLIB_VERSION} (With message UUID {localUUID}) does not match coprocessor version {versionString} (with message UUID {remoteUUID}). Please install photonlibpy version {versionString}, or update your coprocessor to {PHOTONLIB_VERSION}."
             wpilib.reportError(errText, True)
             raise Exception(errText)

photon-lib/src/main/java/org/photonvision/PhotonCamera.java

@@ -422,7 +422,7 @@ else if (!isConnected()) {
             DriverStation.reportWarning(
                     "PhotonVision coprocessor at path "
                             + path
-                            + " has note reported a message interface UUID - is your coprocessor's camera started?",
+                            + " has not reported a message interface UUID - is your coprocessor's camera started?",
                     true);
         } else if (!local_uuid.equals(remote_uuid)) {
             // Error on a verified version mismatch

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);

photon-serde/README.md

@@ -22,3 +22,61 @@ The code for a single type is split across 3 files. Let's look at PnpResult:
 - Protobuf: slow on embedded platforms (at least quickbuf is)
 - Wpi's struct: no VLAs/optionals
 - Rosmsg: I'm not using ros, but I'm stealing their message hash idea
+
+## Deviations from WPI's Struct Schema Typestrings
+
+- Enum types are disallowed
+- Bitfields and bit packing are disallowed
+- Only variable length arrays are supported (no fixed-length arrays)
+- Arrays must be no more than 127 elements long
+- Members can be either VLAs or optional, but not both
+- A top-level NT topic type shall be a single type (eg TargetCorner), and cannot an array of types (eg TargetCorner[] or TargetCorner[?])
+- `float` and `double` types will be replaced with float32/float64 when generating message schema strings. This means that `float32 x;` and `float x;` will result in the same message hash.
+
+For example, this is a valid PhotonStruct schema. Note the WPILib `Transform3d`, the Photon-defined `TargetCorner`, optional prefix, and VLA suffix.
+
+```
+float64 poseAmbiguity;
+optional Transform3d altCameraToTarget;
+TargetCorner:16f6ac0dedc8eaccb951f4895d9e18b6 minAreaRectCorners[?];
+```
+
+## Dynamic Decoding
+
+Dynamic decoding is facilitated by publishing schemas to the `.schema` table in NT, and by encoding the `message_uuid` as a property on a `photonstruct` publisher. Schema names in the .schema table shall be formatted as `photonstruct:{Type Name}:{Message UUID}`. For example, here I've published Photon results to `/photonvision/WPI2024/rawBytes`. This topic has the typestring `photonstruct:PhotonPipelineResult:ed36092eb95e9fc254ebac897e2a74df`, with properties `{message_uuid': 'ed36092eb95e9fc254ebac897e2a74df'}`. It shall be legal to have published multiple versions of the same message, as long as their UUIDs are unique (which they'd better be).
+
+| Topic Name | Type | Type String |
+|------|------|-------|
+| /.schema/photonstruct:PhotonPipelineResult:ed36092eb95e9fc254ebac897e2a74df   | kRaw |  photonstructschema |
+| /.schema/photonstruct:PhotonTrackedTarget:4387ab389a8a78b7beb4492f145831b4    | kRaw |  photonstructschema |
+| /.schema/photonstruct:TargetCorner:16f6ac0dedc8eaccb951f4895d9e18b6           | kRaw |  photonstructschema |
+| /.schema/photonstruct:MultiTargetPNPResult:af2056aaab740eeb889a926071cae6ee   | kRaw |  photonstructschema |
+| /.schema/photonstruct:PnpResult:ae4d655c0a3104d88df4f5db144c1e86              | kRaw |  photonstructschema |
+| /.schema/photonstruct:PhotonPipelineMetadata:626e70461cbdb274fb43ead09c255f4e | kRaw |  photonstructschema |
+| /.schema/proto:geometry3d.proto                                               | kRaw  |  proto:FileDescriptorProto |
+| /.schema/proto:photon.proto    | kRaw | proto:FileDescriptorProto |
+
+The struct definition for PhotonPipelineResult we retrieved from the struct schema database shown above (via the command `python.exe scripts/catnt.py --echo /.schema/photonstruct:PhotonPipelineResult:ed36092eb95e9fc254ebac897e2a74df`) is:
+
+```
+PhotonPipelineMetadata:626e70461cbdb274fb43ead09c255f4e metadata;
+PhotonTrackedTarget:4387ab389a8a78b7beb4492f145831b4[?] targets;
+MultiTargetPNPResult:af2056aaab740eeb889a926071cae6ee? multitagResult;
+```
+
+If we were decoding this, we'd go retrieve the struct definitions for all our nested types. For example, `PhotonTrackedTarget:4387ab389a8a78b7beb4492f145831b4` is defined by it's .schema table entry be the following. This type also demonstrates a mix of WPILib struct types (such as Transform3d), intrinsic types (such as float64), and Photon struct types (such as TargetCorner).
+
+```
+float64 yaw;
+float64 pitch;
+float64 area;
+float64 skew;
+int32 fiducialId;
+int32 objDetectId;
+float32 objDetectConf;
+Transform3d bestCameraToTarget;
+Transform3d altCameraToTarget;
+float64 poseAmbiguity;
+TargetCorner:16f6ac0dedc8eaccb951f4895d9e18b6[?] minAreaRectCorners;
+TargetCorner:16f6ac0dedc8eaccb951f4895d9e18b6[?] detectedCorners;
+```