diff --git a/build.gradle b/build.gradle index 3c00130fd5..1f066ba997 100644 --- a/build.gradle +++ b/build.gradle @@ -4,6 +4,7 @@ plugins { id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2" id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4" id 'edu.wpi.first.WpilibTools' version '1.3.0' + id 'com.google.protobuf' version '0.9.4' apply false } allprojects { diff --git a/photon-client/src/components/settings/NetworkingCard.vue b/photon-client/src/components/settings/NetworkingCard.vue index 0a81f6b39b..2b1724cf31 100644 --- a/photon-client/src/components/settings/NetworkingCard.vue +++ b/photon-client/src/components/settings/NetworkingCard.vue @@ -5,10 +5,12 @@ import PvInput from "@/components/common/pv-input.vue"; import PvRadio from "@/components/common/pv-radio.vue"; import PvSwitch from "@/components/common/pv-switch.vue"; import PvSelect from "@/components/common/pv-select.vue"; -import { NetworkConnectionType } from "@/types/SettingTypes"; +import { NetworkConnectionType, type NetworkSettings } from "@/types/SettingTypes"; import { useStateStore } from "@/stores/StateStore"; const settingsValid = ref(true); +// Copy object to remove reference to store +const tempSettingsStruct = ref(Object.assign({}, useSettingsStore().network)); const isValidNetworkTablesIP = (v: string | undefined): boolean => { // Check if it is a valid team number between 1-9999 const teamNumberRegex = /^[1-9][0-9]{0,3}$/; @@ -38,9 +40,31 @@ const isValidHostname = (v: string | undefined) => { return hostnameRegex.test(v); }; +const settingsHaveChanged = (): boolean => { + const a = useSettingsStore().network; + const b = tempSettingsStruct.value; + + return ( + a.ntServerAddress !== b.ntServerAddress || + a.connectionType !== b.connectionType || + a.staticIp !== b.staticIp || + a.hostname !== b.hostname || + a.runNTServer !== b.runNTServer || + a.shouldManage !== b.shouldManage || + a.shouldPublishProto !== b.shouldPublishProto || + a.canManage !== b.canManage || + a.networkManagerIface !== b.networkManagerIface || + a.setStaticCommand !== b.setStaticCommand || + a.setDHCPcommand !== b.setDHCPcommand + ); +}; + const saveGeneralSettings = () => { const changingStaticIp = useSettingsStore().network.connectionType === NetworkConnectionType.Static; + // Update with new values + Object.assign(useSettingsStore().network, tempSettingsStruct.value); + useSettingsStore() .saveGeneralSettings() .then((response) => { @@ -80,7 +104,7 @@ const saveGeneralSettings = () => { const currentNetworkInterfaceIndex = computed({ get: () => useSettingsStore().networkInterfaceNames.indexOf(useSettingsStore().network.networkManagerIface || ""), - set: (v) => (useSettingsStore().network.networkManagerIface = useSettingsStore().networkInterfaceNames[v]) + set: (v) => (tempSettingsStruct.value.networkManagerIface = useSettingsStore().networkInterfaceNames[v]) }); @@ -90,11 +114,11 @@ const currentNetworkInterfaceIndex = computed({
({ The NetworkTables Server Address is not set or is invalid. NetworkTables is unable to connect. Advanced Networking ({ ({ ({ Photon cannot detect any wired connections! Please send program logs to the developers for help. ({ > This mode is intended for debugging; it should be off for proper usage. PhotonLib will NOT work! + + + This mode is intended for debugging; it should be off for field use. You may notice a performance hit by using + this mode. + Save diff --git a/photon-client/src/stores/settings/GeneralSettingsStore.ts b/photon-client/src/stores/settings/GeneralSettingsStore.ts index 5202d8626e..9ba9fc2223 100644 --- a/photon-client/src/stores/settings/GeneralSettingsStore.ts +++ b/photon-client/src/stores/settings/GeneralSettingsStore.ts @@ -36,6 +36,7 @@ export const useSettingsStore = defineStore("settings", { staticIp: "", hostname: "photonvision", runNTServer: false, + shouldPublishProto: false, networkInterfaceNames: [ { connName: "Example Wired Connection", @@ -112,6 +113,7 @@ export const useSettingsStore = defineStore("settings", { setDHCPcommand: this.network.setDHCPcommand || "", setStaticCommand: this.network.setStaticCommand || "", shouldManage: this.network.shouldManage, + shouldPublishProto: this.network.shouldPublishProto, staticIp: this.network.staticIp }; return axios.post("/settings/general", payload); diff --git a/photon-client/src/types/SettingTypes.ts b/photon-client/src/types/SettingTypes.ts index 7119b8cd90..83bff3cd09 100644 --- a/photon-client/src/types/SettingTypes.ts +++ b/photon-client/src/types/SettingTypes.ts @@ -36,6 +36,7 @@ export interface NetworkSettings { hostname: string; runNTServer: boolean; shouldManage: boolean; + shouldPublishProto: boolean; canManage: boolean; networkManagerIface?: string; setStaticCommand?: string; diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/NetworkConfig.java b/photon-core/src/main/java/org/photonvision/common/configuration/NetworkConfig.java index 36e1efe8e3..3b53096163 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/NetworkConfig.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/NetworkConfig.java @@ -37,6 +37,7 @@ public class NetworkConfig { public String hostname = "photonvision"; public boolean runNTServer = false; public boolean shouldManage; + public boolean shouldPublishProto = false; @JsonIgnore public static final String NM_IFACE_STRING = "${interface}"; @JsonIgnore public static final String NM_IP_STRING = "${ipaddr}"; @@ -72,6 +73,7 @@ public NetworkConfig( @JsonProperty("hostname") String hostname, @JsonProperty("runNTServer") boolean runNTServer, @JsonProperty("shouldManage") boolean shouldManage, + @JsonProperty("shouldPublishProto") boolean shouldPublishProto, @JsonProperty("networkManagerIface") String networkManagerIface, @JsonProperty("setStaticCommand") String setStaticCommand, @JsonProperty("setDHCPcommand") String setDHCPcommand) { @@ -80,6 +82,7 @@ public NetworkConfig( this.staticIp = staticIp; this.hostname = hostname; this.runNTServer = runNTServer; + this.shouldPublishProto = shouldPublishProto; this.networkManagerIface = networkManagerIface; this.setStaticCommand = setStaticCommand; this.setDHCPcommand = setDHCPcommand; 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 0f668a417c..7d474b1fe9 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 @@ -22,6 +22,7 @@ import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.Supplier; +import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.CVPipelineResultConsumer; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; @@ -135,6 +136,9 @@ public void accept(CVPipelineResult result) { result.multiTagResult); ts.resultPublisher.accept(simplified, simplified.getPacketSize()); + if (ConfigManager.getInstance().getConfig().getNetworkConfig().shouldPublishProto) { + ts.protoResultPublisher.set(simplified); + } ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get()); ts.driverModePublisher.set(driverModeSupplier.getAsBoolean()); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java index f29cb2be3d..7da9715508 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/DriverModePipeline.java @@ -20,19 +20,15 @@ import java.util.List; import org.apache.commons.lang3.tuple.Pair; import org.photonvision.common.util.math.MathUtils; -import org.photonvision.raspi.LibCameraJNI; -import org.photonvision.vision.camera.CameraQuirk; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; import org.photonvision.vision.pipe.impl.CalculateFPSPipe; import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe; import org.photonvision.vision.pipe.impl.ResizeImagePipe; -import org.photonvision.vision.pipe.impl.RotateImagePipe; import org.photonvision.vision.pipeline.result.DriverModePipelineResult; public class DriverModePipeline extends CVPipeline { - private final RotateImagePipe rotateImagePipe = new RotateImagePipe(); private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe(); private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe(); private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe(); @@ -51,10 +47,6 @@ public DriverModePipeline(DriverModePipelineSettings settings) { @Override protected void setPipeParamsImpl() { - RotateImagePipe.RotateImageParams rotateImageParams = - new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode); - rotateImagePipe.setParams(rotateImageParams); - Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams = new Draw2dCrosshairPipe.Draw2dCrosshairParams( frameStaticProperties, settings.streamingFrameDivisor, settings.inputImageRotationMode); @@ -72,7 +64,6 @@ protected void setPipeParamsImpl() { @Override public DriverModePipelineResult process(Frame frame, DriverModePipelineSettings settings) { long totalNanos = 0; - boolean accelerated = LibCameraJNI.isSupported() && cameraQuirks.hasQuirk(CameraQuirk.PiCam); // apply pipes var inputMat = frame.colorImage.getMat(); @@ -80,11 +71,6 @@ public DriverModePipelineResult process(Frame frame, DriverModePipelineSettings boolean emptyIn = inputMat.empty(); if (!emptyIn) { - if (!accelerated) { - var rotateImageResult = rotateImagePipe.run(inputMat); - totalNanos += rotateImageResult.nanosElapsed; - } - totalNanos += resizeImagePipe.run(inputMat).nanosElapsed; var draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(inputMat, List.of())); diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java index 60acc0d391..7fd0266d3c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionRunner.java @@ -75,6 +75,7 @@ private void update() { // (pipeline-dependent). I kinda hate how much leak this has... // TODO would a callback object be a better fit? var wantedProcessType = pipeline.getThresholdType(); + frameSupplier.requestFrameThresholdType(wantedProcessType); var settings = pipeline.getSettings(); if (settings instanceof AdvancedPipelineSettings) { diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 099308f470..0a42bf54b6 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -60,6 +60,35 @@ task writeCurrentVersion { } build.mustRunAfter writeCurrentVersion +cppHeadersZip.dependsOn writeCurrentVersion + +// Building photon-lib requires photon-targeting to generate its proto files. This technically shouldn't be required but is needed for it to build. +model { + components { + all { + it.sources.each { + it.exportedHeaders { + srcDirs "src/main/native/include" + srcDirs "src/generate/native/include" + } + } + it.binaries.all { + it.tasks.withType(CppCompile) { + it.dependsOn ":photon-targeting:generateProto" + } + } + } + } + testSuites { + all { + it.binaries.all { + it.tasks.withType(CppCompile) { + it.dependsOn ":photon-targeting:generateProto" + } + } + } + } +} def vendorJson = artifacts.add('archives', file("$photonlibFileOutput")) diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py index fd3e0c8660..187013a286 100644 --- a/photon-lib/py/photonlibpy/photonCamera.py +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -165,11 +165,32 @@ def _versionCheck(self) -> None: versionString = self.versionEntry.get(defaultValue="") if len(versionString) > 0 and versionString != PHOTONVISION_VERSION: - wpilib.reportWarning( - "Photon version " - + PHOTONVISION_VERSION - + " does not match coprocessor version " - + versionString - + f"! Please install photonlibpy version {PHOTONLIB_VERSION}", - True, - ) + # Verified version mismatch + + bfw = """ + \n\n\n + >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> + >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> + >>> ____ _________ ____ ________ ___________ __ + >>> / __ \\/ ____/ | / __ \\ /_ __/ / / / _/ ___/ / / + >>> / /_/ / __/ / /| | / / / / / / / /_/ // / \\__ \\ / / + >>> / _, _/ /___/ ___ |/ /_/ / / / / __ // / ___/ / /_/ + >>>/_/ |_/_____/_/ |_/_____/ /_/ /_/ /_/___//____/ (_) + >>> + >>> You are running an incompatible version + >>> of PhotonVision on your coprocessor! + >>> + >>> This is neither tested nor supported. + >>> You MUST update either PhotonVision, PhotonLib, or both. + >>> + >>> Your code will now crash. We hope your day gets better. + >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> + >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> + \n\n + """ + + wpilib.reportWarning(bfw) + + errText = f"Photon version {PHOTONLIB_VERSION} does not match coprocessor version {versionString}. Please install photonlibpy version {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 c5a704460b..f202b2c520 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -374,13 +374,35 @@ else if (!isConnected()) { if (!versionString.isEmpty() && !PhotonVersion.versionMatches(versionString)) { // Error on a verified version mismatch // But stay silent otherwise - DriverStation.reportWarning( + + String bfw = + "\n\n\n\n>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n" + + ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n" + + ">>> ____ _________ ____ ________ ___________ __ \n" + + ">>> / __ \\/ ____/ | / __ \\ /_ __/ / / / _/ ___/ / / \n" + + ">>> / /_/ / __/ / /| | / / / / / / / /_/ // / \\__ \\ / / \n" + + ">>> / _, _/ /___/ ___ |/ /_/ / / / / __ // / ___/ / /_/ \n" + + ">>>/_/ |_/_____/_/ |_/_____/ /_/ /_/ /_/___//____/ (_) \n" + + ">>> \n" + + ">>> You are running an incompatible version \n" + + ">>> of PhotonVision on your coprocessor! \n" + + ">>> \n" + + ">>> This is neither tested nor supported. \n" + + ">>> You MUST update either PhotonVision, PhotonLib, or both. \n" + + ">>> \n" + + ">>> Your code will now crash. We hope your day gets better. \n" + + ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n" + + ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n\n\n"; + + DriverStation.reportWarning(bfw, false); + var versionMismatchMessage = "Photon version " + PhotonVersion.versionString + " does not match coprocessor version " + versionString - + "!", - true); + + "!"; + DriverStation.reportError(versionMismatchMessage, false); + throw new UnsupportedOperationException(versionMismatchMessage); } } } diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 02d9b46527..3ede0c3247 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -26,6 +26,8 @@ #include +#include + #include #include #include @@ -34,6 +36,29 @@ #include "PhotonVersion.h" #include "photon/dataflow/structures/Packet.h" +namespace { +static constexpr const std::string_view bfw = + "\n\n\n\n" + ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n" + ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n" + ">>> ____ _________ ____ ________ ___________ __ \n" + ">>> / __ \\/ ____/ | / __ \\ /_ __/ / / / _/ ___/ / / \n" + ">>> / /_/ / __/ / /| | / / / / / / / /_/ // / \\__ \\ / / \n" + ">>> / _, _/ /___/ ___ |/ /_/ / / / / __ // / ___/ / /_/ \n" + ">>>/_/ |_/_____/_/ |_/_____/ /_/ /_/ /_/___//____/ (_) \n" + ">>> \n" + ">>> You are running an incompatible version \n" + ">>> of PhotonVision on your coprocessor! \n" + ">>> \n" + ">>> This is neither tested nor supported. \n" + ">>> You MUST update either PhotonVision, PhotonLib, or both. \n" + ">>> \n" + ">>> Your code will now crash. We hope your day gets better. \n" + ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n" + ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n" + "\n\n"; +} // namespace + namespace photon { constexpr const units::second_t VERSION_CHECK_INTERVAL = 5_s; @@ -200,9 +225,12 @@ void PhotonCamera::VerifyVersion() { cameraNameOutString); } } else if (!VersionMatches(versionString)) { - FRC_ReportError(frc::warn::Warning, - "Photon version {} does not match coprocessor version {}!", - PhotonVersion::versionString, versionString); + FRC_ReportError(frc::warn::Warning, bfw); + std::string error_str = fmt::format( + "Photonlib version {} does not match coprocessor version {}!", + PhotonVersion::versionString, versionString); + FRC_ReportError(frc::err::Error, "{}", error_str); + throw std::runtime_error(error_str); } } 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 1fdf013477..0a9e3cdf79 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 @@ -26,6 +26,7 @@ import edu.wpi.first.networktables.IntegerSubscriber; 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 org.photonvision.targeting.PhotonPipelineResult; @@ -41,6 +42,7 @@ public class NTTopicSet { public NetworkTable subTable; public PacketPublisher resultPublisher; + public ProtobufPublisher protoResultPublisher; public IntegerPublisher pipelineIndexPublisher; public IntegerSubscriber pipelineIndexRequestSub; @@ -76,6 +78,10 @@ public void updateEntries() { .publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); resultPublisher = new PacketPublisher<>(rawBytesEntry, PhotonPipelineResult.serde); + protoResultPublisher = + subTable + .getProtobufTopic("result_proto", PhotonPipelineResult.proto) + .publish(PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish(); pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0); diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java index 03a45cb562..8cf84eb6a6 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java @@ -21,6 +21,7 @@ import java.util.List; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; +import org.photonvision.targeting.proto.MultiTargetPNPResultProto; public class MultiTargetPNPResult { // Seeing 32 apriltags at once seems like a sane limit @@ -103,4 +104,5 @@ public MultiTargetPNPResult unpack(Packet packet) { } public static final APacketSerde serde = new APacketSerde(); + public static final MultiTargetPNPResultProto proto = new MultiTargetPNPResultProto(); } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java index 6e4b595c41..e7038cca42 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java @@ -20,6 +20,7 @@ import edu.wpi.first.math.geometry.Transform3d; 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; /** @@ -180,4 +181,5 @@ public PNPResult unpack(Packet packet) { } public static final APacketSerde serde = new APacketSerde(); + public static final PNPResultProto proto = new PNPResultProto(); } 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 d55d4813e7..7f70f3bbb0 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -21,6 +21,7 @@ import java.util.List; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; +import org.photonvision.targeting.proto.PhotonPipelineResultProto; /** Represents a pipeline result from a PhotonCamera. */ public class PhotonPipelineResult { @@ -225,4 +226,5 @@ public PhotonPipelineResult unpack(Packet packet) { } public static final APacketSerde serde = new APacketSerde(); + public static final PhotonPipelineResultProto proto = new PhotonPipelineResultProto(); } 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 2a369a7292..b559fec5f9 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -22,6 +22,7 @@ import java.util.List; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; +import org.photonvision.targeting.proto.PhotonTrackedTargetProto; import org.photonvision.utils.PacketUtils; public class PhotonTrackedTarget { @@ -278,4 +279,5 @@ public PhotonTrackedTarget unpack(Packet packet) { } public static final APacketSerde serde = new APacketSerde(); + public static final PhotonTrackedTargetProto proto = new PhotonTrackedTargetProto(); } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index 38cc489594..0e27c51b82 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -20,6 +20,7 @@ import java.util.Objects; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; +import org.photonvision.targeting.proto.TargetCornerProto; /** * Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels. @@ -71,4 +72,5 @@ public TargetCorner unpack(Packet packet) { } public static final APacketSerde serde = new APacketSerde(); + public static final TargetCornerProto proto = new TargetCornerProto(); } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/MultiTargetPNPResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/MultiTargetPNPResultProto.java new file mode 100644 index 0000000000..87215f78f9 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/MultiTargetPNPResultProto.java @@ -0,0 +1,69 @@ +/* + * 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.proto; + +import edu.wpi.first.util.protobuf.Protobuf; +import java.util.ArrayList; +import org.photonvision.proto.Photon.ProtobufMultiTargetPNPResult; +import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.targeting.PNPResult; +import us.hebi.quickbuf.Descriptors.Descriptor; +import us.hebi.quickbuf.RepeatedInt; + +public class MultiTargetPNPResultProto + implements Protobuf { + @Override + public Class getTypeClass() { + return MultiTargetPNPResult.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufMultiTargetPNPResult.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {PNPResult.proto}; + } + + @Override + public ProtobufMultiTargetPNPResult createMessage() { + return ProtobufMultiTargetPNPResult.newInstance(); + } + + @Override + public MultiTargetPNPResult unpack(ProtobufMultiTargetPNPResult msg) { + ArrayList fidIdsUsed = new ArrayList<>(msg.getFiducialIdsUsed().length()); + for (var packedFidId : msg.getFiducialIdsUsed()) { + fidIdsUsed.add(packedFidId); + } + + return new MultiTargetPNPResult(PNPResult.proto.unpack(msg.getEstimatedPose()), fidIdsUsed); + } + + @Override + public void pack(ProtobufMultiTargetPNPResult msg, MultiTargetPNPResult value) { + PNPResult.proto.pack(msg.getMutableEstimatedPose(), value.estimatedPose); + + RepeatedInt idsUsed = msg.getMutableFiducialIdsUsed().reserve(value.fiducialIDsUsed.size()); + for (int i = 0; i < value.fiducialIDsUsed.size(); i++) { + idsUsed.add(value.fiducialIDsUsed.get(i)); + } + } +} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PNPResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PNPResultProto.java new file mode 100644 index 0000000000..f75d4886c4 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PNPResultProto.java @@ -0,0 +1,70 @@ +/* + * 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.proto; + +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.util.protobuf.Protobuf; +import org.photonvision.proto.Photon.ProtobufPNPResult; +import org.photonvision.targeting.PNPResult; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class PNPResultProto implements Protobuf { + @Override + public Class getTypeClass() { + return PNPResult.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPNPResult.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Transform3d.proto}; + } + + @Override + public ProtobufPNPResult createMessage() { + return ProtobufPNPResult.newInstance(); + } + + @Override + public PNPResult unpack(ProtobufPNPResult msg) { + if (!msg.getIsPresent()) { + return new PNPResult(); + } + + return new PNPResult( + Transform3d.proto.unpack(msg.getBest()), + Transform3d.proto.unpack(msg.getAlt()), + msg.getAmbiguity(), + msg.getBestReprojErr(), + msg.getAltReprojErr()); + } + + @Override + public void pack(ProtobufPNPResult msg, PNPResult value) { + Transform3d.proto.pack(msg.getMutableBest(), value.best); + Transform3d.proto.pack(msg.getMutableAlt(), value.alt); + msg.setAmbiguity(value.ambiguity) + .setBestReprojErr(value.bestReprojErr) + .setAltReprojErr(value.altReprojErr) + .setIsPresent(value.isPresent); + } +} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java new file mode 100644 index 0000000000..dbbc180658 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java @@ -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 . + */ + +package org.photonvision.targeting.proto; + +import edu.wpi.first.util.protobuf.Protobuf; +import org.photonvision.proto.Photon.ProtobufPhotonPipelineResult; +import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class PhotonPipelineResultProto + implements Protobuf { + @Override + public Class getTypeClass() { + return PhotonPipelineResult.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPhotonPipelineResult.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {PhotonTrackedTarget.proto, MultiTargetPNPResult.proto}; + } + + @Override + public ProtobufPhotonPipelineResult createMessage() { + return ProtobufPhotonPipelineResult.newInstance(); + } + + @Override + public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) { + return new PhotonPipelineResult( + msg.getLatencyMs(), + PhotonTrackedTarget.proto.unpack(msg.getTargets()), + MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult())); + } + + @Override + public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) { + PhotonTrackedTarget.proto.pack(msg.getMutableTargets(), value.getTargets()); + MultiTargetPNPResult.proto.pack(msg.getMutableMultiTargetResult(), value.getMultiTagResult()); + + msg.setLatencyMs(value.getLatencyMillis()); + } +} 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 new file mode 100644 index 0000000000..1bbceb9053 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java @@ -0,0 +1,99 @@ +/* + * 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.proto; + +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.util.protobuf.Protobuf; +import java.util.ArrayList; +import java.util.List; +import org.photonvision.proto.Photon.ProtobufPhotonTrackedTarget; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; +import us.hebi.quickbuf.Descriptors.Descriptor; +import us.hebi.quickbuf.RepeatedMessage; + +public class PhotonTrackedTargetProto + implements Protobuf { + @Override + public Class getTypeClass() { + return PhotonTrackedTarget.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPhotonTrackedTarget.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Transform3d.proto, TargetCorner.proto}; + } + + @Override + public ProtobufPhotonTrackedTarget createMessage() { + return ProtobufPhotonTrackedTarget.newInstance(); + } + + @Override + public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) { + return new PhotonTrackedTarget( + msg.getYaw(), + msg.getPitch(), + msg.getArea(), + msg.getSkew(), + msg.getFiducialId(), + Transform3d.proto.unpack(msg.getBestCameraToTarget()), + Transform3d.proto.unpack(msg.getAltCameraToTarget()), + msg.getPoseAmbiguity(), + TargetCorner.proto.unpack(msg.getMinAreaRectCorners()), + TargetCorner.proto.unpack(msg.getDetectedCorners())); + } + + public List unpack(RepeatedMessage msg) { + ArrayList targets = new ArrayList<>(msg.length()); + for (ProtobufPhotonTrackedTarget target : msg) { + targets.add(unpack(target)); + } + return targets; + } + + @Override + public void pack(ProtobufPhotonTrackedTarget msg, PhotonTrackedTarget value) { + msg.setYaw(value.getYaw()) + .setPitch(value.getPitch()) + .setSkew(value.getSkew()) + .setArea(value.getArea()) + .setFiducialId(value.getFiducialId()) + .setPoseAmbiguity(value.getPoseAmbiguity()); + + Transform3d.proto.pack(msg.getMutableBestCameraToTarget(), value.getBestCameraToTarget()); + Transform3d.proto.pack(msg.getMutableAltCameraToTarget(), value.getAlternateCameraToTarget()); + + TargetCorner.proto.pack(msg.getMutableMinAreaRectCorners(), value.getMinAreaRectCorners()); + TargetCorner.proto.pack(msg.getMutableDetectedCorners(), value.getDetectedCorners()); + } + + public void pack( + RepeatedMessage msg, List value) { + var targets = msg.reserve(value.size()); + for (PhotonTrackedTarget trackedTarget : value) { + var target = targets.next(); + pack(target, trackedTarget); + } + } +} diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/proto/TargetCornerProto.java b/photon-targeting/src/main/java/org/photonvision/targeting/proto/TargetCornerProto.java new file mode 100644 index 0000000000..1067320272 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/targeting/proto/TargetCornerProto.java @@ -0,0 +1,69 @@ +/* + * 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.proto; + +import edu.wpi.first.util.protobuf.Protobuf; +import java.util.ArrayList; +import java.util.List; +import org.photonvision.proto.Photon.ProtobufTargetCorner; +import org.photonvision.targeting.TargetCorner; +import us.hebi.quickbuf.Descriptors.Descriptor; +import us.hebi.quickbuf.RepeatedMessage; + +public class TargetCornerProto implements Protobuf { + @Override + public Class getTypeClass() { + return TargetCorner.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTargetCorner.getDescriptor(); + } + + @Override + public ProtobufTargetCorner createMessage() { + return ProtobufTargetCorner.newInstance(); + } + + @Override + public TargetCorner unpack(ProtobufTargetCorner msg) { + return new TargetCorner(msg.getX(), msg.getY()); + } + + public List unpack(RepeatedMessage msg) { + ArrayList corners = new ArrayList<>(msg.length()); + for (ProtobufTargetCorner corner : msg) { + corners.add(unpack(corner)); + } + return corners; + } + + @Override + public void pack(ProtobufTargetCorner msg, TargetCorner value) { + msg.setX(value.x).setY(value.y); + } + + public void pack(RepeatedMessage msg, List value) { + var corners = msg.reserve(value.size()); + for (TargetCorner targetCorner : value) { + var corner = corners.next(); + pack(corner, targetCorner); + } + } +} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp index c901ad65b4..34740a40e7 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp @@ -19,12 +19,12 @@ namespace photon { PhotonPipelineResult::PhotonPipelineResult( - units::second_t latency, std::span targets) + units::millisecond_t latency, std::span targets) : latency(latency), targets(targets.data(), targets.data() + targets.size()) {} PhotonPipelineResult::PhotonPipelineResult( - units::second_t latency, std::span targets, + units::millisecond_t latency, std::span targets, MultiTargetPNPResult multitagResult) : latency(latency), targets(targets.data(), targets.data() + targets.size()), @@ -37,7 +37,7 @@ bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const { Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { // Encode latency and number of targets. - packet << result.latency.value() * 1000 << result.multitagResult + packet << result.latency.value() << result.multitagResult << static_cast(result.targets.size()); // Encode the information of each target. @@ -52,7 +52,7 @@ Packet& operator>>(Packet& packet, PhotonPipelineResult& result) { double latencyMillis = 0; int8_t targetCount = 0; packet >> latencyMillis >> result.multitagResult >> targetCount; - result.latency = units::second_t(latencyMillis / 1000.0); + result.latency = units::millisecond_t(latencyMillis); result.targets.clear(); 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 new file mode 100644 index 0000000000..4289dbf3c7 --- /dev/null +++ b/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp @@ -0,0 +1,56 @@ +/* + * 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 . + */ + +#include "photon/targeting/proto/MultiTargetPNPResultProto.h" + +#include "photon.pb.h" +#include "photon/targeting/proto/PNPResultProto.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufMultiTargetPNPResult>(arena); +} + +photon::MultiTargetPNPResult +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast( + &msg); + + wpi::SmallVector fiducialIdsUsed; + for (int i = 0; i < m->fiducial_ids_used_size(); i++) { + fiducialIdsUsed.push_back(m->fiducial_ids_used(i)); + } + + return photon::MultiTargetPNPResult{ + wpi::UnpackProtobuf(m->estimated_pose()), + fiducialIdsUsed}; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const photon::MultiTargetPNPResult& value) { + auto m = static_cast(msg); + + wpi::PackProtobuf(m->mutable_estimated_pose(), value.result); + + m->clear_fiducial_ids_used(); + for (const auto& t : value.fiducialIdsUsed) { + m->add_fiducial_ids_used(t); + } +} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PNPResultProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PNPResultProto.cpp new file mode 100644 index 0000000000..464bd7c479 --- /dev/null +++ b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PNPResultProto.cpp @@ -0,0 +1,54 @@ +/* + * 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 . + */ + +#include "photon/targeting/proto/PNPResultProto.h" + +#include "photon.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufPNPResult>(arena); +} + +photon::PNPResult wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); + + if (!m->is_present()) { + return photon::PNPResult(); + } + + return photon::PNPResult{true, + wpi::UnpackProtobuf(m->best()), + m->best_reproj_err(), + wpi::UnpackProtobuf(m->alt()), + m->alt_reproj_err(), + m->ambiguity()}; +} + +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const photon::PNPResult& value) { + auto m = static_cast(msg); + + m->set_is_present(value.isPresent); + wpi::PackProtobuf(m->mutable_best(), value.best); + m->set_best_reproj_err(value.bestReprojErr); + wpi::PackProtobuf(m->mutable_alt(), value.alt); + m->set_alt_reproj_err(value.altReprojErr); + m->set_ambiguity(value.ambiguity); +} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp new file mode 100644 index 0000000000..3605ae5946 --- /dev/null +++ b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp @@ -0,0 +1,61 @@ +/* + * 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 . + */ + +#include "photon/targeting/proto/PhotonPipelineResultProto.h" + +#include "photon.pb.h" +#include "photon/targeting/proto/MultiTargetPNPResultProto.h" +#include "photon/targeting/proto/PhotonTrackedTargetProto.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufPhotonPipelineResult>(arena); +} + +photon::PhotonPipelineResult +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast( + &msg); + + std::vector targets; + targets.reserve(m->targets_size()); + for (const auto& t : m->targets()) { + targets.emplace_back(wpi::UnpackProtobuf(t)); + } + + return photon::PhotonPipelineResult{ + units::millisecond_t{m->latency_ms()}, targets, + wpi::UnpackProtobuf( + m->multi_target_result())}; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const photon::PhotonPipelineResult& value) { + auto m = static_cast(msg); + + m->set_latency_ms(value.latency.value()); + + m->clear_targets(); + for (const auto& t : value.GetTargets()) { + wpi::PackProtobuf(m->add_targets(), t); + } + + wpi::PackProtobuf(m->mutable_multi_target_result(), value.multitagResult); +} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonTrackedTargetProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonTrackedTargetProto.cpp new file mode 100644 index 0000000000..352a933835 --- /dev/null +++ b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonTrackedTargetProto.cpp @@ -0,0 +1,84 @@ +/* + * 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 . + */ + +#include "photon/targeting/proto/PhotonTrackedTargetProto.h" + +#include "photon.pb.h" + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufPhotonTrackedTarget>(arena); +} + +photon::PhotonTrackedTarget wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast( + &msg); + + wpi::SmallVector, 4> minAreaRectCorners; + for (const auto& t : m->min_area_rect_corners()) { + minAreaRectCorners.emplace_back(t.x(), t.y()); + } + + std::vector> detectedCorners; + detectedCorners.reserve(m->detected_corners_size()); + for (const auto& t : m->detected_corners()) { + detectedCorners.emplace_back(t.x(), t.y()); + } + + return photon::PhotonTrackedTarget{ + m->yaw(), + m->pitch(), + m->area(), + m->skew(), + m->fiducial_id(), + wpi::UnpackProtobuf(m->best_camera_to_target()), + wpi::UnpackProtobuf(m->alt_camera_to_target()), + m->pose_ambiguity(), + minAreaRectCorners, + detectedCorners}; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const photon::PhotonTrackedTarget& value) { + auto m = static_cast(msg); + + m->set_yaw(value.yaw); + m->set_pitch(value.pitch); + m->set_area(value.area); + m->set_skew(value.skew); + m->set_fiducial_id(value.fiducialId); + wpi::PackProtobuf(m->mutable_best_camera_to_target(), + value.bestCameraToTarget); + wpi::PackProtobuf(m->mutable_alt_camera_to_target(), value.altCameraToTarget); + m->set_pose_ambiguity(value.poseAmbiguity); + + m->clear_min_area_rect_corners(); + for (const auto& t : value.GetMinAreaRectCorners()) { + auto* corner = m->add_min_area_rect_corners(); + corner->set_x(t.first); + corner->set_y(t.second); + } + + m->clear_detected_corners(); + for (const auto& t : value.GetDetectedCorners()) { + auto* corner = m->add_detected_corners(); + corner->set_x(t.first); + corner->set_y(t.second); + } +} diff --git a/photon-targeting/src/main/native/include/geometry3d.pb.h b/photon-targeting/src/main/native/include/geometry3d.pb.h new file mode 100644 index 0000000000..3cff26d7e0 --- /dev/null +++ b/photon-targeting/src/main/native/include/geometry3d.pb.h @@ -0,0 +1,22 @@ +/* + * 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 . + */ + +#pragma once +// So wpilib publishes protbufs here at wpimath/protobuf. but generated code +// assumes that the protobuf includes are on your include path. So we need this +// stupid shim +#include "wpimath/protobuf/geometry3d.pb.h" diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h index 16aa6d5f6e..c0965fbd9c 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -44,7 +44,7 @@ class PhotonPipelineResult { * @param latency The latency in the pipeline. * @param targets The list of targets identified by the pipeline. */ - PhotonPipelineResult(units::second_t latency, + PhotonPipelineResult(units::millisecond_t latency, std::span targets); /** @@ -53,7 +53,7 @@ class PhotonPipelineResult { * @param targets The list of targets identified by the pipeline. * @param multitagResult The multitarget result */ - PhotonPipelineResult(units::second_t latency, + PhotonPipelineResult(units::millisecond_t latency, std::span targets, MultiTargetPNPResult multitagResult); @@ -81,7 +81,7 @@ class PhotonPipelineResult { * Returns the latency in the pipeline. * @return The latency in the pipeline. */ - units::second_t GetLatency() const { return latency; } + units::millisecond_t GetLatency() const { return latency; } /** * Returns the estimated time the frame was taken, @@ -125,7 +125,7 @@ class PhotonPipelineResult { friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result); friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result); - units::second_t latency = 0_s; + units::millisecond_t latency = 0_s; units::second_t timestamp = -1_s; wpi::SmallVector targets; MultiTargetPNPResult multitagResult; diff --git a/photon-targeting/src/main/native/include/photon/targeting/proto/MultiTargetPNPResultProto.h b/photon-targeting/src/main/native/include/photon/targeting/proto/MultiTargetPNPResultProto.h new file mode 100644 index 0000000000..8d62863005 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/targeting/proto/MultiTargetPNPResultProto.h @@ -0,0 +1,31 @@ +/* + * 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 . + */ + +#pragma once + +#include + +#include "photon/targeting/MultiTargetPNPResult.h" + +template <> +struct wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static photon::MultiTargetPNPResult Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const photon::MultiTargetPNPResult& value); +}; diff --git a/photon-targeting/src/main/native/include/photon/targeting/proto/PNPResultProto.h b/photon-targeting/src/main/native/include/photon/targeting/proto/PNPResultProto.h new file mode 100644 index 0000000000..a3c3abcb4d --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/targeting/proto/PNPResultProto.h @@ -0,0 +1,30 @@ +/* + * 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 . + */ + +#pragma once + +#include + +#include "photon/targeting/PNPResult.h" + +template <> +struct wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static photon::PNPResult Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const photon::PNPResult& value); +}; diff --git a/photon-targeting/src/main/native/include/photon/targeting/proto/PhotonPipelineResultProto.h b/photon-targeting/src/main/native/include/photon/targeting/proto/PhotonPipelineResultProto.h new file mode 100644 index 0000000000..72434fa136 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/targeting/proto/PhotonPipelineResultProto.h @@ -0,0 +1,31 @@ +/* + * 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 . + */ + +#pragma once + +#include + +#include "photon/targeting/PhotonPipelineResult.h" + +template <> +struct wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static photon::PhotonPipelineResult Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const photon::PhotonPipelineResult& value); +}; diff --git a/photon-targeting/src/main/native/include/photon/targeting/proto/PhotonTrackedTargetProto.h b/photon-targeting/src/main/native/include/photon/targeting/proto/PhotonTrackedTargetProto.h new file mode 100644 index 0000000000..030ab0e030 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/targeting/proto/PhotonTrackedTargetProto.h @@ -0,0 +1,31 @@ +/* + * 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 . + */ + +#pragma once + +#include + +#include "photon/targeting/PhotonTrackedTarget.h" + +template <> +struct wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static photon::PhotonTrackedTarget Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const photon::PhotonTrackedTarget& value); +}; diff --git a/photon-targeting/src/main/proto/photon.proto b/photon-targeting/src/main/proto/photon.proto new file mode 100644 index 0000000000..d5b75802fc --- /dev/null +++ b/photon-targeting/src/main/proto/photon.proto @@ -0,0 +1,63 @@ +/* + * 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 . + */ + +syntax = "proto3"; + +package photonvision.proto; + +import "geometry3d.proto"; + +option java_package = "org.photonvision.proto"; + +message ProtobufTargetCorner { + double x = 1; + double y = 2; +} + +message ProtobufPNPResult { + bool is_present = 1; + wpi.proto.ProtobufTransform3d best = 2; + double best_reproj_err = 3; + optional wpi.proto.ProtobufTransform3d alt = 4; + optional double alt_reproj_err = 5; + double ambiguity = 6; +} + +message ProtobufMultiTargetPNPResult { + ProtobufPNPResult estimated_pose = 1; + repeated int32 fiducial_ids_used = 2; +} + +message ProtobufPhotonTrackedTarget { + double yaw = 1; + double pitch = 2; + double area = 3; + double skew = 4; + int32 fiducial_id = 5; + wpi.proto.ProtobufTransform3d best_camera_to_target = 6; + wpi.proto.ProtobufTransform3d alt_camera_to_target = 7; + double pose_ambiguity = 8; + repeated ProtobufTargetCorner min_area_rect_corners = 9; + repeated ProtobufTargetCorner detected_corners = 10; +} + +message ProtobufPhotonPipelineResult { + double latency_ms = 1; + + repeated ProtobufPhotonTrackedTarget targets = 2; + ProtobufMultiTargetPNPResult multi_target_result = 3; +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/proto/MultiTargetPNPResultProtoTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/proto/MultiTargetPNPResultProtoTest.java new file mode 100644 index 0000000000..e3ea854ce1 --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/proto/MultiTargetPNPResultProtoTest.java @@ -0,0 +1,49 @@ +/* + * 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.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import org.junit.jupiter.api.Test; +import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.targeting.PNPResult; + +public class MultiTargetPNPResultProtoTest { + @Test + public void protobufTest() { + var result = new MultiTargetPNPResult(); + var serializedResult = MultiTargetPNPResult.proto.createMessage(); + MultiTargetPNPResult.proto.pack(serializedResult, result); + var unpackedResult = MultiTargetPNPResult.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + + result = + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3)); + serializedResult = MultiTargetPNPResult.proto.createMessage(); + MultiTargetPNPResult.proto.pack(serializedResult, result); + unpackedResult = MultiTargetPNPResult.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PNPResultProtoTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PNPResultProtoTest.java new file mode 100644 index 0000000000..49d18434db --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PNPResultProtoTest.java @@ -0,0 +1,42 @@ +/* + * 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.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import org.junit.jupiter.api.Test; +import org.photonvision.targeting.PNPResult; + +public class PNPResultProtoTest { + @Test + public void protobufTest() { + var pnpRes = new PNPResult(); + var serializedPNPRes = PNPResult.proto.createMessage(); + PNPResult.proto.pack(serializedPNPRes, pnpRes); + var unpackedPNPRes = PNPResult.proto.unpack(serializedPNPRes); + assertEquals(pnpRes, unpackedPNPRes); + + pnpRes = new PNPResult(new Transform3d(1, 2, 3, new Rotation3d(1, 2, 3)), 0.1); + serializedPNPRes = PNPResult.proto.createMessage(); + PNPResult.proto.pack(serializedPNPRes, pnpRes); + unpackedPNPRes = PNPResult.proto.unpack(serializedPNPRes); + assertEquals(pnpRes, unpackedPNPRes); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java new file mode 100644 index 0000000000..c745fab8a3 --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonPipelineResultProtoTest.java @@ -0,0 +1,139 @@ +/* + * 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.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import org.junit.jupiter.api.Test; +import org.photonvision.targeting.*; + +public class PhotonPipelineResultProtoTest { + @Test + public void protobufTest() { + // Empty Result + var result = new PhotonPipelineResult(); + var serializedResult = PhotonPipelineResult.proto.createMessage(); + PhotonPipelineResult.proto.pack(serializedResult, result); + var unpackedResult = PhotonPipelineResult.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + + // non multitag result + result = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + serializedResult = PhotonPipelineResult.proto.createMessage(); + PhotonPipelineResult.proto.pack(serializedResult, result); + unpackedResult = PhotonPipelineResult.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + + // multitag result + result = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3))); + serializedResult = PhotonPipelineResult.proto.createMessage(); + PhotonPipelineResult.proto.pack(serializedResult, result); + unpackedResult = PhotonPipelineResult.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonTrackedTargetProtoTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonTrackedTargetProtoTest.java new file mode 100644 index 0000000000..9aa88c1fca --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/proto/PhotonTrackedTargetProtoTest.java @@ -0,0 +1,115 @@ +/* + * 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.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import org.junit.jupiter.api.Test; +import org.photonvision.proto.Photon.ProtobufPhotonTrackedTarget; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; +import us.hebi.quickbuf.RepeatedMessage; + +public class PhotonTrackedTargetProtoTest { + @Test + public void protobufTest() { + var target = + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + var serializedTarget = PhotonTrackedTarget.proto.createMessage(); + PhotonTrackedTarget.proto.pack(serializedTarget, target); + var unpackedTarget = PhotonTrackedTarget.proto.unpack(serializedTarget); + assertEquals(target, unpackedTarget); + } + + @Test + public void protobufListTest() { + List targets = List.of(); + var serializedTargets = + RepeatedMessage.newEmptyInstance(ProtobufPhotonTrackedTarget.getFactory()); + PhotonTrackedTarget.proto.pack(serializedTargets, targets); + var unpackedTargets = PhotonTrackedTarget.proto.unpack(serializedTargets); + assertEquals(targets, unpackedTargets); + + targets = + List.of( + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 7.0, + 2.0, + 1.0, + -9.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))); + serializedTargets = RepeatedMessage.newEmptyInstance(ProtobufPhotonTrackedTarget.getFactory()); + PhotonTrackedTarget.proto.pack(serializedTargets, targets); + unpackedTargets = PhotonTrackedTarget.proto.unpack(serializedTargets); + assertEquals(targets, unpackedTargets); + } +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/proto/TargetCornerProtoTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/proto/TargetCornerProtoTest.java new file mode 100644 index 0000000000..432e43d3e8 --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/targeting/proto/TargetCornerProtoTest.java @@ -0,0 +1,34 @@ +/* + * 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.proto; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; +import org.photonvision.targeting.TargetCorner; + +public class TargetCornerProtoTest { + @Test + public void protobufTest() { + var corner = new TargetCorner(0, 1); + var serializedCorner = TargetCorner.proto.createMessage(); + TargetCorner.proto.pack(serializedCorner, corner); + var unpackedCorner = TargetCorner.proto.unpack(serializedCorner); + assertEquals(corner, unpackedCorner); + } +} diff --git a/photon-targeting/src/test/native/cpp/targeting/proto/MultiTargetPNPResultProtoTest.cpp b/photon-targeting/src/test/native/cpp/targeting/proto/MultiTargetPNPResultProtoTest.cpp new file mode 100644 index 0000000000..df630ec5c5 --- /dev/null +++ b/photon-targeting/src/test/native/cpp/targeting/proto/MultiTargetPNPResultProtoTest.cpp @@ -0,0 +1,55 @@ +/* + * 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 . + */ + +#include "gtest/gtest.h" +#include "photon.pb.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/proto/MultiTargetPNPResultProto.h" + +TEST(MultiTargetPNPResultTest, Roundtrip) { + photon::MultiTargetPNPResult result; + + google::protobuf::Arena arena; + google::protobuf::Message* proto = + wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result); + + photon::MultiTargetPNPResult unpacked_data = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result, unpacked_data); + + photon::PNPResult pnpRes{ + true, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, + 0}; + + photon::MultiTargetPNPResult result1{pnpRes, {1, 2, 3, 4}}; + + proto = wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result1); + + photon::MultiTargetPNPResult unpacked_data1 = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result1, unpacked_data1); +} diff --git a/photon-targeting/src/test/native/cpp/targeting/proto/PNPResultProtoTest.cpp b/photon-targeting/src/test/native/cpp/targeting/proto/PNPResultProtoTest.cpp new file mode 100644 index 0000000000..2b6cb2fcc0 --- /dev/null +++ b/photon-targeting/src/test/native/cpp/targeting/proto/PNPResultProtoTest.cpp @@ -0,0 +1,53 @@ +/* + * 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 . + */ + +#include "gtest/gtest.h" +#include "photon.pb.h" +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/proto/PNPResultProto.h" + +TEST(PNPResultTest, Roundtrip) { + photon::PNPResult result; + + google::protobuf::Arena arena; + google::protobuf::Message* proto = + wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result); + + photon::PNPResult unpacked_data = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result, unpacked_data); + + photon::PNPResult result1{ + true, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, + 0}; + + proto = wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result1); + + photon::PNPResult unpacked_data2 = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result1, unpacked_data2); +} diff --git a/photon-targeting/src/test/native/cpp/targeting/proto/PhotonPipelineResultProtoTest.cpp b/photon-targeting/src/test/native/cpp/targeting/proto/PhotonPipelineResultProtoTest.cpp new file mode 100644 index 0000000000..65c60f0d13 --- /dev/null +++ b/photon-targeting/src/test/native/cpp/targeting/proto/PhotonPipelineResultProtoTest.cpp @@ -0,0 +1,96 @@ +/* + * 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 . + */ + +#include "gtest/gtest.h" +#include "photon.pb.h" +#include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/proto/PhotonPipelineResultProto.h" + +TEST(PhotonPipelineResultTest, Roundtrip) { + photon::PhotonPipelineResult result{12_ms, {}}; + + google::protobuf::Arena arena; + google::protobuf::Message* proto = + wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result); + + photon::PhotonPipelineResult unpacked_data = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result, unpacked_data); + + wpi::SmallVector targets{ + photon::PhotonTrackedTarget{ + 3.0, + -4.0, + 9.0, + 4.0, + 1, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + -1, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}, + photon::PhotonTrackedTarget{ + 3.0, + -4.0, + 9.1, + 6.7, + -1, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + -1, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, + std::pair{7, 8}}}}; + + photon::PhotonPipelineResult result2{12_ms, targets}; + + proto = wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result2); + + photon::PhotonPipelineResult unpacked_data2 = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result2, unpacked_data2); + + photon::PNPResult pnpRes{ + true, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + 0.1, + 0}; + + photon::MultiTargetPNPResult multitagRes{pnpRes, {1, 2, 3, 4}}; + + photon::PhotonPipelineResult result3{12_ms, targets, multitagRes}; + + proto = wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result3); + + photon::PhotonPipelineResult unpacked_data3 = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result3, unpacked_data3); +} diff --git a/photon-targeting/src/test/native/cpp/targeting/proto/PhotonTrackedTargetProtoTest.cpp b/photon-targeting/src/test/native/cpp/targeting/proto/PhotonTrackedTargetProtoTest.cpp new file mode 100644 index 0000000000..73ef41f77c --- /dev/null +++ b/photon-targeting/src/test/native/cpp/targeting/proto/PhotonTrackedTargetProtoTest.cpp @@ -0,0 +1,47 @@ +/* + * 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 . + */ + +#include "gtest/gtest.h" +#include "photon.pb.h" +#include "photon/targeting/PhotonTrackedTarget.h" +#include "photon/targeting/proto/PhotonTrackedTargetProto.h" + +TEST(PhotonTrackedTargetTest, Roundtrip) { + photon::PhotonTrackedTarget target{ + 3.0, + 4.0, + 9.0, + -5.0, + -1, + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), + frc::Rotation3d(1_rad, 2_rad, 3_rad)), + -1, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, + {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}; + + google::protobuf::Arena arena; + google::protobuf::Message* proto = + wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, target); + + photon::PhotonTrackedTarget unpacked_data = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(target, unpacked_data); +} diff --git a/shared/javacommon.gradle b/shared/javacommon.gradle index 55f72d0f60..49a998e198 100644 --- a/shared/javacommon.gradle +++ b/shared/javacommon.gradle @@ -1,6 +1,7 @@ apply plugin: 'maven-publish' apply plugin: 'java-library' apply plugin: 'jacoco' +apply plugin: 'com.google.protobuf' java { sourceCompatibility = JavaVersion.VERSION_11 @@ -138,3 +139,27 @@ jacocoTestReport { html.required = true } } + +protobuf { + protoc { + artifact = 'com.google.protobuf:protoc:3.21.12' + } + plugins { + quickbuf { + artifact = 'us.hebi.quickbuf:protoc-gen-quickbuf:1.3.3' + } + } + generateProtoTasks { + all().configureEach { task -> + task.builtins { + cpp {} + remove java + } + task.plugins { + quickbuf { + option "gen_descriptors=true" + } + } + } + } +} diff --git a/shared/javacpp/setupBuild.gradle b/shared/javacpp/setupBuild.gradle index dd8a5bec32..233fb4d5ec 100644 --- a/shared/javacpp/setupBuild.gradle +++ b/shared/javacpp/setupBuild.gradle @@ -38,11 +38,11 @@ model { sources { cpp { source { - srcDirs 'src/main/native/cpp' - include '**/*.cpp' + srcDirs 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp" + include '**/*.cpp', '**/*.cc' } exportedHeaders { - srcDirs 'src/main/native/include' + srcDirs 'src/main/native/include', "$buildDir/generated/source/proto/main/cpp" if (project.hasProperty('generatedHeaders')) { srcDir generatedHeaders } @@ -51,8 +51,11 @@ model { } } - if(project.hasProperty('includePhotonTargeting')) { - binaries.all { + binaries.all { + it.tasks.withType(CppCompile) { + it.dependsOn generateProto + } + if(project.hasProperty('includePhotonTargeting')) { lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared' } } @@ -77,13 +80,16 @@ model { include '**/*.cpp' } exportedHeaders { - srcDirs 'src/test/native/include', 'src/main/native/cpp' + srcDirs 'src/test/native/include', "$buildDir/generated/source/proto/main/cpp" } } } - if(project.hasProperty('includePhotonTargeting')) { - binaries.all { + binaries.all { + it.tasks.withType(CppCompile) { + it.dependsOn generateProto + } + if(project.hasProperty('includePhotonTargeting')) { lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared' } }