diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index fb97eb77af..09f86fc6c6 100644 --- a/.github/workflows/lint-format.yml +++ b/.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 diff --git a/README.md b/README.md index 4fcf69c59e..c559bb963c 100644 --- a/README.md +++ b/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 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/py/photonlibpy/generated/MultiTargetPNPResultSerde.py b/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py index a40d07fe44..a1bf4c4090 100644 --- a/photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py +++ b/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": diff --git a/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py b/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py index b5ce2d8a9c..f0e8731f33 100644 --- a/photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py +++ b/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;" ) diff --git a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py b/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py index f638b3bb17..fa32c9bdd2 100644 --- a/photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py +++ b/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 diff --git a/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py b/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py index c728295c6b..4765fa5e37 100644 --- a/photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py +++ b/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 diff --git a/photon-lib/py/photonlibpy/generated/PnpResultSerde.py b/photon-lib/py/photonlibpy/generated/PnpResultSerde.py index aaeb74c86c..e4c1f261ea 100644 --- a/photon-lib/py/photonlibpy/generated/PnpResultSerde.py +++ b/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 diff --git a/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py b/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py index beccc9e2db..f55207f9be 100644 --- a/photon-lib/py/photonlibpy/generated/TargetCornerSerde.py +++ b/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 diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py index 53c3f84c5b..a4e375d431 100644 --- a/photon-lib/py/photonlibpy/packet.py +++ b/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,7 +98,7 @@ def decode8(self) -> int: * * @return A decoded byte from the packet. """ - return self._decodeGeneric(">b", 1) + return self._decodeGeneric(" int: """ @@ -104,7 +106,7 @@ def decode16(self) -> int: * * @return A decoded short from the packet. """ - return self._decodeGeneric(">h", 2) + return self._decodeGeneric(" int: """ @@ -112,7 +114,7 @@ def decodeInt(self) -> int: * * @return A decoded int from the packet. """ - return self._decodeGeneric(">l", 4) + return self._decodeGeneric(" float: """ @@ -120,7 +122,7 @@ def decodeFloat(self) -> float: * * @return A decoded float from the packet. """ - return self._decodeGeneric(">f", 4) + return self._decodeGeneric(" int: """ @@ -128,7 +130,7 @@ def decodeLong(self) -> int: * * @return A decoded int64 from the packet. """ - return self._decodeGeneric(">q", 8) + return self._decodeGeneric(" float: """ @@ -136,7 +138,7 @@ def decodeDouble(self) -> float: * * @return A decoded double from the packet. """ - return self._decodeGeneric(">d", 8) + return self._decodeGeneric(" bool: """ diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index 32f337b20d..b5bb659e88 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/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) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 1130d20a0a..633cf5fcbb 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/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 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-serde/README.md b/photon-serde/README.md index 71ec72a52a..3e57626e91 100644 --- a/photon-serde/README.md +++ b/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; +``` diff --git a/photon-serde/generate_messages.py b/photon-serde/generate_messages.py index fe2b16277d..2b4922dc0a 100644 --- a/photon-serde/generate_messages.py +++ b/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,23 +176,65 @@ 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 @@ -195,6 +242,9 @@ def get_struct_schema_str(message: MessageType): 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", ) diff --git a/photon-serde/messages.yaml b/photon-serde/messages.yaml index 2eac0b4100..d7121c36cf 100644 --- a/photon-serde/messages.yaml +++ b/photon-serde/messages.yaml @@ -15,6 +15,7 @@ cpp_type: frc::Transform3d cpp_include: "" python_decode_shim: packet.decodeTransform + java_import: edu.wpi.first.math.geometry.Transform3d # shim since we expect fields to at least exist fields: [] diff --git a/photon-serde/templates/Message.java.jinja b/photon-serde/templates/Message.java.jinja index bf47b4eaf3..a7975295d5 100644 --- a/photon-serde/templates/Message.java.jinja +++ b/photon-serde/templates/Message.java.jinja @@ -26,17 +26,24 @@ import org.photonvision.utils.PacketUtils; // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// WPILib imports (if any) +import edu.wpi.first.util.struct.Struct; +{% for type in nested_wpilib_types -%} +import {{ get_message_by_name(type).java_import }}; +{%- if not loop.last %},{% endif -%} +{%- endfor%} /** * Auto-generated serialization/deserialization helper for {{name}} */ public class {{ name }}Serde implements PacketSerde<{{name}}> { - // Message definition md5sum. See photon_packet.adoc for details - public static final String MESSAGE_VERSION = "{{ message_hash }}"; - public static final String MESSAGE_FORMAT = "{{ message_fmt }}"; - public final String getTypeString() { return MESSAGE_FORMAT; } - public final String getInterfaceUUID() { return MESSAGE_VERSION; } + @Override + public final String getInterfaceUUID() { return "{{ message_hash }}"; } + @Override + public final String getSchema() { return "{{ message_fmt }}"; } + @Override + public final String getTypeName() { return "{{ name }}"; } @Override public int getMaxByteSize() { @@ -48,7 +55,6 @@ public class {{ name }}Serde implements PacketSerde<{{name}}> { public void pack(Packet packet, {{ name }} value) { {%- for field in fields -%} {%- if field.type | is_shimmed %} - // field is shimmed! {{ get_message_by_name(field.type).java_encode_shim }}(packet, value.{{ field.name }}); {%- elif field.optional == True %} // {{ field.name }} is optional! it better not be a VLA too @@ -76,7 +82,6 @@ public class {{ name }}Serde implements PacketSerde<{{name}}> { var ret = new {{ name }}(); {% for field in fields -%} {%- if field.type | is_shimmed %} - // field is shimmed! ret.{{ field.name }} = {{ get_message_by_name(field.type).java_decode_shim }}(packet); {%- elif field.optional == True %} // {{ field.name }} is optional! it better not be a VLA too @@ -100,4 +105,24 @@ public class {{ name }}Serde implements PacketSerde<{{name}}> { return ret; } + + @Override + public PacketSerde[] getNestedPhotonMessages() { + return new PacketSerde[] { + {% for type in nested_photon_types -%} + {{ type }}.photonStruct + {%- if not loop.last %},{% endif -%} + {%- endfor%} + }; + } + + @Override + public Struct[] getNestedWpilibMessages() { + return new Struct[] { + {% for type in nested_wpilib_types -%} + {{ type }}.struct + {%- if not loop.last %},{% endif -%} + {%- endfor%} + }; + } } diff --git a/photon-serde/templates/ThingSerde.py.jinja b/photon-serde/templates/ThingSerde.py.jinja index 3282cb51e8..335adc038c 100644 --- a/photon-serde/templates/ThingSerde.py.jinja +++ b/photon-serde/templates/ThingSerde.py.jinja @@ -33,7 +33,6 @@ class {{ name }}Serde: ret = {{ name }}() {% for field in fields -%} {%- if field.type | is_shimmed %} - # field is shimmed! ret.{{ field.name }} = {{ get_message_by_name(field.type).python_decode_shim }}() {%- elif field.optional == True %} # {{ field.name }} is optional! it better not be a VLA too diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java index 961f465a43..1be3cf633d 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java @@ -26,17 +26,20 @@ // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// WPILib imports (if any) +import edu.wpi.first.util.struct.Struct; + /** * Auto-generated serialization/deserialization helper for MultiTargetPNPResult */ public class MultiTargetPNPResultSerde implements PacketSerde { - // Message definition md5sum. See photon_packet.adoc for details - public static final String MESSAGE_VERSION = "ffc1cb847deb6e796a583a5b1885496b"; - public static final String MESSAGE_FORMAT = "PnpResult estimatedPose;int16[?] fiducialIDsUsed;"; - - public final String getTypeString() { return MESSAGE_FORMAT; } - public final String getInterfaceUUID() { return MESSAGE_VERSION; } + @Override + public final String getInterfaceUUID() { return "541096947e9f3ca2d3f425ff7b04aa7b"; } + @Override + public final String getSchema() { return "PnpResult:ae4d655c0a3104d88df4f5db144c1e86 estimatedPose;int16 fiducialIDsUsed[?];"; } + @Override + public final String getTypeName() { return "MultiTargetPNPResult"; } @Override public int getMaxByteSize() { @@ -65,4 +68,17 @@ public MultiTargetPNPResult unpack(Packet packet) { return ret; } + + @Override + public PacketSerde[] getNestedPhotonMessages() { + return new PacketSerde[] { + PnpResult.photonStruct + }; + } + + @Override + public Struct[] getNestedWpilibMessages() { + return new Struct[] { + }; + } } diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java index 1ba176d809..fb87bea148 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java @@ -26,17 +26,20 @@ // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// WPILib imports (if any) +import edu.wpi.first.util.struct.Struct; + /** * Auto-generated serialization/deserialization helper for PhotonPipelineMetadata */ public class PhotonPipelineMetadataSerde implements PacketSerde { - // Message definition md5sum. See photon_packet.adoc for details - public static final String MESSAGE_VERSION = "2a7039527bda14d13028a1b9282d40a2"; - public static final String MESSAGE_FORMAT = "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;"; - - public final String getTypeString() { return MESSAGE_FORMAT; } - public final String getInterfaceUUID() { return MESSAGE_VERSION; } + @Override + public final String getInterfaceUUID() { return "626e70461cbdb274fb43ead09c255f4e"; } + @Override + public final String getSchema() { return "int64 sequenceID;int64 captureTimestampMicros;int64 publishTimestampMicros;"; } + @Override + public final String getTypeName() { return "PhotonPipelineMetadata"; } @Override public int getMaxByteSize() { @@ -71,4 +74,16 @@ public PhotonPipelineMetadata unpack(Packet packet) { return ret; } + + @Override + public PacketSerde[] getNestedPhotonMessages() { + return new PacketSerde[] { + }; + } + + @Override + public Struct[] getNestedWpilibMessages() { + return new Struct[] { + }; + } } diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java index 21e54ad470..edd17aba43 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java @@ -26,17 +26,20 @@ // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// WPILib imports (if any) +import edu.wpi.first.util.struct.Struct; + /** * Auto-generated serialization/deserialization helper for PhotonPipelineResult */ public class PhotonPipelineResultSerde implements PacketSerde { - // Message definition md5sum. See photon_packet.adoc for details - public static final String MESSAGE_VERSION = "cb3e1605048ba49325888eb797399fe2"; - public static final String MESSAGE_FORMAT = "PhotonPipelineMetadata metadata;PhotonTrackedTarget[?] targets;MultiTargetPNPResult? multitagResult;"; - - public final String getTypeString() { return MESSAGE_FORMAT; } - public final String getInterfaceUUID() { return MESSAGE_VERSION; } + @Override + public final String getInterfaceUUID() { return "5eeaa293d0c69aea90eaddea786a2b3b"; } + @Override + public final String getSchema() { return "PhotonPipelineMetadata:626e70461cbdb274fb43ead09c255f4e metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"; } + @Override + public final String getTypeName() { return "PhotonPipelineResult"; } @Override public int getMaxByteSize() { @@ -71,4 +74,17 @@ public PhotonPipelineResult unpack(Packet packet) { return ret; } + + @Override + public PacketSerde[] getNestedPhotonMessages() { + return new PacketSerde[] { + PhotonPipelineMetadata.photonStruct,MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct + }; + } + + @Override + public Struct[] getNestedWpilibMessages() { + return new Struct[] { + }; + } } diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java index d07482017a..0c2beef2c3 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java @@ -26,17 +26,20 @@ // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// WPILib imports (if any) +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.math.geometry.Transform3d; /** * Auto-generated serialization/deserialization helper for PhotonTrackedTarget */ public class PhotonTrackedTargetSerde implements PacketSerde { - // Message definition md5sum. See photon_packet.adoc for details - public static final String MESSAGE_VERSION = "8fdada56b9162f2e32bd24f0055d7b60"; - public static final String 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;"; - - public final String getTypeString() { return MESSAGE_FORMAT; } - public final String getInterfaceUUID() { return MESSAGE_VERSION; } + @Override + public final String getInterfaceUUID() { return "cc6dbb5c5c1e0fa808108019b20863f1"; } + @Override + public final String getSchema() { return "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[?];"; } + @Override + public final String getTypeName() { return "PhotonTrackedTarget"; } @Override public int getMaxByteSize() { @@ -67,10 +70,8 @@ public void pack(Packet packet, PhotonTrackedTarget value) { // field objDetectConf is of intrinsic type float32 packet.encode((float) value.objDetectConf); - // field is shimmed! PacketUtils.packTransform3d(packet, value.bestCameraToTarget); - // field is shimmed! PacketUtils.packTransform3d(packet, value.altCameraToTarget); // field poseAmbiguity is of intrinsic type float64 @@ -108,10 +109,8 @@ public PhotonTrackedTarget unpack(Packet packet) { // objDetectConf is of intrinsic type float32 ret.objDetectConf = packet.decodeFloat(); - // field is shimmed! ret.bestCameraToTarget = PacketUtils.unpackTransform3d(packet); - // field is shimmed! ret.altCameraToTarget = PacketUtils.unpackTransform3d(packet); // poseAmbiguity is of intrinsic type float64 @@ -125,4 +124,18 @@ public PhotonTrackedTarget unpack(Packet packet) { return ret; } + + @Override + public PacketSerde[] getNestedPhotonMessages() { + return new PacketSerde[] { + TargetCorner.photonStruct + }; + } + + @Override + public Struct[] getNestedWpilibMessages() { + return new Struct[] { + Transform3d.struct + }; + } } diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java index a0c90f0f3a..b9c7e695b3 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java @@ -26,17 +26,20 @@ // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// WPILib imports (if any) +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.math.geometry.Transform3d; /** * Auto-generated serialization/deserialization helper for PnpResult */ public class PnpResultSerde implements PacketSerde { - // Message definition md5sum. See photon_packet.adoc for details - public static final String MESSAGE_VERSION = "0d1f2546b00f24718e30f38d206d4491"; - public static final String MESSAGE_FORMAT = "Transform3d best;Transform3d alt;float64 bestReprojErr;float64 altReprojErr;float64 ambiguity;"; - - public final String getTypeString() { return MESSAGE_FORMAT; } - public final String getInterfaceUUID() { return MESSAGE_VERSION; } + @Override + public final String getInterfaceUUID() { return "ae4d655c0a3104d88df4f5db144c1e86"; } + @Override + public final String getSchema() { return "Transform3d best;Transform3d alt;float64 bestReprojErr;float64 altReprojErr;float64 ambiguity;"; } + @Override + public final String getTypeName() { return "PnpResult"; } @Override public int getMaxByteSize() { @@ -46,10 +49,8 @@ public int getMaxByteSize() { @Override public void pack(Packet packet, PnpResult value) { - // field is shimmed! PacketUtils.packTransform3d(packet, value.best); - // field is shimmed! PacketUtils.packTransform3d(packet, value.alt); // field bestReprojErr is of intrinsic type float64 @@ -66,10 +67,8 @@ public void pack(Packet packet, PnpResult value) { public PnpResult unpack(Packet packet) { var ret = new PnpResult(); - // field is shimmed! ret.best = PacketUtils.unpackTransform3d(packet); - // field is shimmed! ret.alt = PacketUtils.unpackTransform3d(packet); // bestReprojErr is of intrinsic type float64 @@ -83,4 +82,17 @@ public PnpResult unpack(Packet packet) { return ret; } + + @Override + public PacketSerde[] getNestedPhotonMessages() { + return new PacketSerde[] { + }; + } + + @Override + public Struct[] getNestedWpilibMessages() { + return new Struct[] { + Transform3d.struct + }; + } } diff --git a/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java b/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java index 8c5d2a637d..64f190bf76 100644 --- a/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java @@ -26,17 +26,20 @@ // Assume that the base class lives here and we can import it import org.photonvision.targeting.*; +// WPILib imports (if any) +import edu.wpi.first.util.struct.Struct; + /** * Auto-generated serialization/deserialization helper for TargetCorner */ public class TargetCornerSerde implements PacketSerde { - // Message definition md5sum. See photon_packet.adoc for details - public static final String MESSAGE_VERSION = "22b1ff7551d10215af6fb3672fe4eda8"; - public static final String MESSAGE_FORMAT = "float64 x;float64 y;"; - - public final String getTypeString() { return MESSAGE_FORMAT; } - public final String getInterfaceUUID() { return MESSAGE_VERSION; } + @Override + public final String getInterfaceUUID() { return "16f6ac0dedc8eaccb951f4895d9e18b6"; } + @Override + public final String getSchema() { return "float64 x;float64 y;"; } + @Override + public final String getTypeName() { return "TargetCorner"; } @Override public int getMaxByteSize() { @@ -65,4 +68,16 @@ public TargetCorner unpack(Packet packet) { return ret; } + + @Override + public PacketSerde[] getNestedPhotonMessages() { + return new PacketSerde[] { + }; + } + + @Override + public Struct[] getNestedWpilibMessages() { + return new Struct[] { + }; + } } diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/MultiTargetPNPResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/MultiTargetPNPResultSerde.h index 7a75354baf..2fc1a5f0c7 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/MultiTargetPNPResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/MultiTargetPNPResultSerde.h @@ -36,11 +36,12 @@ namespace photon { template <> struct WPILIB_DLLEXPORT SerdeType { static constexpr std::string_view GetSchemaHash() { - return "ffc1cb847deb6e796a583a5b1885496b"; + return "541096947e9f3ca2d3f425ff7b04aa7b"; } static constexpr std::string_view GetSchema() { - return "PnpResult estimatedPose;int16[?] fiducialIDsUsed;"; + return "PnpResult:ae4d655c0a3104d88df4f5db144c1e86 estimatedPose;int16 " + "fiducialIDsUsed[?];"; } static photon::MultiTargetPNPResult Unpack(photon::Packet& packet); diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h index d3883605a0..ecdc882379 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h @@ -34,7 +34,7 @@ namespace photon { template <> struct WPILIB_DLLEXPORT SerdeType { static constexpr std::string_view GetSchemaHash() { - return "2a7039527bda14d13028a1b9282d40a2"; + return "626e70461cbdb274fb43ead09c255f4e"; } static constexpr std::string_view GetSchema() { diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h index b7872d067b..49845033cb 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h @@ -39,12 +39,15 @@ namespace photon { template <> struct WPILIB_DLLEXPORT SerdeType { static constexpr std::string_view GetSchemaHash() { - return "cb3e1605048ba49325888eb797399fe2"; + return "5eeaa293d0c69aea90eaddea786a2b3b"; } static constexpr std::string_view GetSchema() { - return "PhotonPipelineMetadata metadata;PhotonTrackedTarget[?] " - "targets;MultiTargetPNPResult? multitagResult;"; + return "PhotonPipelineMetadata:626e70461cbdb274fb43ead09c255f4e " + "metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 " + "targets[?];optional " + "MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b " + "multitagResult;"; } static photon::PhotonPipelineResult Unpack(photon::Packet& packet); diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h index abc2196b8b..b1c026a782 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h @@ -37,15 +37,16 @@ namespace photon { template <> struct WPILIB_DLLEXPORT SerdeType { static constexpr std::string_view GetSchemaHash() { - return "8fdada56b9162f2e32bd24f0055d7b60"; + return "cc6dbb5c5c1e0fa808108019b20863f1"; } static constexpr std::string_view GetSchema() { return "float64 yaw;float64 pitch;float64 area;float64 skew;int32 " "fiducialId;int32 objDetectId;float32 objDetectConf;Transform3d " "bestCameraToTarget;Transform3d altCameraToTarget;float64 " - "poseAmbiguity;TargetCorner[?] minAreaRectCorners;TargetCorner[?] " - "detectedCorners;"; + "poseAmbiguity;TargetCorner:16f6ac0dedc8eaccb951f4895d9e18b6 " + "minAreaRectCorners[?];TargetCorner:" + "16f6ac0dedc8eaccb951f4895d9e18b6 detectedCorners[?];"; } static photon::PhotonTrackedTarget Unpack(photon::Packet& packet); diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h index 8e507b0341..f4359e4512 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h @@ -35,7 +35,7 @@ namespace photon { template <> struct WPILIB_DLLEXPORT SerdeType { static constexpr std::string_view GetSchemaHash() { - return "0d1f2546b00f24718e30f38d206d4491"; + return "ae4d655c0a3104d88df4f5db144c1e86"; } static constexpr std::string_view GetSchema() { diff --git a/photon-targeting/src/generated/main/native/include/photon/serde/TargetCornerSerde.h b/photon-targeting/src/generated/main/native/include/photon/serde/TargetCornerSerde.h index 27ce589a1e..718d613a4b 100644 --- a/photon-targeting/src/generated/main/native/include/photon/serde/TargetCornerSerde.h +++ b/photon-targeting/src/generated/main/native/include/photon/serde/TargetCornerSerde.h @@ -34,7 +34,7 @@ namespace photon { template <> struct WPILIB_DLLEXPORT SerdeType { static constexpr std::string_view GetSchemaHash() { - return "22b1ff7551d10215af6fb3672fe4eda8"; + return "16f6ac0dedc8eaccb951f4895d9e18b6"; } static constexpr std::string_view GetSchema() { 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 54daf04ab9..62d5d2c2c0 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 @@ -160,8 +160,8 @@ public void encode(byte src) { */ 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 void encode(short src) { */ 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 void encode(int src) { 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 void encode(float src) { */ 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 void encode(long data) { 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 int decodeInt() { 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 long decodeLong() { 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 double decodeDouble() { 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 float decodeFloat() { 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 short decodeShort() { 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); } /** diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java index a9009d41b5..a4e095cbbd 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java +++ b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java @@ -17,6 +17,8 @@ package org.photonvision.common.dataflow.structures; +import edu.wpi.first.util.struct.Struct; + public interface PacketSerde { int getMaxByteSize(); @@ -24,7 +26,32 @@ public interface PacketSerde { 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(); } 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 5617ef869e..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. @@ -75,7 +77,10 @@ public void updateEntries() { 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(rawBytesEntry, PhotonPipelineResult.photonStruct); @@ -99,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 aa714fd477..3c36d19774 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 @@ -20,6 +20,8 @@ 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 PacketPublisher(RawPublisher publisher, PacketSerde photonStruct) { 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 PacketPublisher(RawPublisher publisher, PacketSerde photonStruct) { 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 void set(T value) { public void close() { publisher.close(); } + + /** + * Publish the schema for our type (and all nested types under it) to NT. + * + *

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 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); + } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 4ae34afa7b..4ddb042b34 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -182,6 +182,8 @@ public int hashCode() { 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 boolean equals(Object obj) { 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 boolean equals(Object obj) { @Override public String toString() { - return "PhotonTrackedTarget{" - + "yaw=" + return "PhotonTrackedTarget [yaw=" + yaw + ", pitch=" + pitch @@ -232,11 +236,21 @@ public String toString() { + 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(); diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java index cdd4f6cf21..8eb24ef6b6 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java @@ -81,7 +81,9 @@ public void pack(ProtobufPhotonTrackedTarget msg, PhotonTrackedTarget value) { .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()); diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp index 0bd08c1cee..cc67ec213f 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp @@ -17,6 +17,8 @@ #include "photon/targeting/proto/MultiTargetPNPResultProto.h" +#include + #include "photon.pb.h" #include "photon/targeting/proto/PNPResultProto.h" diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp index 0fb5697029..5398fae4c1 100644 --- a/photon-targeting/src/test/native/cpp/PacketTest.cpp +++ b/photon-targeting/src/test/native/cpp/PacketTest.cpp @@ -16,6 +16,7 @@ */ #include +#include #include diff --git a/photonlib-python-examples/aimandrange/robot.py b/photonlib-python-examples/aimandrange/robot.py index 617f0ccbad..9afa3b3551 100644 --- a/photonlib-python-examples/aimandrange/robot.py +++ b/photonlib-python-examples/aimandrange/robot.py @@ -25,8 +25,6 @@ import math import wpilib -import wpimath -import wpimath.geometry import drivetrain from photonlibpy import PhotonCamera diff --git a/photonlib-python-examples/aimandrange/tests/pyfrc_test.py b/photonlib-python-examples/aimandrange/tests/pyfrc_test.py index e195147143..235b61fa34 100644 --- a/photonlib-python-examples/aimandrange/tests/pyfrc_test.py +++ b/photonlib-python-examples/aimandrange/tests/pyfrc_test.py @@ -2,5 +2,3 @@ This test module imports tests that come with pyfrc, and can be used to test basic functionality of just about any robot. """ - -from pyfrc.tests import * diff --git a/photonlib-python-examples/aimattarget/tests/pyfrc_test.py b/photonlib-python-examples/aimattarget/tests/pyfrc_test.py index e195147143..235b61fa34 100644 --- a/photonlib-python-examples/aimattarget/tests/pyfrc_test.py +++ b/photonlib-python-examples/aimattarget/tests/pyfrc_test.py @@ -2,5 +2,3 @@ This test module imports tests that come with pyfrc, and can be used to test basic functionality of just about any robot. """ - -from pyfrc.tests import * diff --git a/photonlib-python-examples/poseest/tests/pyfrc_test.py b/photonlib-python-examples/poseest/tests/pyfrc_test.py index e195147143..235b61fa34 100644 --- a/photonlib-python-examples/poseest/tests/pyfrc_test.py +++ b/photonlib-python-examples/poseest/tests/pyfrc_test.py @@ -2,5 +2,3 @@ This test module imports tests that come with pyfrc, and can be used to test basic functionality of just about any robot. """ - -from pyfrc.tests import * diff --git a/scripts/catnt.py b/scripts/catnt.py new file mode 100644 index 0000000000..8010e68411 --- /dev/null +++ b/scripts/catnt.py @@ -0,0 +1,90 @@ +from time import sleep +import ntcore +import argparse +from tabulate import tabulate + + +def list_topics(inst: ntcore.NetworkTableInstance, root: str): + topics = inst.getTable(root).getTopics() + subtables = inst.getTable(root).getSubTables() + + print(f"Topics under {root}") + print( + tabulate( + [ + [topic.getName(), topic.getType().name, topic.getTypeString()] + for topic in topics + ], + headers=["Topic Name", "Type", "Type String"], + ) + ) + print("") + print(f"Tables under {root}") + print(tabulate([[table] for table in subtables], headers=["Table Name"])) + print("") + + +def print_topic(inst: ntcore.NetworkTableInstance, topic: str): + sub = inst.getTopic(topic).genericSubscribe( + options=ntcore.PubSubOptions(sendAll=True, pollStorage=20) + ) + print("") + print(f"Subscribed to {topic}, typestring '{sub.getTopic().getTypeString()}'") + print(f"Properties:") + print(sub.getTopic().getProperties()) + print("") + + start_time = ntcore._now() + count = 0 + while True: + now = ntcore._now() + new_count = len(sub.readQueue()) + count = count + new_count + + hz = count / float(max(now - start_time, 0.1) * 1e-6) + + print(f"{topic} = {sub.get().value()} (rate={hz:.1f}hz, samples={count})") + sleep(1) + + +def connect(inst: ntcore.NetworkTableInstance, server: str): + inst.stopServer() + inst.setServer(server) + inst.startClient4("catnt") + + +def main(): + parser = argparse.ArgumentParser(description="Cat a topic") + parser.add_argument( + "--echo", type=str, help="Fully qualified topic name", required=False + ) + parser.add_argument( + "--server", + type=str, + default="127.0.0.1", + help="IP address of the NT4 server", + required=False, + ) + parser.add_argument("--list", help="List all topics", required=False) + + args = parser.parse_args() + inst = ntcore.NetworkTableInstance.getDefault() + + connect(inst, args.server) + # retained to keep the subscriber alive + topicNameSubscriber = ntcore.MultiSubscriber( + inst, ["/"], ntcore.PubSubOptions(topicsOnly=True) + ) + sleep(1) + + while not inst.isConnected(): + sleep(0.1) + + if args.list: + list_topics(inst, args.list) + if args.echo: + print_topic(inst, args.echo) + + +if __name__ == "__main__": + main()