Add message UUID and type names to hash and message defintion

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

@@ -26,8 +26,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

@@ -26,7 +26,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

@@ -26,8 +26,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":
@@ -39,8 +39,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

@@ -26,8 +26,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":
@@ -54,10 +54,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

@@ -26,17 +26,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

@@ -26,7 +26,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/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
+
+## Deviatioons from WPI's Struct
+
+- 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;
+```

photon-serde/generate_messages.py

@@ -47,6 +47,11 @@ class MessageType(TypedDict):
     cpp_include: str
     # python shim types
     python_decode_shim: str
+    # Java import name
+    java_import: str
+    # Remember our message hash. Recalculated by us. All intrinsic types are unhashed so this is fine to live here
+    message_hash: str
+    schema_str: str
 
 
 def yaml_to_dict(path: str):
@@ -118,7 +123,7 @@ def get_field_by_name(message: MessageType, field_name: str):
     return next(f for f in message["fields"] if f["name"] == field_name)
 
 
-def get_message_hash(message_db: List[MessageType], message: MessageType):
+def get_message_hash(message_db: List[MessageType], message: MessageType) -> str:
     """
     Calculate a unique message hash via MD5 sum. This is a very similar approach to rosmsg, documented:
     http://wiki.ros.org/ROS/Technical%20Overview#Message_serialization_and_msg_MD5_sums
@@ -136,15 +141,15 @@ def get_message_hash(message_db: List[MessageType], message: MessageType):
 
     for field in fields_to_hash:
         sub_message = get_message_by_name(message_db, field["type"])
-        subhash = get_message_hash(message_db, sub_message)
+        get_message_hash(message_db, sub_message)
 
-        # change the type to be our new md5sum
-        field["type"] = subhash.hexdigest()
+    schema = get_struct_schema_str(message, message_db)
+    message_hash = hashlib.md5(schema.encode("ascii")).hexdigest()
+
+    # and remember the hash
+    message["message_hash"] = message_hash
+    message["schema_str"] = schema
 
-    # base case: message is all intrinsic types
-    # Hash a comments-stripped version for message integrity checking
-    cleaned_yaml = yaml.dump(modified_message, default_flow_style=False).strip()
-    message_hash = hashlib.md5(cleaned_yaml.encode("ascii"))
     return message_hash
 
 
@@ -171,30 +176,75 @@ def get_includes(db, message: MessageType) -> str:
     return sorted(set(includes))
 
 
-def parse_yaml():
-    Path(__file__).resolve().parent
+def parse_yaml() -> List[MessageType]:
     config = yaml_to_dict("messages.yaml")
 
     return config
 
 
-def get_struct_schema_str(message: MessageType):
+INTRINSIC_TYPE_ALIASES = {
+    "float": "float32",
+    "double": "float64",
+}
+
+
+def get_fully_defined_field_name(field: SerdeField, message_db: List[MessageType]):
+    """
+    Get the fully-defined, globally unique type name for a field. Returns something like
+    Transform3d:b290703ff9e54f9ec2c733b90d7fc30b for user-defined types, or just
+    something like int64 for built-in types. Also normalizes float/double to float32/float64
+
+    Args:
+        field: The field we want the name of
+        message_db: All other loaded messages
+    """
+
+    typestr = field["type"]
+    if not is_intrinsic_type(field["type"]):
+        msg = get_message_by_name(message_db, field["type"])
+        is_shimmed = get_shimmed_filter(message_db)(field["type"])
+        if not is_shimmed:
+            typestr = field["type"] + ":" + msg["message_hash"]
+    else:
+        # handle replacing float/doubles
+        typestr = field["type"]
+        typestr = INTRINSIC_TYPE_ALIASES.get(typestr, typestr)
+
+    return typestr
+
+
+def get_struct_schema_str(message: MessageType, message_db: List[MessageType]):
     ret = ""
 
     for field in message["fields"]:
-        typestr = field["type"]
+        if (
+            "optional" in field
+            and field["optional"] == True
+            and "vla" in field
+            and field["vla"] == True
+        ):
+            raise Exception(f"Field {field} must be optional OR vla!")
+
+        typestr = get_fully_defined_field_name(field, message_db)
+
+        array_modifier = ""
+
         if "optional" in field and field["optional"] == True:
-            typestr += "?"
+            typestr = "optional " + typestr
         if "vla" in field and field["vla"] == True:
-            typestr += "[?]"
-        ret += f"{typestr} {field['name']};"
+            array_modifier = "[?]"
+
+        ret += f"{typestr} {field['name']}{array_modifier};"
 
     return ret
 
 
 def generate_photon_messages(cpp_java_root, py_root, template_root):
     messages = parse_yaml()
 
+    for message in messages:
+        message["message_hash"] = get_message_hash(messages, message)
+
     env = Environment(
         loader=FileSystemLoader(str(template_root)),
         # autoescape=False,
@@ -267,14 +317,37 @@ def generate_photon_messages(cpp_java_root, py_root, template_root):
                 messages, name
             )
 
+            nested_photon_types = set(
+                [
+                    field["type"]
+                    for field in message["fields"]
+                    if (
+                        not is_intrinsic_type(field["type"])
+                        and not get_shimmed_filter(messages)(field["type"])
+                    )
+                ]
+            )
+            nested_wpilib_types = set(
+                [
+                    field["type"]
+                    for field in message["fields"]
+                    if (
+                        not is_intrinsic_type(field["type"])
+                        and get_shimmed_filter(messages)(field["type"])
+                    )
+                ]
+            )
+
             output_file = output_folder / output_name
             output_file.write_text(
                 template.render(
                     message,
                     type_map=extended_data_types,
-                    message_fmt=get_struct_schema_str(message),
-                    message_hash=message_hash.hexdigest(),
+                    message_fmt=get_struct_schema_str(message, messages),
+                    message_hash=message_hash,
                     cpp_includes=get_includes(messages, message),
+                    nested_photon_types=nested_photon_types,
+                    nested_wpilib_types=nested_wpilib_types,
                 ),
                 encoding="utf-8",
             )

photon-serde/messages.yaml

@@ -15,6 +15,7 @@
   cpp_type: frc::Transform3d
   cpp_include: "<frc/geometry/Transform3d.h>"
   python_decode_shim: packet.decodeTransform
+  java_import: edu.wpi.first.math.geometry.Transform3d
   # shim since we expect fields to at least exist
   fields: []