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

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

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

2
.github/workflows/lint-format.yml vendored
View File

@@ -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

2
README.md
View File

@@ -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

14
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View File

@@ -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 class NTDataPublisher implements CVPipelineResultConsumer {
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 class NTDataPublisher implements CVPipelineResultConsumer {
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);
}

4
photon-lib/py/photonlibpy/generated/MultiTargetPNPResultSerde.py
View File

@@ -25,8 +25,8 @@ from ..targeting import *
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":

2
photon-lib/py/photonlibpy/generated/PhotonPipelineMetadataSerde.py
View File

@@ -25,7 +25,7 @@ from ..targeting import *
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;"
)

8
photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py
View File

@@ -25,8 +25,8 @@ from ..targeting import *
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 @@ class PhotonPipelineResultSerde:
# 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

6
photon-lib/py/photonlibpy/generated/PhotonTrackedTargetSerde.py
View File

@@ -25,8 +25,8 @@ from ..targeting import *
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 @@ class PhotonTrackedTargetSerde:
# 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

4
photon-lib/py/photonlibpy/generated/PnpResultSerde.py
View File

@@ -25,17 +25,15 @@ from ..targeting import *
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

2
photon-lib/py/photonlibpy/generated/TargetCornerSerde.py
View File

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

16
photon-lib/py/photonlibpy/packet.py
View File

@@ -85,7 +85,9 @@ class Packet:
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 @@ class Packet:
*
* @return A decoded byte from the packet.
"""
return self._decodeGeneric(">b", 1)
return self._decodeGeneric("<b", 1)
def decode16(self) -> int:
"""
@@ -104,7 +106,7 @@ class Packet:
*
* @return A decoded short from the packet.
"""
return self._decodeGeneric(">h", 2)
return self._decodeGeneric("<h", 2)
def decodeInt(self) -> int:
"""
@@ -112,7 +114,7 @@ class Packet:
*
* @return A decoded int from the packet.
"""
return self._decodeGeneric(">l", 4)
return self._decodeGeneric("<l", 4)
def decodeFloat(self) -> float:
"""
@@ -120,7 +122,7 @@ class Packet:
*
* @return A decoded float from the packet.
"""
return self._decodeGeneric(">f", 4)
return self._decodeGeneric("<f", 4)
def decodeLong(self) -> int:
"""
@@ -128,7 +130,7 @@ class Packet:
*
* @return A decoded int64 from the packet.
"""
return self._decodeGeneric(">q", 8)
return self._decodeGeneric("<q", 8)
def decodeDouble(self) -> float:
"""
@@ -136,7 +138,7 @@ class Packet:
*
* @return A decoded double from the packet.
"""
return self._decodeGeneric(">d", 8)
return self._decodeGeneric("<d", 8)
def decodeBoolean(self) -> bool:
"""

12
photon-lib/py/photonlibpy/photonCamera.py
View File

@@ -225,7 +225,15 @@ class PhotonCamera:
)
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 @@ class PhotonCamera:
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)

2
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View File

@@ -422,7 +422,7 @@ public class PhotonCamera implements AutoCloseable {
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

8
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

@@ -32,6 +32,7 @@ import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
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 class PhotonCameraSim implements AutoCloseable {
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 class PhotonCameraSim implements AutoCloseable {
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);

58
photon-serde/README.md
View File

@@ -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;
```

107
photon-serde/generate_messages.py
View File

@@ -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",
)

1
photon-serde/messages.yaml
View File

@@ -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: []

39
photon-serde/templates/Message.java.jinja
View File

@@ -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%}
};
}
}

1
photon-serde/templates/ThingSerde.py.jinja
View File

@@ -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

28
photon-targeting/src/generated/main/java/org/photonvision/struct/MultiTargetPNPResultSerde.java
View File

@@ -26,17 +26,20 @@ 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;
/**
* Auto-generated serialization/deserialization helper for MultiTargetPNPResult
*/
public class MultiTargetPNPResultSerde implements PacketSerde<MultiTargetPNPResult> {
// 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 class MultiTargetPNPResultSerde implements PacketSerde<MultiTargetPNPResu
return ret;
}
@Override
public PacketSerde<?>[] getNestedPhotonMessages() {
return new PacketSerde<?>[] {
PnpResult.photonStruct
};
}
@Override
public Struct<?>[] getNestedWpilibMessages() {
return new Struct<?>[] {
};
}
}

27
photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java
View File

@@ -26,17 +26,20 @@ 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;
/**
* Auto-generated serialization/deserialization helper for PhotonPipelineMetadata
*/
public class PhotonPipelineMetadataSerde implements PacketSerde<PhotonPipelineMetadata> {
// 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 class PhotonPipelineMetadataSerde implements PacketSerde<PhotonPipelineMe
return ret;
}
@Override
public PacketSerde<?>[] getNestedPhotonMessages() {
return new PacketSerde<?>[] {
};
}
@Override
public Struct<?>[] getNestedWpilibMessages() {
return new Struct<?>[] {
};
}
}

28
photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java
View File

@@ -26,17 +26,20 @@ 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;
/**
* Auto-generated serialization/deserialization helper for PhotonPipelineResult
*/
public class PhotonPipelineResultSerde implements PacketSerde<PhotonPipelineResult> {
// 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 class PhotonPipelineResultSerde implements PacketSerde<PhotonPipelineResu
return ret;
}
@Override
public PacketSerde<?>[] getNestedPhotonMessages() {
return new PacketSerde<?>[] {
PhotonPipelineMetadata.photonStruct,MultiTargetPNPResult.photonStruct,PhotonTrackedTarget.photonStruct
};
}
@Override
public Struct<?>[] getNestedWpilibMessages() {
return new Struct<?>[] {
};
}
}

33
photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonTrackedTargetSerde.java
View File

@@ -26,17 +26,20 @@ 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;
import edu.wpi.first.math.geometry.Transform3d;
/**
* Auto-generated serialization/deserialization helper for PhotonTrackedTarget
*/
public class PhotonTrackedTargetSerde implements PacketSerde<PhotonTrackedTarget> {
// 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 class PhotonTrackedTargetSerde implements PacketSerde<PhotonTrackedTarget
// 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 class PhotonTrackedTargetSerde implements PacketSerde<PhotonTrackedTarget
// 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 class PhotonTrackedTargetSerde implements PacketSerde<PhotonTrackedTarget
return ret;
}
@Override
public PacketSerde<?>[] getNestedPhotonMessages() {
return new PacketSerde<?>[] {
TargetCorner.photonStruct
};
}
@Override
public Struct<?>[] getNestedWpilibMessages() {
return new Struct<?>[] {
Transform3d.struct
};
}
}

32
photon-targeting/src/generated/main/java/org/photonvision/struct/PnpResultSerde.java
View File

@@ -26,17 +26,20 @@ 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;
import edu.wpi.first.math.geometry.Transform3d;
/**
* Auto-generated serialization/deserialization helper for PnpResult
*/
public class PnpResultSerde implements PacketSerde<PnpResult> {
// 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 class PnpResultSerde implements PacketSerde<PnpResult> {
@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 class PnpResultSerde implements PacketSerde<PnpResult> {
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 class PnpResultSerde implements PacketSerde<PnpResult> {
return ret;
}
@Override
public PacketSerde<?>[] getNestedPhotonMessages() {
return new PacketSerde<?>[] {
};
}
@Override
public Struct<?>[] getNestedWpilibMessages() {
return new Struct<?>[] {
Transform3d.struct
};
}
}

27
photon-targeting/src/generated/main/java/org/photonvision/struct/TargetCornerSerde.java
View File

@@ -26,17 +26,20 @@ 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;
/**
* Auto-generated serialization/deserialization helper for TargetCorner
*/
public class TargetCornerSerde implements PacketSerde<TargetCorner> {
// 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 class TargetCornerSerde implements PacketSerde<TargetCorner> {
return ret;
}
@Override
public PacketSerde<?>[] getNestedPhotonMessages() {
return new PacketSerde<?>[] {
};
}
@Override
public Struct<?>[] getNestedWpilibMessages() {
return new Struct<?>[] {
};
}
}

5
photon-targeting/src/generated/main/native/include/photon/serde/MultiTargetPNPResultSerde.h
View File

@@ -36,11 +36,12 @@ namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<MultiTargetPNPResult> {
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);

2
photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineMetadataSerde.h
View File

@@ -34,7 +34,7 @@ namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<PhotonPipelineMetadata> {
static constexpr std::string_view GetSchemaHash() {
return "2a7039527bda14d13028a1b9282d40a2";
return "626e70461cbdb274fb43ead09c255f4e";
}
static constexpr std::string_view GetSchema() {

9
photon-targeting/src/generated/main/native/include/photon/serde/PhotonPipelineResultSerde.h
View File

@@ -39,12 +39,15 @@ namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<PhotonPipelineResult> {
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);

7
photon-targeting/src/generated/main/native/include/photon/serde/PhotonTrackedTargetSerde.h
View File

@@ -37,15 +37,16 @@ namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<PhotonTrackedTarget> {
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);

2
photon-targeting/src/generated/main/native/include/photon/serde/PnpResultSerde.h
View File

@@ -35,7 +35,7 @@ namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<PnpResult> {
static constexpr std::string_view GetSchemaHash() {
return "0d1f2546b00f24718e30f38d206d4491";
return "ae4d655c0a3104d88df4f5db144c1e86";
}
static constexpr std::string_view GetSchema() {

2
photon-targeting/src/generated/main/native/include/photon/serde/TargetCornerSerde.h
View File

@@ -34,7 +34,7 @@ namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<TargetCorner> {
static constexpr std::string_view GetSchemaHash() {
return "22b1ff7551d10215af6fb3672fe4eda8";
return "16f6ac0dedc8eaccb951f4895d9e18b6";
}
static constexpr std::string_view GetSchema() {

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

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

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

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

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

@@ -17,6 +17,7 @@
package org.photonvision.common.networktables;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.networktables.BooleanPublisher;
import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.BooleanTopic;
@@ -28,6 +29,7 @@ import edu.wpi.first.networktables.IntegerTopic;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.ProtobufPublisher;
import edu.wpi.first.networktables.PubSubOption;
import edu.wpi.first.networktables.StructPublisher;
import org.photonvision.targeting.PhotonPipelineResult;
/**
@@ -56,7 +58,7 @@ public class NTTopicSet {
public DoublePublisher targetPitchEntry;
public DoublePublisher targetYawEntry;
public DoublePublisher targetAreaEntry;
public DoubleArrayPublisher targetPoseEntry;
public StructPublisher<Transform3d> targetPoseEntry;
public DoublePublisher targetSkewEntry;
// The raw position of the best target, in pixels.
@@ -75,7 +77,10 @@ public class NTTopicSet {
var rawBytesEntry =
subTable
.getRawTopic("rawBytes")
.publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
.publish(
PhotonPipelineResult.photonStruct.getTypeString(),
PubSubOption.periodic(0.01),
PubSubOption.sendAll(true));
resultPublisher =
new PacketPublisher<PhotonPipelineResult>(rawBytesEntry, PhotonPipelineResult.photonStruct);
@@ -99,7 +104,7 @@ public class NTTopicSet {
targetPitchEntry = subTable.getDoubleTopic("targetPitch").publish();
targetAreaEntry = subTable.getDoubleTopic("targetArea").publish();
targetYawEntry = subTable.getDoubleTopic("targetYaw").publish();
targetPoseEntry = subTable.getDoubleArrayTopic("targetPose").publish();
targetPoseEntry = subTable.getStructTopic("targetPose", Transform3d.struct).publish();
targetSkewEntry = subTable.getDoubleTopic("targetSkew").publish();
bestTargetPosX = subTable.getDoubleTopic("targetPixelsX").publish();

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

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

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

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

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

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

2
photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp
View File

@@ -17,6 +17,8 @@
#include "photon/targeting/proto/MultiTargetPNPResultProto.h"
#include <vector>
#include "photon.pb.h"
#include "photon/targeting/proto/PNPResultProto.h"

1
photon-targeting/src/test/native/cpp/PacketTest.cpp
View File

@@ -16,6 +16,7 @@
*/
#include <chrono>
#include <vector>
#include <units/angle.h>

2
photonlib-python-examples/aimandrange/robot.py
View File

@@ -25,8 +25,6 @@
import math
import wpilib
import wpimath
import wpimath.geometry
import drivetrain
from photonlibpy import PhotonCamera

2
photonlib-python-examples/aimandrange/tests/pyfrc_test.py
View File

@@ -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 *

2
photonlib-python-examples/aimattarget/tests/pyfrc_test.py
View File

@@ -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 *

2
photonlib-python-examples/poseest/tests/pyfrc_test.py
View File

@@ -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 *

90
scripts/catnt.py Normal file
View File

@@ -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()