diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 4424bf398..14ff4a0d3 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -17,6 +17,8 @@ nativeUtils { } } +sourceSets.main.java.srcDir "${projectDir}/src/generated/main/java" + model { components { "${nativeName}"(NativeLibrarySpec) { diff --git a/photon-targeting/generate_messages.py b/photon-targeting/generate_messages.py new file mode 100644 index 000000000..c3d9f8cef --- /dev/null +++ b/photon-targeting/generate_messages.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python3 + +import argparse +import hashlib +import json +import os +import sys +from pathlib import Path +from typing import Any, Dict, List, TypedDict, cast + +import yaml +from jinja2 import Environment, FileSystemLoader +from jinja2.environment import Template + + +class SerdeField(TypedDict): + name: str + type: str + +class MessageType(TypedDict): + name: str + fields: List[SerdeField] + + +def yaml_to_dict(path: str): + script_dir = os.path.dirname(os.path.abspath(__file__)) + yaml_file_path = os.path.join(script_dir, path) + + with open(yaml_file_path, "r") as file: + file_dict: List[MessageType] = yaml.safe_load(file) + + # Print for testing + print(file_dict) + + return file_dict + + +data_types = yaml_to_dict("src/generate/message_data_types.yaml") + +def parse_yaml(): + config = yaml_to_dict("src/generate/messages.yaml") + + # Hash a comments-stripped version for message integrity checking + cleaned_yaml = yaml.dump(config, default_flow_style=False).strip() + message_hash = hashlib.md5(cleaned_yaml.encode("ascii")).digest() + message_hash = list(message_hash) + print(message_hash) + + return config, message_hash + + +def generate_photon_messages(output_root, template_root): + messages, message_hash = parse_yaml() + + env = Environment( + loader=FileSystemLoader(str(template_root)), + # autoescape=False, + # keep_trailing_newline=False, + ) + + + # add our custom types + for message in messages: + name = message['name'] + data_types[name] = { + 'len': -1, + 'java_type': name, + 'cpp_type': name, + } + + root_path = Path(output_root) / "main/java/org/photonvision/struct" + template = env.get_template("Message.java.jinja") + + root_path.mkdir(parents=True, exist_ok=True) + + for message in messages: + java_name = f"{message['name']}Serde.java" + + + output_file = root_path / java_name + output_file.write_text(template.render(message, type_map=data_types), encoding="utf-8") + + + +def main(argv): + script_path = Path(__file__).resolve() + dirname = script_path.parent + + parser = argparse.ArgumentParser() + parser.add_argument( + "--output_directory", + help="Optional. If set, will output the generated files to this directory, otherwise it will use a path relative to the script", + default=dirname / "src/generated", + type=Path, + ) + parser.add_argument( + "--template_root", + help="Optional. If set, will use this directory as the root for the jinja templates", + default=dirname / "src/generate", + type=Path, + ) + args = parser.parse_args(argv) + + generate_photon_messages(args.output_directory, args.template_root) + + +if __name__ == "__main__": + main(sys.argv[1:]) diff --git a/photon-targeting/src/generate/Message.java.jinja b/photon-targeting/src/generate/Message.java.jinja new file mode 100644 index 000000000..70f708fe7 --- /dev/null +++ b/photon-targeting/src/generate/Message.java.jinja @@ -0,0 +1,64 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +// THIS FILE WAS AUTO-GENERATED BY ./photon-targeting/generate_messages.py. DO NOT MODIFY + +package org.photonvision.struct; + +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.dataflow.structures.PacketSerde; + +// Assume that the base class lives here and we can import it +import org.photonvision.targeting.*; + + +/** + * This is a test + */ +public class {{ name }}Serde implements PacketSerde<{{name}}> { + + + {% for field in fields %}public {{ type_map[field.type].java_type }} {{ field.name }}; + {% endfor %} + + @Override + public int getMaxByteSize() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'"); + } + + @Override + public void pack(Packet packet, {{ name }} value) { + // explicitly cast to avoid accidentally encoding the wrong thing + {% for field in fields -%} + packet.encode(({{type_map[field.type].java_type}}) value.{{field.name}}); + {%- if not loop.last %} + {% endif -%} + {% endfor %} + } + + @Override + public {{ name }} unpack(Packet packet) { + var ret = new {{ name }}(); + {% for field in fields -%} + ret.{{field.name}} = packet.{{type_map[field.type].java_decode_method}}(); + {%- if not loop.last %} + {% endif -%} + {% endfor %} + return ret; + } +} diff --git a/photon-targeting/src/generate/message_data_types.yaml b/photon-targeting/src/generate/message_data_types.yaml new file mode 100644 index 000000000..ee82f46eb --- /dev/null +++ b/photon-targeting/src/generate/message_data_types.yaml @@ -0,0 +1,12 @@ +--- +bool: + # length in bytes + len: 1 + java_type: bool + cpp_type: bool + java_decode_method: decodeBoolean +int64: + len: 8 + java_type: long + cpp_type: int64_t + java_decode_method: decodeLong \ No newline at end of file diff --git a/photon-targeting/src/generate/messages.yaml b/photon-targeting/src/generate/messages.yaml new file mode 100644 index 000000000..830fd82e5 --- /dev/null +++ b/photon-targeting/src/generate/messages.yaml @@ -0,0 +1,17 @@ +--- +- name: PhotonPipelineMetadata + fields: + - name: sequenceID + type: int64 + - name: captureTimestampMicros + type: int64 + - name: publishTimestampMicros + type: int64 + +- name: PhotonPipelineResult + fields: + - name: metadata + type: PhotonPipelineMetadata + - name: targets + type: int64 + list_len: VLA 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 new file mode 100644 index 000000000..b0ca01db3 --- /dev/null +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineMetadataSerde.java @@ -0,0 +1,62 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +// THIS FILE WAS AUTO-GENERATED BY ./photon-targeting/generate_messages.py. DO NOT MODIFY + +package org.photonvision.struct; + +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.dataflow.structures.PacketSerde; + +// Assume that the base class lives here and we can import it +import org.photonvision.targeting.*; + + +/** + * This is a test + */ +public class PhotonPipelineMetadataSerde implements PacketSerde { + + + public long sequenceID; + public long captureTimestampMicros; + public long publishTimestampMicros; + + + @Override + public int getMaxByteSize() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'"); + } + + @Override + public void pack(Packet packet, PhotonPipelineMetadata value) { + // explicitly cast to avoid accidentally encoding the wrong thing + packet.encode((long) value.sequenceID); + packet.encode((long) value.captureTimestampMicros); + packet.encode((long) value.publishTimestampMicros); + } + + @Override + public PhotonPipelineMetadata unpack(Packet packet) { + var ret = new PhotonPipelineMetadata(); + ret.sequenceID = packet.decodeLong(); + ret.captureTimestampMicros = packet.decodeLong(); + ret.publishTimestampMicros = packet.decodeLong(); + return ret; + } +} \ No newline at end of file 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 new file mode 100644 index 000000000..168b1a694 --- /dev/null +++ b/photon-targeting/src/generated/main/java/org/photonvision/struct/PhotonPipelineResultSerde.java @@ -0,0 +1,59 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +// THIS FILE WAS AUTO-GENERATED BY ./photon-targeting/generate_messages.py. DO NOT MODIFY + +package org.photonvision.struct; + +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.dataflow.structures.PacketSerde; + +// Assume that the base class lives here and we can import it +import org.photonvision.targeting.*; + + +/** + * This is a test + */ +public class PhotonPipelineResultSerde implements PacketSerde { + + + public PhotonPipelineMetadata metadata; + public long targets; + + + @Override + public int getMaxByteSize() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getMaxByteSize'"); + } + + @Override + public void pack(Packet packet, PhotonPipelineResult value) { + // explicitly cast to avoid accidentally encoding the wrong thing + packet.encode((PhotonPipelineMetadata) value.metadata); + packet.encode((long) value.targets); + } + + @Override + public PhotonPipelineResult unpack(Packet packet) { + var ret = new PhotonPipelineResult(); + ret.metadata = packet.(); + ret.targets = packet.decodeLong(); + return ret; + } +} \ No newline at end of file diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java new file mode 100644 index 000000000..10713ea6a --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java @@ -0,0 +1,50 @@ +package org.photonvision.targeting; + +import org.photonvision.struct.PhotonPacketSerdeStruct; + +public class PhotonPipelineMetadata { + // Mirror of the heartbeat entry -- monotonically increasing + public long sequenceID; + + // Image capture and NT publish timestamp, in microseconds and in the + // coprocessor timebase. As + // reported by WPIUtilJNI::now. + public long captureTimestampMicros; + public long publishTimestampMicros; + + public PhotonPipelineMetadata( + long captureTimestampMicros, long publishTimestampMicros, long sequenceID) { + this.captureTimestampMicros = captureTimestampMicros; + this.publishTimestampMicros = publishTimestampMicros; + this.sequenceID = sequenceID; + } + + public PhotonPipelineMetadata() { + this(-1, -1, -1); + } + + /** Returns the time between image capture and publish to NT */ + public double getLatencyMillis() { + return (publishTimestampMicros - captureTimestampMicros) / 1e3; + } + + /** The time that this image was captured, in the coprocessor's time base. */ + public long getCaptureTimestampMicros() { + return captureTimestampMicros; + } + + /** The time that this result was published to NT, in the coprocessor's time base. */ + public long getPublishTimestampMicros() { + return publishTimestampMicros; + } + + /** + * The number of non-empty frames processed by this camera since boot. Useful to checking if a + * camera is alive. + */ + public long getSequenceID() { + return sequenceID; + } + + PhotonPipelineResultMetadataSerde serde = new PhotonPipelineResultMetadataSerde(); +} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 1b75f96c7..d4dc2c5e1 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -20,33 +20,30 @@ import edu.wpi.first.util.protobuf.ProtobufSerializable; import java.util.ArrayList; import java.util.List; -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.common.dataflow.structures.PacketSerde; import org.photonvision.targeting.proto.PhotonPipelineResultProto; +import org.photonvision.targeting.serde.APacketSerde; +import org.photonvision.targeting.serde.APhotonStructSerde; /** Represents a pipeline result from a PhotonCamera. */ public class PhotonPipelineResult implements ProtobufSerializable { private static boolean HAS_WARNED = false; - // Image capture and NT publish timestamp, in microseconds and in the coprocessor timebase. As - // reported by WPIUtilJNI::now. - private long captureTimestampMicros = -1; - private long publishTimestampMicros = -1; - - // Mirror of the heartbeat entry -- monotonically increasing - private long sequenceID = -1; + // Frame capture metadata + public final PhotonPipelineMetadata metadata; // Targets to store. public final List targets = new ArrayList<>(); // Multi-tag result - private MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult(); + final MultiTargetPNPResult multiTagResult; - // Since we don't trust NT time sync, keep track of when we got this packet into robot code - private long ntRecieveTimestampMicros; + // HACK: Since we don't trust NT time sync, keep track of when we got this packet into robot code + long ntRecieveTimestampMicros = -1; /** Constructs an empty pipeline result. */ - public PhotonPipelineResult() {} + public PhotonPipelineResult() { + this(new PhotonPipelineMetadata(), List.of(), new MultiTargetPNPResult()); + } /** * Constructs a pipeline result. @@ -63,10 +60,10 @@ public PhotonPipelineResult( long captureTimestamp, long publishTimestamp, List targets) { - this.captureTimestampMicros = captureTimestamp; - this.publishTimestampMicros = publishTimestamp; - this.sequenceID = sequenceID; - this.targets.addAll(targets); + this( + new PhotonPipelineMetadata(sequenceID, captureTimestamp, publishTimestamp), + targets, + new MultiTargetPNPResult()); } /** @@ -86,9 +83,17 @@ public PhotonPipelineResult( long publishTimestamp, List targets, MultiTargetPNPResult result) { - this.captureTimestampMicros = captureTimestamp; - this.publishTimestampMicros = publishTimestamp; - this.sequenceID = sequenceID; + this( + new PhotonPipelineMetadata(sequenceID, captureTimestamp, publishTimestamp), + targets, + result); + } + + public PhotonPipelineResult( + PhotonPipelineMetadata metadata, + List targets, + MultiTargetPNPResult result) { + this.metadata = metadata; this.targets.addAll(targets); this.multiTagResult = result; } @@ -124,50 +129,6 @@ public PhotonTrackedTarget getBestTarget() { return hasTargets() ? targets.get(0) : null; } - /** Returns the time between image capture and publish to NT */ - public double getLatencyMillis() { - return (publishTimestampMicros - captureTimestampMicros) / 1e3; - } - - /** - * Returns the estimated time the frame was taken, in the recieved system's time base. This is - * calculated as (NT recieve time (robot base) - (publish timestamp, coproc timebase - capture - * timestamp, coproc timebase)) - * - * @return The timestamp in seconds - */ - public double getTimestampSeconds() { - return (ntRecieveTimestampMicros - (publishTimestampMicros - captureTimestampMicros)) / 1e6; - } - - /** The time that this image was captured, in the coprocessor's time base. */ - public long getCaptureTimestampMicros() { - return captureTimestampMicros; - } - - /** The time that this result was published to NT, in the coprocessor's time base. */ - public long getPublishTimestampMicros() { - return publishTimestampMicros; - } - - /** - * The number of non-empty frames processed by this camera since boot. Useful to checking if a - * camera is alive. - */ - public long getSequenceID() { - return sequenceID; - } - - /** The time that the robot recieved this result, in the FPGA timebase. */ - public long getNtRecieveTimestampMicros() { - return ntRecieveTimestampMicros; - } - - /** Sets the FPGA timestamp this result was recieved by robot code */ - public void setRecieveTimestampMicros(long timestampMicros) { - this.ntRecieveTimestampMicros = timestampMicros; - } - /** * Returns whether the pipeline has targets. * @@ -194,18 +155,36 @@ public MultiTargetPNPResult getMultiTagResult() { return multiTagResult; } + /** + * Returns the estimated time the frame was taken, in the recieved system's time base. This is + * calculated as (NT recieve time (robot base) - (publish timestamp, coproc timebase - capture + * timestamp, coproc timebase)) + * + * @return The timestamp in seconds + */ + public double getTimestampSeconds() { + return (ntRecieveTimestampMicros + - (metadata.publishTimestampMicros - metadata.captureTimestampMicros)) + / 1e6; + } + + /** The time that the robot recieved this result, in the FPGA timebase. */ + public long getNtRecieveTimestampMicros() { + return ntRecieveTimestampMicros; + } + + /** Sets the FPGA timestamp this result was recieved by robot code */ + public void setRecieveTimestampMicros(long timestampMicros) { + this.ntRecieveTimestampMicros = timestampMicros; + } + @Override public int hashCode() { final int prime = 31; int result = 1; - result = prime * result + (int) (captureTimestampMicros ^ (captureTimestampMicros >>> 32)); - long temp; - temp = Double.doubleToLongBits(publishTimestampMicros); - result = prime * result + (int) (temp ^ (temp >>> 32)); - result = prime * result + (int) (sequenceID ^ (sequenceID >>> 32)); + result = prime * result + ((metadata == null) ? 0 : metadata.hashCode()); result = prime * result + ((targets == null) ? 0 : targets.hashCode()); result = prime * result + ((multiTagResult == null) ? 0 : multiTagResult.hashCode()); - result = prime * result + (int) (ntRecieveTimestampMicros ^ (ntRecieveTimestampMicros >>> 32)); return result; } @@ -215,70 +194,30 @@ public boolean equals(Object obj) { if (obj == null) return false; if (getClass() != obj.getClass()) return false; PhotonPipelineResult other = (PhotonPipelineResult) obj; - if (captureTimestampMicros != other.captureTimestampMicros) return false; - if (Double.doubleToLongBits(publishTimestampMicros) - != Double.doubleToLongBits(other.publishTimestampMicros)) return false; - if (sequenceID != other.sequenceID) return false; + if (metadata == null) { + if (other.metadata != null) return false; + } else if (!metadata.equals(other.metadata)) return false; if (targets == null) { if (other.targets != null) return false; } else if (!targets.equals(other.targets)) return false; if (multiTagResult == null) { if (other.multiTagResult != null) return false; } else if (!multiTagResult.equals(other.multiTagResult)) return false; - if (ntRecieveTimestampMicros != other.ntRecieveTimestampMicros) return false; return true; } @Override public String toString() { - return "PhotonPipelineResult [captureTimestamp=" - + captureTimestampMicros - + ", publishTimestamp=" - + publishTimestampMicros - + ", sequenceID=" - + sequenceID + return "PhotonPipelineResult [metadata=" + + metadata + ", targets=" + targets + ", multiTagResult=" + multiTagResult - + ", ntRecieveTimestamp=" - + ntRecieveTimestampMicros + "]"; } - public static final class APacketSerde implements PacketSerde { - @Override - public int getMaxByteSize() { - // This uses dynamic packets so it doesn't matter - return -1; - } - - @Override - public void pack(Packet packet, PhotonPipelineResult value) { - packet.encode(value.sequenceID); - packet.encode(value.captureTimestampMicros); - packet.encode(value.publishTimestampMicros); - packet.encode((byte) value.targets.size()); - for (var target : value.targets) PhotonTrackedTarget.serde.pack(packet, target); - MultiTargetPNPResult.serde.pack(packet, value.multiTagResult); - } - - @Override - public PhotonPipelineResult unpack(Packet packet) { - var seq = packet.decodeLong(); - var cap = packet.decodeLong(); - var pub = packet.decodeLong(); - var len = packet.decodeByte(); - var targets = new ArrayList(len); - for (int i = 0; i < len; i++) { - targets.add(PhotonTrackedTarget.serde.unpack(packet)); - } - var result = MultiTargetPNPResult.serde.unpack(packet); - - return new PhotonPipelineResult(seq, cap, pub, targets, result); - } - } - public static final APacketSerde serde = new APacketSerde(); + public static final APhotonStructSerde photonStruct = new APhotonStructSerde(); public static final PhotonPipelineResultProto proto = new PhotonPipelineResultProto(); } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/serde/APacketSerde.java b/photon-targeting/src/main/java/org/photonvision/targeting/serde/APacketSerde.java new file mode 100644 index 000000000..a87c8b3cd --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/serde/APacketSerde.java @@ -0,0 +1,43 @@ +package org.photonvision.targeting.serde; + +import java.util.ArrayList; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.dataflow.structures.PacketSerde; +import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +public final class APacketSerde implements PacketSerde { + @Override + public int getMaxByteSize() { + // This uses dynamic packets so it doesn't matter + return -1; + } + + @Override + public void pack(Packet packet, PhotonPipelineResult value) { + packet.encode(value.getSequenceID()); + packet.encode(value.getCaptureTimestampMicros()); + packet.encode(value.getPublishTimestampMicros()); + + packet.encode((byte) value.targets.size()); + for (var target : value.targets) PhotonTrackedTarget.serde.pack(packet, target); + + MultiTargetPNPResult.serde.pack(packet, value.getMultiTagResult()); + } + + @Override + public PhotonPipelineResult unpack(Packet packet) { + var seq = packet.decodeLong(); + var cap = packet.decodeLong(); + var pub = packet.decodeLong(); + var len = packet.decodeByte(); + var targets = new ArrayList(len); + for (int i = 0; i < len; i++) { + targets.add(PhotonTrackedTarget.serde.unpack(packet)); + } + var result = MultiTargetPNPResult.serde.unpack(packet); + + return new PhotonPipelineResult(seq, cap, pub, targets, result); + } +} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/serde/APhotonStructSerde.java b/photon-targeting/src/main/java/org/photonvision/targeting/serde/APhotonStructSerde.java new file mode 100644 index 000000000..dbecebdf7 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/serde/APhotonStructSerde.java @@ -0,0 +1,3 @@ +package org.photonvision.targeting.serde; + +public class APhotonStructSerde {} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/serde/Message.java b/photon-targeting/src/main/java/org/photonvision/targeting/serde/Message.java new file mode 100644 index 000000000..aa69d4d21 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/serde/Message.java @@ -0,0 +1,186 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.targeting.serde; + +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.util.protobuf.ProtobufSerializable; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.dataflow.structures.PacketSerde; +import org.photonvision.targeting.proto.PNPResultProto; +import org.photonvision.utils.PacketUtils; + +/** + * The best estimated transformation from solvePnP, and possibly an alternate transformation + * depending on the solvePNP method. If an alternate solution is present, the ambiguity value + * represents the ratio of reprojection error in the best solution to the alternate (best / + * alternate). + * + *

Note that the coordinate frame of these transforms depends on the implementing solvePnP + * method. + */ +public class Message implements ProtobufSerializable { + /** + * If this result is valid. A false value indicates there was an error in estimation, and this + * result should not be used. + */ + public final boolean isPresent; + + /** + * The best-fit transform. The coordinate frame of this transform depends on the method which gave + * this result. + */ + public final Transform3d best; + + /** Reprojection error of the best solution, in pixels */ + public final double bestReprojErr; + + /** + * Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal + * to the best solution. + */ + public final Transform3d alt; + + /** If no alternate solution is found, this is bestReprojErr */ + public final double altReprojErr; + + /** If no alternate solution is found, this is 0 */ + public final double ambiguity; + + /** An empty (invalid) result. */ + public Message() { + this.isPresent = false; + this.best = new Transform3d(); + this.alt = new Transform3d(); + this.ambiguity = 0; + this.bestReprojErr = 0; + this.altReprojErr = 0; + } + + public Message(Transform3d best, double bestReprojErr) { + this(best, best, 0, bestReprojErr, bestReprojErr); + } + + public Message( + Transform3d best, + Transform3d alt, + double ambiguity, + double bestReprojErr, + double altReprojErr) { + this.isPresent = true; + this.best = best; + this.alt = alt; + this.ambiguity = ambiguity; + this.bestReprojErr = bestReprojErr; + this.altReprojErr = altReprojErr; + } + + @Override + public int hashCode() { + final int prime = 31; + int result = 1; + result = prime * result + (isPresent ? 1231 : 1237); + result = prime * result + ((best == null) ? 0 : best.hashCode()); + long temp; + temp = Double.doubleToLongBits(bestReprojErr); + result = prime * result + (int) (temp ^ (temp >>> 32)); + result = prime * result + ((alt == null) ? 0 : alt.hashCode()); + temp = Double.doubleToLongBits(altReprojErr); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(ambiguity); + result = prime * result + (int) (temp ^ (temp >>> 32)); + return result; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null) return false; + if (getClass() != obj.getClass()) return false; + Message other = (Message) obj; + if (isPresent != other.isPresent) return false; + if (best == null) { + if (other.best != null) return false; + } else if (!best.equals(other.best)) return false; + if (Double.doubleToLongBits(bestReprojErr) != Double.doubleToLongBits(other.bestReprojErr)) + return false; + if (alt == null) { + if (other.alt != null) return false; + } else if (!alt.equals(other.alt)) return false; + if (Double.doubleToLongBits(altReprojErr) != Double.doubleToLongBits(other.altReprojErr)) + return false; + if (Double.doubleToLongBits(ambiguity) != Double.doubleToLongBits(other.ambiguity)) + return false; + return true; + } + + @Override + public String toString() { + return "PNPResult [isPresent=" + + isPresent + + ", best=" + + best + + ", bestReprojErr=" + + bestReprojErr + + ", alt=" + + alt + + ", altReprojErr=" + + altReprojErr + + ", ambiguity=" + + ambiguity + + "]"; + } + + public static final class APacketSerde implements PacketSerde { + @Override + public int getMaxByteSize() { + return 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3); + } + + @Override + public void pack(Packet packet, Message value) { + packet.encode(value.isPresent); + + if (value.isPresent) { + PacketUtils.packTransform3d(packet, value.best); + PacketUtils.packTransform3d(packet, value.alt); + packet.encode(value.bestReprojErr); + packet.encode(value.altReprojErr); + packet.encode(value.ambiguity); + } + } + + @Override + public Message unpack(Packet packet) { + var present = packet.decodeBoolean(); + + if (!present) { + return new Message(); + } + + var best = PacketUtils.unpackTransform3d(packet); + var alt = PacketUtils.unpackTransform3d(packet); + var bestEr = packet.decodeDouble(); + var altEr = packet.decodeDouble(); + var ambiguity = packet.decodeDouble(); + return new Message(best, alt, ambiguity, bestEr, altEr); + } + } + + public static final APacketSerde serde = new APacketSerde(); + public static final PNPResultProto proto = new PNPResultProto(); +}