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/cameras/CameraCalibrationInfoCard.vue b/photon-client/src/components/cameras/CameraCalibrationInfoCard.vue index ecd8bb791f..42bde716f7 100644 --- a/photon-client/src/components/cameras/CameraCalibrationInfoCard.vue +++ b/photon-client/src/components/cameras/CameraCalibrationInfoCard.vue @@ -166,9 +166,41 @@ onBeforeMount(() => { diff --git a/photon-client/src/components/dashboard/tabs/TargetsTab.vue b/photon-client/src/components/dashboard/tabs/TargetsTab.vue index 11f20f0810..ff624fb77e 100644 --- a/photon-client/src/components/dashboard/tabs/TargetsTab.vue +++ b/photon-client/src/components/dashboard/tabs/TargetsTab.vue @@ -3,12 +3,32 @@ import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore"; import { PipelineType } from "@/types/PipelineTypes"; import { useStateStore } from "@/stores/StateStore"; +const wrapToPi = (delta: number): number => { + let ret = delta; + while (ret < -Math.PI) ret += Math.PI * 2; + while (ret > Math.PI) ret -= Math.PI * 2; + return ret; +}; + const calculateStdDev = (values: number[]): number => { if (values.length < 2) return 0; - const mean = values.reduce((sum, number) => sum + number, 0) / values.length; + // Use mean of cosine/sine components to handle angle wrapping + const cosines = values.map((it) => Math.cos(it)); + const sines = values.map((it) => Math.sin(it)); + const cosmean = cosines.reduce((sum, number) => sum + number, 0) / values.length; + const sinmean = sines.reduce((sum, number) => sum + number, 0) / values.length; + + // Borrowed from WPILib's Rotation2d + const hypot = Math.hypot(cosmean, sinmean); + let mean; + if (hypot > 1e-6) { + mean = Math.atan2(sinmean / hypot, cosmean / hypot); + } else { + mean = 0; + } - return Math.sqrt(values.map((x) => Math.pow(x - mean, 2)).reduce((a, b) => a + b) / values.length); + return Math.sqrt(values.map((x) => Math.pow(wrapToPi(x - mean), 2)).reduce((a, b) => a + b) / values.length); }; const resetCurrentBuffer = () => { // Need to clear the array in place @@ -119,7 +139,7 @@ const resetCurrentBuffer = () => { {{ ( - useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x * + (useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_x || 0) * (180.0 / Math.PI) ).toFixed(2) }}° @@ -127,7 +147,7 @@ const resetCurrentBuffer = () => { {{ ( - useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y * + (useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_y || 0) * (180.0 / Math.PI) ).toFixed(2) }}° @@ -135,7 +155,7 @@ const resetCurrentBuffer = () => { {{ ( - useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z * + (useStateStore().currentPipelineResults?.multitagResult?.bestTransform.angle_z || 0) * (180.0 / Math.PI) ).toFixed(2) }}° @@ -146,8 +166,8 @@ const resetCurrentBuffer = () => { Multi-tag pose standard deviation over the last {{ useStateStore().currentMultitagBuffer.length }}/100 - samples + >Multi-tag pose standard deviation over the last + {{ useStateStore().currentMultitagBuffer?.length || "NaN" }}/100 samples Reset Samples { {{ - calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.x)).toFixed(5) + calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.x) || []).toFixed(5) }} m {{ - calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.y)).toFixed(5) + calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.y) || []).toFixed(5) }} m {{ - calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.z)).toFixed(5) + calculateStdDev(useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.z) || []).toFixed(5) }} m {{ calculateStdDev( - useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_x * (180.0 / Math.PI)) + useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_x * (180.0 / Math.PI)) || [] ).toFixed(5) }}° {{ calculateStdDev( - useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_y * (180.0 / Math.PI)) + useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_y * (180.0 / Math.PI)) || [] ).toFixed(5) }}° {{ calculateStdDev( - useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_z * (180.0 / Math.PI)) + useStateStore().currentMultitagBuffer?.map((v) => v.bestTransform.angle_z * (180.0 / Math.PI)) || [] ).toFixed(5) }}° 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 cc134268ce..4994530760 100644 --- a/photon-client/src/types/SettingTypes.ts +++ b/photon-client/src/types/SettingTypes.ts @@ -37,6 +37,7 @@ export interface NetworkSettings { hostname: string; runNTServer: boolean; shouldManage: boolean; + shouldPublishProto: boolean; canManage: boolean; networkManagerIface?: string; setStaticCommand?: string; diff --git a/photon-client/src/types/WebsocketDataTypes.ts b/photon-client/src/types/WebsocketDataTypes.ts index d63e3fd8f8..15a5dc26c2 100644 --- a/photon-client/src/types/WebsocketDataTypes.ts +++ b/photon-client/src/types/WebsocketDataTypes.ts @@ -27,15 +27,6 @@ export interface WebsocketNumberPair { second: number; } -export interface WebsocketCompleteCalib { - distCoeffs: number[]; - height: number; - width: number; - standardDeviation: number; - perViewErrors: number[]; - intrinsics: number[]; -} - export type WebsocketVideoFormat = Record< number, { diff --git a/photon-core/src/main/java/org/photonvision/common/ProgramStatus.java b/photon-core/src/main/java/org/photonvision/common/ProgramStatus.java index a1ba6d091d..bc3bc1e1f0 100644 --- a/photon-core/src/main/java/org/photonvision/common/ProgramStatus.java +++ b/photon-core/src/main/java/org/photonvision/common/ProgramStatus.java @@ -20,6 +20,5 @@ public enum ProgramStatus { UHOH, RUNNING, - RUNNING_NT, - RUNNING_NT_TARGET + RUNNING_NT } 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/configuration/SqlConfigProvider.java b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java index 883b3607d8..95f01e4079 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/SqlConfigProvider.java @@ -343,6 +343,29 @@ private void addFile(PreparedStatement ps, String key, String value) throws SQLE ps.setString(2, value); } + // NOTE to Future Developers: + // These booleans form a mechanism to prevent saveGlobal() and + // saveOneFile() from stepping on each other's toes. Both write + // to the database on disk, and both write to the same keys, but + // they use different sources. Generally, if the user has done something + // to trigger saveOneFile() to get called, it implies they want that + // configuration, and not whatever is in RAM right now (which is what + // saveGlobal() uses to write). Therefor, once saveOneFile() is invoked, + // we record which entry was overwritten in the database and prevent + // overwriting it when saveGlobal() is invoked (likely by the shutdown + // that should almost almost almost happen right after saveOneFile() is + // invoked). + // + // In the future, this may not be needed. A better architecture would involve + // manipulating the RAM representation of configuration when new .json files + // are uploaded in the UI, and eliminate all other usages of saveOneFile(). + // But, seeing as it's Dec 28 and kickoff is nigh, we put this here and moved on. + // Thank you for coming to my TED talk. + private boolean skipSavingHWCfg = false; + private boolean skipSavingHWSet = false; + private boolean skipSavingNWCfg = false; + private boolean skipSavingAPRTG = false; + private void saveGlobal(Connection conn) { PreparedStatement statement1 = null; PreparedStatement statement2 = null; @@ -351,28 +374,34 @@ private void saveGlobal(Connection conn) { // Replace this camera's row with the new settings var sqlString = "REPLACE INTO global (filename, contents) VALUES " + "(?,?);"; - statement1 = conn.prepareStatement(sqlString); - addFile( - statement1, - TableKeys.HARDWARE_SETTINGS, - JacksonUtils.serializeToString(config.getHardwareSettings())); - statement1.executeUpdate(); + if (!skipSavingHWSet) { + statement1 = conn.prepareStatement(sqlString); + addFile( + statement1, + TableKeys.HARDWARE_SETTINGS, + JacksonUtils.serializeToString(config.getHardwareSettings())); + statement1.executeUpdate(); + } - statement2 = conn.prepareStatement(sqlString); - addFile( - statement2, - TableKeys.NETWORK_CONFIG, - JacksonUtils.serializeToString(config.getNetworkConfig())); - statement2.executeUpdate(); - statement2.close(); - - statement3 = conn.prepareStatement(sqlString); - addFile( - statement3, - TableKeys.HARDWARE_CONFIG, - JacksonUtils.serializeToString(config.getHardwareConfig())); - statement3.executeUpdate(); - statement3.close(); + if (!skipSavingNWCfg) { + statement2 = conn.prepareStatement(sqlString); + addFile( + statement2, + TableKeys.NETWORK_CONFIG, + JacksonUtils.serializeToString(config.getNetworkConfig())); + statement2.executeUpdate(); + statement2.close(); + } + + if (!skipSavingHWCfg) { + statement3 = conn.prepareStatement(sqlString); + addFile( + statement3, + TableKeys.HARDWARE_CONFIG, + JacksonUtils.serializeToString(config.getHardwareConfig())); + statement3.executeUpdate(); + statement3.close(); + } } catch (SQLException | IOException e) { logger.error("Err saving global", e); @@ -431,21 +460,25 @@ private boolean saveOneFile(String fname, Path path) { @Override public boolean saveUploadedHardwareConfig(Path uploadPath) { + skipSavingHWCfg = true; return saveOneFile(TableKeys.HARDWARE_CONFIG, uploadPath); } @Override public boolean saveUploadedHardwareSettings(Path uploadPath) { + skipSavingHWSet = true; return saveOneFile(TableKeys.HARDWARE_SETTINGS, uploadPath); } @Override public boolean saveUploadedNetworkConfig(Path uploadPath) { + skipSavingNWCfg = true; return saveOneFile(TableKeys.NETWORK_CONFIG, uploadPath); } @Override public boolean saveUploadedAprilTagFieldLayout(Path uploadPath) { + skipSavingAPRTG = true; return saveOneFile(TableKeys.ATFL_CONFIG_FILE, uploadPath); } 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 e759c07ecc..87e736d7a2 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/common/dataflow/networktables/NetworkTablesManager.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java index a62e0f3b4c..4cd2d1e0bd 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NetworkTablesManager.java @@ -32,6 +32,7 @@ import org.photonvision.common.configuration.NetworkConfig; import org.photonvision.common.dataflow.DataChangeService; import org.photonvision.common.dataflow.events.OutgoingUIEvent; +import org.photonvision.common.hardware.HardwareManager; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.scripting.ScriptEventType; @@ -91,6 +92,7 @@ public void accept(NetworkTableEvent event) { event.connInfo.remote_port, event.connInfo.protocol_version); logger.error(msg); + HardwareManager.getInstance().setNTConnected(false); hasReportedConnectionFailure = true; getInstance().broadcastConnectedStatus(); @@ -102,6 +104,7 @@ public void accept(NetworkTableEvent event) { event.connInfo.remote_port, event.connInfo.protocol_version); logger.info(msg); + HardwareManager.getInstance().setNTConnected(true); hasReportedConnectionFailure = false; ScriptManager.queueEvent(ScriptEventType.kNTConnected); diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/statusLEDs/StatusLEDConsumer.java b/photon-core/src/main/java/org/photonvision/common/dataflow/statusLEDs/StatusLEDConsumer.java new file mode 100644 index 0000000000..175b4c4d91 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/statusLEDs/StatusLEDConsumer.java @@ -0,0 +1,35 @@ +/* + * 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.common.dataflow.statusLEDs; + +import org.photonvision.common.dataflow.CVPipelineResultConsumer; +import org.photonvision.common.hardware.HardwareManager; +import org.photonvision.vision.pipeline.result.CVPipelineResult; + +public class StatusLEDConsumer implements CVPipelineResultConsumer { + private final int index; + + public StatusLEDConsumer(int index) { + this.index = index; + } + + @Override + public void accept(CVPipelineResult t) { + HardwareManager.getInstance().setTargetsVisibleStatus(this.index, t.hasTargets()); + } +} diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java index 2a049cccae..dcb0ac69dc 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java @@ -20,7 +20,8 @@ import edu.wpi.first.networktables.IntegerPublisher; import edu.wpi.first.networktables.IntegerSubscriber; import java.io.IOException; -import org.photonvision.common.ProgramStatus; +import java.util.HashMap; +import java.util.Map; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.configuration.HardwareConfig; import org.photonvision.common.configuration.HardwareSettings; @@ -32,6 +33,7 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.ShellExec; +import org.photonvision.common.util.TimedTaskManager; public class HardwareManager { private static HardwareManager instance; @@ -96,6 +98,10 @@ private HardwareManager(HardwareConfig hardwareConfig, HardwareSettings hardware ? new StatusLED(hardwareConfig.statusRGBPins) : null; + if (statusLED != null) { + TimedTaskManager.getInstance().addTask("StatusLEDUpdate", this::statusLEDUpdate, 150); + } + var hasBrightnessRange = hardwareConfig.ledBrightnessRange.size() == 2; visionLED = hardwareConfig.ledPins.isEmpty() @@ -160,21 +166,61 @@ public boolean restartDevice() { } } - public void setStatus(ProgramStatus status) { - switch (status) { - case UHOH: - // red flashing, green off - break; - case RUNNING: - // red solid, green off - break; - case RUNNING_NT: - // red off, green solid - break; - case RUNNING_NT_TARGET: - // red off, green flashing - break; + // API's supporting status LEDs + + private Map pipelineTargets = new HashMap(); + private boolean ntConnected = false; + private boolean systemRunning = false; + private int blinkCounter = 0; + + public void setTargetsVisibleStatus(int pipelineIdx, boolean hasTargets) { + pipelineTargets.put(pipelineIdx, hasTargets); + } + + public void setNTConnected(boolean isConnected) { + this.ntConnected = isConnected; + } + + public void setRunning(boolean isRunning) { + this.systemRunning = isRunning; + } + + private void statusLEDUpdate() { + // make blinky + boolean blinky = ((blinkCounter % 3) > 0); + + // check if any pipeline has a visible target + boolean anyTarget = false; + for (var t : this.pipelineTargets.values()) { + if (t) { + anyTarget = true; + } + } + + if (this.systemRunning) { + if (!this.ntConnected) { + if (anyTarget) { + // Blue Flashing + statusLED.setRGB(false, false, blinky); + } else { + // Yellow flashing + statusLED.setRGB(blinky, blinky, false); + } + } else { + if (anyTarget) { + // Blue + statusLED.setRGB(false, false, blinky); + } else { + // blinky green + statusLED.setRGB(false, blinky, false); + } + } + } else { + // Faulted, not running... blinky red + statusLED.setRGB(blinky, false, false); } + + blinkCounter++; } public HardwareConfig getConfig() { diff --git a/photon-core/src/main/java/org/photonvision/common/hardware/StatusLED.java b/photon-core/src/main/java/org/photonvision/common/hardware/StatusLED.java index 0c10b3548e..efef23454e 100644 --- a/photon-core/src/main/java/org/photonvision/common/hardware/StatusLED.java +++ b/photon-core/src/main/java/org/photonvision/common/hardware/StatusLED.java @@ -45,4 +45,14 @@ public StatusLED(List statusLedPins) { blueLED = new CustomGPIO(statusLedPins.get(2)); } } + + public void setRGB(boolean r, boolean g, boolean b) { + // Outputs are active-low, so invert the level applied + redLED.setState(!r); + redLED.setBrightness(r ? 0 : 100); + greenLED.setState(!g); + greenLED.setBrightness(g ? 0 : 100); + blueLED.setState(!b); + blueLED.setBrightness(b ? 0 : 100); + } } diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java index a2f2911431..66b0a4fff7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java @@ -22,6 +22,7 @@ import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.cscore.VideoException; import edu.wpi.first.cscore.VideoMode; +import edu.wpi.first.cscore.VideoProperty.Kind; import java.util.*; import java.util.stream.Collectors; import org.photonvision.common.configuration.CameraConfiguration; @@ -29,6 +30,7 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.TestUtils; +import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.frame.FrameProvider; import org.photonvision.vision.frame.provider.FileFrameProvider; import org.photonvision.vision.frame.provider.USBFrameProvider; @@ -210,6 +212,12 @@ public void setExposure(double exposure) { camera.getProperty("raw_exposure_time_absolute").set(scaledExposure); camera.getProperty("raw_exposure_time_absolute").set(scaledExposure); + } else if (camera.getProperty("exposure_time_absolute").getKind() != Kind.kNone) { + // Seems like the name changed at some point in v4l? set it instead + var prop = camera.getProperty("exposure_time_absolute"); + var exposure_manual_val = + MathUtils.map(Math.round(exposure), 0, 100, prop.getMin(), prop.getMax()); + prop.set((int) exposure_manual_val); } else { scaledExposure = (int) Math.round(exposure); logger.debug("Setting camera exposure to " + scaledExposure); diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index 8a2d57ecc1..2929a3ee87 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -34,6 +34,7 @@ import org.photonvision.common.dataflow.DataChangeService; import org.photonvision.common.dataflow.events.OutgoingUIEvent; import org.photonvision.common.dataflow.networktables.NTDataPublisher; +import org.photonvision.common.dataflow.statusLEDs.StatusLEDConsumer; import org.photonvision.common.dataflow.websocket.UIDataPublisher; import org.photonvision.common.hardware.HardwareManager; import org.photonvision.common.logging.LogGroup; @@ -73,6 +74,7 @@ public class VisionModule { new LinkedList<>(); private final NTDataPublisher ntConsumer; private final UIDataPublisher uiDataConsumer; + private final StatusLEDConsumer statusLEDsConsumer; protected final int moduleIndex; protected final QuirkyCamera cameraQuirks; @@ -144,8 +146,10 @@ public VisionModule(PipelineManager pipelineManager, VisionSource visionSource, pipelineManager::getDriverMode, this::setDriverMode); uiDataConsumer = new UIDataPublisher(index); + statusLEDsConsumer = new StatusLEDConsumer(index); addResultConsumer(ntConsumer); addResultConsumer(uiDataConsumer); + addResultConsumer(statusLEDsConsumer); addResultConsumer( (result) -> lastPipelineResultBestTarget = result.hasTargets() ? result.targets.get(0) : null); 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-server/src/main/java/org/photonvision/Main.java b/photon-server/src/main/java/org/photonvision/Main.java index f01e431d30..daf8c243ca 100644 --- a/photon-server/src/main/java/org/photonvision/Main.java +++ b/photon-server/src/main/java/org/photonvision/Main.java @@ -375,6 +375,7 @@ public static void main(String[] args) { } logger.info("Starting server..."); + HardwareManager.getInstance().setRunning(true); Server.start(DEFAULT_WEBPORT); } } diff --git a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java index d93fd58f34..f0ec331ca7 100644 --- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java +++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java @@ -89,8 +89,8 @@ public static void onSettingsImportRequest(Context ctx) { if (ConfigManager.saveUploadedSettingsZip(tempFilePath.get())) { ctx.status(200); - ctx.result("Successfully saved the uploaded settings zip, rebooting"); - logger.info("Successfully saved the uploaded settings zip, rebooting"); + ctx.result("Successfully saved the uploaded settings zip, rebooting..."); + logger.info("Successfully saved the uploaded settings zip, rebooting..."); restartProgram(); } else { ctx.status(500); @@ -153,8 +153,9 @@ public static void onHardwareConfigRequest(Context ctx) { if (ConfigManager.getInstance().saveUploadedHardwareConfig(tempFilePath.get().toPath())) { ctx.status(200); - ctx.result("Successfully saved the uploaded hardware config"); - logger.info("Successfully saved the uploaded hardware config"); + ctx.result("Successfully saved the uploaded hardware config, rebooting..."); + logger.info("Successfully saved the uploaded hardware config, rebooting..."); + restartProgram(); } else { ctx.status(500); ctx.result("There was an error while saving the uploaded hardware config"); @@ -195,8 +196,9 @@ public static void onHardwareSettingsRequest(Context ctx) { if (ConfigManager.getInstance().saveUploadedHardwareSettings(tempFilePath.get().toPath())) { ctx.status(200); - ctx.result("Successfully saved the uploaded hardware settings"); - logger.info("Successfully saved the uploaded hardware settings"); + ctx.result("Successfully saved the uploaded hardware settings, rebooting..."); + logger.info("Successfully saved the uploaded hardware settings, rebooting..."); + restartProgram(); } else { ctx.status(500); ctx.result("There was an error while saving the uploaded hardware settings"); @@ -237,8 +239,9 @@ public static void onNetworkConfigRequest(Context ctx) { if (ConfigManager.getInstance().saveUploadedNetworkConfig(tempFilePath.get().toPath())) { ctx.status(200); - ctx.result("Successfully saved the uploaded network config"); - logger.info("Successfully saved the uploaded network config"); + ctx.result("Successfully saved the uploaded network config, rebooting..."); + logger.info("Successfully saved the uploaded network config, rebooting..."); + restartProgram(); } else { ctx.status(500); ctx.result("There was an error while saving the uploaded network config"); @@ -279,8 +282,9 @@ public static void onAprilTagFieldLayoutRequest(Context ctx) { if (ConfigManager.getInstance().saveUploadedAprilTagFieldLayout(tempFilePath.get().toPath())) { ctx.status(200); - ctx.result("Successfully saved the uploaded AprilTagFieldLayout"); - logger.info("Successfully saved the uploaded AprilTagFieldLayout"); + ctx.result("Successfully saved the uploaded AprilTagFieldLayout, rebooting..."); + logger.info("Successfully saved the uploaded AprilTagFieldLayout, rebooting..."); + restartProgram(); } else { ctx.status(500); ctx.result("There was an error while saving the uploaded AprilTagFieldLayout"); 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' } }